ObjectDetectionUtility.hpp

 
#ifndef OBJECT_DETECTION_UTILITY_HPP
#define OBJECT_DETECTION_UTILITY_HPP
 
#include "../../AutonomyConstants.h"
#include "../../AutonomyLogging.h"
#include "../GeospatialOperations.hpp"
 
#include <opencv2/opencv.hpp>
 
 
 
 
namespace objectdetectutils
{
 
    enum class ObjectDetectionMethod
    {
        eUnknown,    // Unknown detection method.
        eTorch,      // Torch detection using a YOLO model.
    };
 
 
    enum class ObjectDetectionType
    {
        eUnknown,        // Unknown detection type.
        eMallet,         // Detection is a mallet.
        eWaterBottle,    // Detection is a water bottle.
        eRockPick        // Detection is a rock pick.
    };
 
 
    struct Object
    {
        public:
            // Declare public struct member attributes.
            std::shared_ptr<cv::Rect2d> pBoundingBox         = std::make_shared<cv::Rect2d>();      // The bounding box of the detected object.
            double dConfidence                               = 0.0;                                 // The detection confidence of the object (from Torch models).
            double dStraightLineDistance                     = 0.0;                                 // Distance between the object and the camera.
            double dYawAngle                                 = 0.0;                                 // This is the yaw angle so roll and pitch are ignored.
            std::string szClassName                          = "";                                  // The class name of the object (used in Torch models).
            std::chrono::system_clock::time_point tmCreation = std::chrono::system_clock::now();    // Set the time detected to the minimum time point.
            ObjectDetectionMethod eDetectionMethod           = ObjectDetectionMethod::eUnknown;     // The detection method used to detect the object.
            ObjectDetectionType eDetectionType               = ObjectDetectionType::eUnknown;       // The detection type used to detect the object.
            cv::Size cvImageResolution                       = cv::Size(0, 0);                      // The resolution of the image used to detect the object.
            double dHorizontalFOV                            = 0.0;                   // The horizontal field of view of the camera used to detect the object.
            geoops::Waypoint stGeolocatedPosition            = geoops::Waypoint();    // The geolocated position of the object.
            std::string szDetectorUUID = "";    // The UUID of the detector that detected the object. This is used to associate objects with their detectors.
 
 
            bool operator==(const Object& stOther) const
            {
                return *pBoundingBox == *stOther.pBoundingBox && dConfidence == stOther.dConfidence && dStraightLineDistance == stOther.dStraightLineDistance &&
                       dYawAngle == stOther.dYawAngle && szClassName == stOther.szClassName && tmCreation == stOther.tmCreation &&
                       eDetectionMethod == stOther.eDetectionMethod && eDetectionType == stOther.eDetectionType && cvImageResolution == stOther.cvImageResolution &&
                       dHorizontalFOV == stOther.dHorizontalFOV && stGeolocatedPosition == stOther.stGeolocatedPosition && szDetectorUUID == stOther.szDetectorUUID;
            }
 
 
            bool operator!=(const Object& stOther) const { return !(*this == stOther); }
 
 
            Object& operator=(const Object& stOther)
            {
                // Check if the other Object is not the same as this one.
                if (this != &stOther)
                {
                    // Shallow copy the bounding box.
                    pBoundingBox = stOther.pBoundingBox;
 
                    // Copy other member variables.
                    dConfidence           = stOther.dConfidence;
                    dStraightLineDistance = stOther.dStraightLineDistance;
                    dYawAngle             = stOther.dYawAngle;
                    szClassName           = stOther.szClassName;
                    tmCreation            = stOther.tmCreation;
                    eDetectionMethod      = stOther.eDetectionMethod;
                    eDetectionType        = stOther.eDetectionType;
                    cvImageResolution     = stOther.cvImageResolution;
                    dHorizontalFOV        = stOther.dHorizontalFOV;
                    stGeolocatedPosition  = stOther.stGeolocatedPosition;
                    szDetectorUUID        = stOther.szDetectorUUID;
                }
                return *this;
            }
    };
 
 
    inline cv::Point2f FindObjectCenter(const Object& stObject)
    {
        // Find the center of the object.
        return cv::Point2f(stObject.pBoundingBox->x + stObject.pBoundingBox->width / 2, stObject.pBoundingBox->y + stObject.pBoundingBox->height / 2);
    }
 
 
    inline void EstimatePoseFromCameraFrame(Object& stTag)
    {
        // Use camera field of view and camera frame size to determine tag angle in degrees from center of camera.
        double dDegreesPerPixel = stTag.dHorizontalFOV / stTag.cvImageResolution.width;
        // Find tag error in pixels from center of image.
        double dTagErrorX = (stTag.pBoundingBox->x + stTag.pBoundingBox->width / 2) - (stTag.cvImageResolution.width / 2);
        // Find angle error.
        double dTagAngleX = dTagErrorX * dDegreesPerPixel;
        // Reassign yaw and distance to tag.
        stTag.dYawAngle = dTagAngleX;
 
        // For the distance, we'll just use the screen percentage of the tag.
        stTag.dStraightLineDistance = (stTag.pBoundingBox->area() / (stTag.cvImageResolution.width * stTag.cvImageResolution.height)) * 100.0;
    }
}    // namespace objectdetectutils
 
#endif