ObjectDetectionChecker.hpp

Go to the documentation of this file.

 
#ifndef OBJECT_DETECTION_CHECKER_HPP
#define OBJECT_DETECTION_CHECKER_HPP
 
#include "../../AutonomyGlobals.h"
#include "../../vision/objects/ObjectDetector.h"
 
 
 
 
namespace statemachine
{
 
    inline void LoadDetectedObjects(std::vector<objectdetectutils::Object>& vDetectedObjects, const std::vector<std::shared_ptr<ObjectDetector>>& vObjectDetectors)
    {
        // Number of object detectors.
        size_t siNumObjectDetectors = vObjectDetectors.size();
 
        // Initialize vectors to store detected objects temporarily.
        std::vector<std::vector<objectdetectutils::Object>> vDetectedObjectBuffers(siNumObjectDetectors);
 
        // Initialize vectors to store detected objects futures.
        std::vector<std::future<bool>> vDetectedObjectsFuture;
 
        // Request objects from each detector.
        for (size_t siIdx = 0; siIdx < siNumObjectDetectors; ++siIdx)
        {
            // Check if this object detector is ready.
            if (vObjectDetectors[siIdx]->GetIsReady())
            {
                // Request detected objects from detector.
                vDetectedObjectsFuture.emplace_back(vObjectDetectors[siIdx]->RequestDetectedObjects(vDetectedObjectBuffers[siIdx]));
            }
        }
 
        // Ensure all requests have been fulfilled.
        // Then transfer objects from the buffer to vDetectedObjects for the user to access.
        for (size_t siIdx = 0; siIdx < vDetectedObjectsFuture.size(); ++siIdx)
        {
            // Wait for the request to be fulfilled.
            vDetectedObjectsFuture[siIdx].get();
 
            // Loop through the detected objects and add them to the vDetectedObjects vector.
            for (const objectdetectutils::Object& tObject : vDetectedObjectBuffers[siIdx])
            {
                vDetectedObjects.emplace_back(tObject);
            }
        }
    }
 
 
    inline int IdentifyTargetObject(const std::vector<std::shared_ptr<ObjectDetector>>& vObjectDetectors,
                                    objectdetectutils::Object& stObjectTarget,
                                    const geoops::WaypointType& eDesiredDetectionType = geoops::WaypointType::eUNKNOWN)
    {
        // Create instance variables.
        std::vector<objectdetectutils::Object> vDetectedObjects;
        objectdetectutils::Object stBestObject;
        std::string szIdentifiedObjects = "";
 
        // Get the current time
        std::chrono::system_clock::time_point tmCurrentTime = std::chrono::system_clock::now();
 
        // Load all detected objects in the rover's vision.
        LoadDetectedObjects(vDetectedObjects, vObjectDetectors);
        // Find the best object.
        for (const objectdetectutils::Object& stCandidate : vDetectedObjects)
        {
            // Calculate the total age of the object.
            double dObjectTotalAge = std::fabs(std::chrono::duration_cast<std::chrono::milliseconds>(tmCurrentTime - stCandidate.tmCreation).count() / 1000.0);
            // Calculate the total object area.
            double dArea = stCandidate.pBoundingBox->area();
            // Calculate what percentage of the screen the object takes up.
            double dAreaPercentage = (dArea / (stCandidate.cvImageResolution.width * stCandidate.cvImageResolution.height)) * 100.0;
 
            // If the distance of the object is not greater than 0, skip it.
            if (stCandidate.dStraightLineDistance <= 0.0)
            {
                continue;
            }
 
            // Determine the desired detection type.
            switch (eDesiredDetectionType)
            {
                case geoops::WaypointType::eMalletWaypoint:
                {
                    if (stCandidate.eDetectionType != objectdetectutils::ObjectDetectionType::eMallet)
                    {
                        continue;
                    }
                    break;
                }
                case geoops::WaypointType::eWaterBottleWaypoint:
                {
                    if (stCandidate.eDetectionType != objectdetectutils::ObjectDetectionType::eWaterBottle)
                    {
                        continue;
                    }
                    break;
                }
                case geoops::WaypointType::eObjectWaypoint:
                {
                    if (stCandidate.eDetectionType != objectdetectutils::ObjectDetectionType::eMallet &&
                        stCandidate.eDetectionType != objectdetectutils::ObjectDetectionType::eWaterBottle)
                    {
                        continue;
                    }
                    break;
                }
                default:
                {
                    break;
                }
            }
 
            //  Check the object detection method type.
            if (stCandidate.eDetectionMethod == objectdetectutils::ObjectDetectionMethod::eTorch)
            {
                // Assemble the identified objects string.
                szIdentifiedObjects += "\tObject Class: " + stCandidate.szClassName + " Object Age: " + std::to_string(dObjectTotalAge) +
                                       "s Object Screen Percentage: " + std::to_string(dAreaPercentage) + "%\n";
                // Check if the object meets the requirements.
                if (dAreaPercentage < constants::BBOX_MIN_SCREEN_PERCENTAGE || dObjectTotalAge < constants::BBOX_MIN_LIFETIME_THRESHOLD)
                {
                    continue;
                }
 
                // Check other object requirements.
                if (dArea > stBestObject.pBoundingBox->area())
                {
                    // Set the target object to the detected object.
                    stBestObject = stCandidate;
                }
            }
        }
 
        // Only print the identified objects if there are any.
        if (stBestObject.dConfidence != 0.0)
        {
            // Submit logger message.
            LOG_DEBUG(logging::g_qSharedLogger, "ObjectDetectionChecker: Identified objects:\n{}", szIdentifiedObjects);
        }
 
        // Set the target object to the best object.
        stObjectTarget = stBestObject;
 
        return static_cast<int>(vDetectedObjects.size());
    }
}    // namespace statemachine
#endif