TensorflowTagDetection.hpp

 
#ifndef TENSORFLOW_TAG_DETECTION_HPP
#define TENSORFLOW_TAG_DETECTION_HPP
 
#include "../../util/vision/YOLOModel.hpp"
 
#include <opencv2/objdetect/aruco_detector.hpp>
#include <opencv2/opencv.hpp>
#include <vector>
 
 
 
namespace tensorflowtag
{
 
    struct TensorflowTag
    {
        public:
            // Declare public struct member attributes.
            cv::Point2f CornerTL;                  // The top left corner of the bounding box.
            cv::Point2f CornerTR;                  // The top right corner of the bounding box.
            cv::Point2f CornerBL;                  // The bottom left corner of the bounding box.
            cv::Point2f CornerBR;                  // The bottom right corner of bounding box.
            double dConfidence           = 0.0;    // The detection confidence of the tag reported from the tensorflow model.
            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.
    };
 
 
    inline cv::Point2f FindTagCenter(const TensorflowTag& stTag)
    {
        // Average of the four corners
        cv::Point2f cvCenter(0, 0);
 
        // Add each tag x, y to the center x, y.
        cvCenter.x += stTag.CornerBL.x + stTag.CornerBR.x + stTag.CornerTL.x + stTag.CornerTR.x;
        cvCenter.y += stTag.CornerBL.y + stTag.CornerBR.y + stTag.CornerTL.y + stTag.CornerTR.y;
        // Divide by number of corners.
        cvCenter.x /= 4;
        cvCenter.y /= 4;
 
        // Return a copy of the center point of the tag.
        return cvCenter;
    }
 
 
    inline std::vector<TensorflowTag> Detect(const cv::Mat& cvFrame,
                                             yolomodel::tensorflow::TPUInterpreter& tfTensorflowDetector,
                                             const float fMinObjectConfidence = 0.40f,
                                             const float fNMSThreshold        = 0.60f)
    {
        // Check if the input frame is in RGB format.
        if (cvFrame.channels() != 3)
        {
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "Detect() requires a RGB image.");
            return {};
        }
 
        // Declare instance variables.
        std::vector<TensorflowTag> vDetectedTags;
 
        // Check if the tensorflow TPU interpreter hardware is opened and the model is loaded.
        if (tfTensorflowDetector.GetDeviceIsOpened())
        {
            // Run inference on YOLO model with current image.
            std::vector<std::vector<yolomodel::Detection>> vOutputTensorTags = tfTensorflowDetector.Inference(cvFrame, fMinObjectConfidence, fNMSThreshold);
 
            // Repackage detections into tensorflow tags.
            for (std::vector<yolomodel::Detection> vTagDetections : vOutputTensorTags)
            {
                // Loop through each detection.
                for (yolomodel::Detection stTagDetection : vTagDetections)
                {
                    // Create and initialize new TensorflowTag.
                    TensorflowTag stDetectedTag;
                    stDetectedTag.dConfidence = stTagDetection.fConfidence;
                    stDetectedTag.CornerTL    = cv::Point2f(stTagDetection.cvBoundingBox.x, stTagDetection.cvBoundingBox.y);
                    stDetectedTag.CornerTR    = cv::Point2f(stTagDetection.cvBoundingBox.x + stTagDetection.cvBoundingBox.width, stTagDetection.cvBoundingBox.y);
                    stDetectedTag.CornerBL    = cv::Point2f(stTagDetection.cvBoundingBox.x, stTagDetection.cvBoundingBox.y + stTagDetection.cvBoundingBox.height);
                    stDetectedTag.CornerBR    = cv::Point2f(stTagDetection.cvBoundingBox.x + stTagDetection.cvBoundingBox.width,
                                                         stTagDetection.cvBoundingBox.y + stTagDetection.cvBoundingBox.height);
 
                    // Add TensorflowTag to return vector.
                    vDetectedTags.emplace_back(stDetectedTag);
                }
            }
        }
        else
        {
            // Submit logger message.
            LOG_WARNING(logging::g_qSharedLogger,
                        "TensorflowDetect: Unable to detect tags using YOLO tensorflow detection because hardware is not opened or model is not initialized.");
        }
 
