TagDetectionUtilty.hpp

 
#ifndef TAG_DETECTION_UTILITY_HPP
#define TAG_DETECTION_UTILITY_HPP
 
 
#include <type_traits>
 
#include "../../vision/aruco/ArucoDetection.hpp"
#include "../../vision/aruco/TagDetector.h"
#include "../../vision/aruco/TensorflowTagDetection.hpp"
 
 
 
 
namespace tagdetectutils
{
 
    inline void LoadDetectedTags(std::vector<arucotag::ArucoTag>& vDetectedArucoTags,
                                 std::vector<tensorflowtag::TensorflowTag>& vDetectedTensorflowTags,
                                 const std::vector<TagDetector*>& vTagDetectors,
                                 bool bUnique = false)
    {
        // Number of tag detectors.
        size_t siNumTagDetectors = vTagDetectors.size();
 
        // Initialize vectors to store detected tags temporarily.
        std::vector<std::vector<arucotag::ArucoTag>> vDetectedArucoTagBuffers(siNumTagDetectors);
        std::vector<std::vector<tensorflowtag::TensorflowTag>> vDetectedTensorflowTagBuffers(siNumTagDetectors);
 
        // Initialize vectors to store detected tags futures.
        std::vector<std::future<bool>> vDetectedArucoTagsFuture;
        std::vector<std::future<bool>> vDetectedTensorflowTagsFuture;
 
        // Request tags from each detector.
        for (size_t siIdx = 0; siIdx < siNumTagDetectors; ++siIdx)
        {
            // Check if this tag detector is ready.
            if (vTagDetectors[siIdx]->GetIsReady())
            {
                // Request detected Aruco tags from detector.
                vDetectedArucoTagsFuture.emplace_back(vTagDetectors[siIdx]->RequestDetectedArucoTags(vDetectedArucoTagBuffers[siIdx]));
                // Request detected Tensorflow tags from detector.
                vDetectedTensorflowTagsFuture.emplace_back(vTagDetectors[siIdx]->RequestDetectedTensorflowTags(vDetectedTensorflowTagBuffers[siIdx]));
            }
        }
 
        // Ensure all requests have been fulfilled.
        // Then transfer tags from the buffer to vDetectedArucoTags and vDetectedTensorflowTags for the user to access.
        for (size_t siIdx = 0; siIdx < vDetectedArucoTagsFuture.size(); ++siIdx)
        {
            // Wait for the request to be fulfilled.
            vDetectedArucoTagsFuture[siIdx].get();
            vDetectedTensorflowTagsFuture[siIdx].get();
 
            // Loop through the detected Aruco tags and add them to the vDetectedArucoTags vector.
            for (const arucotag::ArucoTag& tTag : vDetectedArucoTagBuffers[siIdx])
            {
                vDetectedArucoTags.emplace_back(tTag);
            }
 
            // Loop through the detected Tensorflow tags and add them to the vDetectedTensorflowTags vector.
            for (const tensorflowtag::TensorflowTag& tTag : vDetectedTensorflowTagBuffers[siIdx])
            {
                vDetectedTensorflowTags.emplace_back(tTag);
            }
        }
 
        if (bUnique)
        {
            // Remove all Aruco tags with a duplicate ID.
            std::set<int> setIds;
            size_t szIdx = 0;
            while (szIdx < vDetectedArucoTags.size())
            {
                // Tag was detected by another tag detector.
                if (setIds.count(vDetectedArucoTags[szIdx].nID))
                {
                    vDetectedArucoTags.erase(vDetectedArucoTags.begin() + szIdx);
                }
                else
                {
                    setIds.insert(vDetectedArucoTags[szIdx].nID);
                    ++szIdx;
                }
            }
        }
    }
 
 
    inline bool FindArucoTagByID(int nID, arucotag::ArucoTag& stIdentifiedArucoTag, const std::vector<TagDetector*>& vTagDetectors)
    {
        // Load all detected tags in the rover's vision.
        std::vector<arucotag::ArucoTag> vDetectedArucoTags;
        std::vector<tensorflowtag::TensorflowTag> vDetectedTensorflowTags;
        LoadDetectedTags(vDetectedArucoTags, vDetectedTensorflowTags, vTagDetectors, true);
 
        // Find the tag with the corresponding id.
        for (const arucotag::ArucoTag& tTag : vDetectedArucoTags)
        {
            // Is this the tag being searched for.
            if (tTag.nID == nID)
            {
                stIdentifiedArucoTag = tTag;
                return true;
            }
        }
 
        // The tag was not found by the tag detectors.
        return false;
    }
 
    // LEAD: Commented this out since TensorflowTag has no ID. Can this be removed or reimplemented in another way?
    // /******************************************************************************
    //  * @brief Find a tag in the rover's vision with the specified ID, using Tensorflow detection.
    //  *
    //  * @param nID - The ID of the tag being looked for.
    //  * @param tIdentifiedTag - Reference to save the identified tag.
    //  * @param vTagDetectors - Vector of pointers to tag detectors that will be used to request their detected tags.
    //  * @return true - The tag was found.
    //  * @return false - The tag was not found.
    //  *
    //  * @author JSpencerPittman (jspencerpittman@gmail.com)
    //  * @date 2024-03-08
    //  ******************************************************************************/
    // inline bool FindTensorflowTagByID(int nID, tensorflowtag::TensorflowTag& stIdentifiedTag, const std::vector<TagDetector*>& vTagDetectors)
    // {
    //     // Load all detected tags in the rover's vision.
    //     std::vector<tensorflowtag::TensorflowTag> vDetectedTags;
    //     LoadDetectedTensorflowTags(vDetectedTags, vTagDetectors, true);
 
    //     // Find the tag with the corresponding id.
    //     for (const tensorflowtag::TensorflowTag& tTag : vDetectedTags)
    //     {
    //         // Is this the tag being searched for.
    //         if (tTag.nID == nID)
    //         {
    //             stIdentifiedTag = tTag;
    //             return true;
    //         }
    //     }
 
    //     // The tag was not found by the tag detectors.
    //     return false;
    // }
}    // namespace tagdetectutils
 
#endif