TagDetectionUtilty.hpp

 
#ifndef TAG_DETECTION_UTILITY_HPP
#define TAG_DETECTION_UTILITY_HPP
 
#include "../../AutonomyConstants.h"
#include "../../AutonomyLogging.h"
#include "../GeospatialOperations.hpp"
 
#include <opencv2/opencv.hpp>
 
 
 
 
namespace tagdetectutils
{
 
    enum class TagDetectionMethod
    {
        eUnknown,    // Unknown detection method.
        eOpenCV,     // Standard OpenCV detection using the ArUco library.
        eTorch       // Torch detection using a YOLO model.
    };
 
 
    struct ArucoTag
    {
        public:
            // Declare public struct member attributes.
            std::shared_ptr<cv::Rect2d> pBoundingBox         = std::make_shared<cv::Rect2d>();      // The bounding box of the detected tag.
            double dConfidence                               = 0.0;                                 // The detection confidence of the tag (from Torch models).
            double dStraightLineDistance                     = 0.0;                                 // Distance between the tag and the camera.
            double dYawAngle                                 = 0.0;                                 // This is the yaw angle so roll and pitch are ignored.
            int nID                                          = -1;                                  // The ID of the tag. This is set to -1 if the tag is not detected.
            std::string szClassName                          = "";                                  // The class name of the tag (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.
            TagDetectionMethod eDetectionMethod              = TagDetectionMethod::eUnknown;        // The detection method used to detect the tag.
            cv::Size cvImageResolution                       = cv::Size(0, 0);                      // The resolution of the image used to detect the tag.
            double dHorizontalFOV                            = 0.0;                                 // The horizontal field of view of the camera used to detect the tag.
            geoops::Waypoint stGeolocatedPosition            = geoops::Waypoint();                  // The geolocated position of the tag.
            std::string szDetectorUUID = "";    // The UUID of the detector that detected the tag. This is used to associate tags with their detectors.
 
 
            bool operator==(const ArucoTag& stOther) const
            {
                return *pBoundingBox == *stOther.pBoundingBox && dConfidence == stOther.dConfidence && dStraightLineDistance == stOther.dStraightLineDistance &&
                       dYawAngle == stOther.dYawAngle && nID == stOther.nID && szClassName == stOther.szClassName && tmCreation == stOther.tmCreation &&
                       eDetectionMethod == stOther.eDetectionMethod && cvImageResolution == stOther.cvImageResolution && dHorizontalFOV == stOther.dHorizontalFOV &&
                       stGeolocatedPosition == stOther.stGeolocatedPosition && szDetectorUUID == stOther.szDetectorUUID;
            }
 
 
            bool operator!=(const ArucoTag& stOther) const { return !(*this == stOther); }
 
 
            ArucoTag& operator=(const ArucoTag& stOther)
            {
                // Check if the other ArucoTag 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;
                    nID                   = stOther.nID;
                    szClassName           = stOther.szClassName;
                    tmCreation            = stOther.tmCreation;
                    eDetectionMethod      = stOther.eDetectionMethod;
                    cvImageResolution     = stOther.cvImageResolution;
                    dHorizontalFOV        = stOther.dHorizontalFOV;
                    stGeolocatedPosition  = stOther.stGeolocatedPosition;
                    szDetectorUUID        = stOther.szDetectorUUID;
                }
                return *this;
            }
    };
 
 
    inline cv::Point2f FindTagCenter(const ArucoTag& stTag)
    {
        // Calculate the center point of the tag.
        cv::Point2f cvCenter = cv::Point2f(stTag.pBoundingBox->x + stTag.pBoundingBox->width / 2, stTag.pBoundingBox->y + stTag.pBoundingBox->height / 2);
 
        return cvCenter;
    }
 
 
    inline void EstimatePoseFromPNP(cv::Mat& cvCameraMatrix, cv::Mat& cvDistCoeffs, ArucoTag& stTag)
    {
        // rotVec is how the tag is orientated with respect to the camera. It's 3 numbers defining an axis of rotation around which we rotate the angle which is the
        // euclidean distance of the vector. transVec is the XYZ translation of the tag from the camera if you image the convergence of light as a pinhole sitting at
        // (0,0,0) in space.
        cv::Vec3d cvRotVec, cvTransVec;
 
        // Set expected object coordinate system shape.
        cv::Mat cvObjPoints(4, 1, CV_32FC3);
        cvObjPoints.at<cv::Vec3f>(0) = cv::Vec3f{0, 0, 0};                                                                  // Top-left corner.
        cvObjPoints.at<cv::Vec3f>(1) = cv::Vec3f{constants::ARUCO_TAG_SIDE_LENGTH, 0, 0};                                   // Bottom-left corner.
        cvObjPoints.at<cv::Vec3f>(2) = cv::Vec3f{0, constants::ARUCO_TAG_SIDE_LENGTH, 0};                                   // Top-right corner.
        cvObjPoints.at<cv::Vec3f>(3) = cv::Vec3f{constants::ARUCO_TAG_SIDE_LENGTH, constants::ARUCO_TAG_SIDE_LENGTH, 0};    // Bottom-right corner.
 
        // Repackage tag image points into a mat.
        cv::Mat cvImgPoints(4, 1, CV_32FC3);
        cvImgPoints.at<cv::Vec3f>(0) = cv::Vec3f{static_cast<float>(stTag.pBoundingBox->x), static_cast<float>(stTag.pBoundingBox->y), 0.0f};      // Top-left corner.
        cvImgPoints.at<cv::Vec3f>(1) =
            cv::Vec3f{static_cast<float>(stTag.pBoundingBox->x), static_cast<float>(stTag.pBoundingBox->y + stTag.pBoundingBox->height), 0.0f};    // Bottom-left corner.
        cvImgPoints.at<cv::Vec3f>(2) =
            cv::Vec3f{static_cast<float>(stTag.pBoundingBox->x + stTag.pBoundingBox->width), static_cast<float>(stTag.pBoundingBox->y), 0.0f};     // Top-right corner.
        cvImgPoints.at<cv::Vec3f>(3) = cv::Vec3f{static_cast<float>(stTag.pBoundingBox->x + stTag.pBoundingBox->width),
                                                 static_cast<float>(stTag.pBoundingBox->y + stTag.pBoundingBox->height),
                                                 0.0f};    // Bottom-right corner.
 
        // Use solve perspective n' point algorithm to estimate pose of the tag.
        cv::solvePnP(cvObjPoints, cvImgPoints, cvCameraMatrix, cvDistCoeffs, cvRotVec, cvTransVec);
 
        // Grab (x,y,z) coordinates from where the tag was detected
        double dForward = cvTransVec[2];
        double dRight   = cvTransVec[0];
        double dUp      = cvTransVec[1];
 
        // Calculate euclidean distance from ZED camera left eye to the point of interest
        stTag.dStraightLineDistance = std::sqrt(std::pow(dForward, 2) + std::pow(dRight, 2) + std::pow(dUp, 2));
 
        // Calculate the angle on plane horizontal to the viewpoint
        stTag.dYawAngle = std::atan2(dRight, dForward);
    }
 
 
    inline void EstimatePoseFromCameraFrame(ArucoTag& 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 tagdetectutils
 
#endif