        // Return the detected tags.
        return vDetectedTags;
    }
 
 
    inline void DrawDetections(cv::Mat& cvDetectionsFrame, const std::vector<TensorflowTag>& vDetectedTags)
    {
        // Check if the given frame is a 1 or 3 channel image. (not BGRA)
        if (!cvDetectionsFrame.empty() && (cvDetectionsFrame.channels() == 1 || cvDetectionsFrame.channels() == 3))
        {
            // Loop through each detection.
            for (TensorflowTag stTag : vDetectedTags)
            {
                // Draw bounding box onto image.
                cv::rectangle(cvDetectionsFrame, stTag.CornerTL, stTag.CornerBR, cv::Scalar(255, 255, 255), 2);
                // Draw classID background box onto image.
                cv::rectangle(cvDetectionsFrame,
                              cv::Point(stTag.CornerTL.x, stTag.CornerTL.y - 20),
                              cv::Point(stTag.CornerTR.x, stTag.CornerTL.y),
                              cv::Scalar(255, 255, 255),
                              cv::FILLED);
                // Draw class text onto image.
                cv::putText(cvDetectionsFrame,
                            "Tag Conf: " + std::to_string(stTag.dConfidence),
                            cv::Point(stTag.CornerTL.x, stTag.CornerTL.y - 5),
                            cv::FONT_HERSHEY_SIMPLEX,
                            0.5,
                            cv::Scalar(0, 0, 0));
            }
        }
        else
        {
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger,
                      "TensorflowDetect: Unable to draw markers on image because it is empty or because it has {} channels. (Should be 1 or 3)",
                      cvDetectionsFrame.channels());
        }
    }
 
 
    inline void EstimatePoseFromPointCloud(const cv::Mat& cvPointCloud, TensorflowTag& stTag)
    {
        // Confirm correct coordinate system.
        if (constants::ZED_COORD_SYSTEM != sl::COORDINATE_SYSTEM::LEFT_HANDED_Y_UP)
        {
            // Submit logger message.
            LOG_CRITICAL(logging::g_qSharedLogger, "TensorflowDetection: Calculations won't work for anything other than ZED coordinate system == LEFT_HANDED_Y_UP");
        }
 
        // Find the center point of the given tag.
        cv::Point2f cvCenter = FindTagCenter(stTag);
 
        // Ensure the detected center is inside the domain of the point cloud.
        if (cvCenter.y > cvPointCloud.rows || cvCenter.x > cvPointCloud.cols || cvCenter.y < 0 || cvCenter.x < 0)
        {
            LOG_ERROR(logging::g_qSharedLogger,
                      "Detected tag center ({}, {}) out of point cloud's domain ({},{})",
                      cvCenter.y,
                      cvCenter.x,
                      cvPointCloud.rows,
                      cvPointCloud.cols);
            return;
        }
 
        // Get tag center point location relative to the camera. Point cloud location stores float x, y, z, BGRA.
        cv::Vec4f cvCoordinate = cvPointCloud.at<cv::Vec4f>(cvCenter.y, cvCenter.x);
        float fForward         = cvCoordinate[2];    // Z
        float fRight           = cvCoordinate[0];    // X
        float fUp              = cvCoordinate[1];    // Y
 
        // Calculate euclidean distance from ZED camera left eye to the point of interest
        stTag.dStraightLineDistance = sqrt(pow(fForward, 2) + pow(fRight, 2) + pow(fUp, 2));
 
        // Calculate the angle on plane horizontal to the viewpoint
        stTag.dYawAngle = atan2(fRight, fForward);
    }
}    // namespace tensorflowtag
 
#endif