ArucoDetection.hpp

 
#ifndef ARUCO_DETECTION_HPP
#define ARUCO_DETECTION_HPP
 
#include "../../AutonomyLogging.h"
#include "../../util/vision/ImageOperations.hpp"
#include "../../util/vision/TagDetectionUtilty.hpp"
 
#include <chrono>
#include <opencv2/imgproc.hpp>
#include <opencv2/objdetect/aruco_detector.hpp>
#include <opencv2/opencv.hpp>
#include <vector>
 
 
 
namespace arucotag
{
 
    inline void PreprocessFrame(const cv::Mat& cvInputFrame, cv::Mat& cvOutputFrame)
    {
        // Check if the input frame is in BGR format.
        if (cvInputFrame.channels() != 3)
        {
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "PreprocessFrame() requires a BGR image.");
            return;
        }
 
        // Grayscale.
        cv::cvtColor(cvInputFrame, cvOutputFrame, cv::COLOR_BGR2GRAY);
        // cv::filter2D(cvOutputFrame, cvOutputFrame, -1, constants::ARUCO_EDGE_KERNEL);
        // Reduce number of colors/gradients in the image.
        // imgops::ColorReduce(cvOutputFrame);
        // Denoise (Looks like bilateral filter is req. for ArUco, check speed since docs say it's slow)
        // cv::bilateralFilter(cvInputFrame, cvInputFrame, /*diameter =*/5, /*sigmaColor =*/0.2, /*sigmaSpace =*/3);
        // imgops::CustomBilateralFilter(cvInputFrame, 3, 0.1, 3);
        // Deblur? (Would require determining point spread function that caused the blur)
 
        // Threshold mask (could use OTSU or TRIANGLE, just a const threshold for now)
        // cv::threshold(cvInputFrame, cvInputFrame, constants::ARUCO_PIXEL_THRESHOLD, constants::ARUCO_PIXEL_THRESHOLD_MAX_VALUE, cv::THRESH_BINARY);
 
        // Super-Resolution
        // std::string szModelPath = "ESPCN_x3.pb";
        // std::string szModelName = "espcn";
        // dnn_superres::DnnSuperResImpl dnSuperResModel;
        // dnSuperResModel.readModel(/*path*/);
        // dnSuperResModel.setModel(/*path, scale*/);
        // dnSuperResModel.upsample(cvInputFrame, cvOutputFrame);
    }
 
 
    inline std::vector<tagdetectutils::ArucoTag> Detect(const cv::Mat& cvFrame, const cv::aruco::ArucoDetector& cvArucoDetector)
    {
        std::vector<int> vIDs;
        std::vector<std::vector<cv::Point2f>> cvMarkerCorners, cvRejectedCandidates;
 
        // Run Aruco detection algorithm.
        cvArucoDetector.detectMarkers(cvFrame, cvMarkerCorners, vIDs, cvRejectedCandidates);
 
        // Store all of the detected tags as tagdetectutils::ArucoTag.
        std::vector<tagdetectutils::ArucoTag> vDetectedTags;
        vDetectedTags.reserve(vIDs.size());
 
        // Loop through each detection and build tag for it.
        for (long unsigned int unIter = 0; unIter < vIDs.size(); unIter++)
        {
            // Create and initialize new tag.
            tagdetectutils::ArucoTag stDetectedTag;
            stDetectedTag.nID = vIDs[unIter];
            // Copy corners.
            stDetectedTag.pBoundingBox      = std::make_shared<cv::Rect2d>(cv::boundingRect(cvMarkerCorners[unIter]));
            stDetectedTag.eDetectionMethod  = tagdetectutils::TagDetectionMethod::eOpenCV;
            stDetectedTag.szClassName       = "OpenCVTag";
            stDetectedTag.cvImageResolution = cvFrame.size();
 
            // Add new tag to detected tags vector.
            vDetectedTags.push_back(stDetectedTag);
        }
 
        // Return the detected tags.
        return vDetectedTags;
    }
 
 
    inline void DrawDetections(cv::Mat& cvDetectionsFrame, const std::vector<tagdetectutils::ArucoTag>& 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 of the given AR tags and repackage them so that the draw function can read them.
            for (long unsigned int nIter = 0; nIter < vDetectedTags.size(); ++nIter)
            {
                // Check if the tag detection type is OpenCV.
                if (vDetectedTags[nIter].eDetectionMethod == tagdetectutils::TagDetectionMethod::eOpenCV)
                {
                    // Draw bounding box onto image.
                    cv::rectangle(cvDetectionsFrame, *vDetectedTags[nIter].pBoundingBox, cv::Scalar(0, 0, 0), 2);
                    // Draw tag ID onto image.
                    std::string szText  = "TAG " + std::to_string(vDetectedTags[nIter].nID);
                    cv::Size cvTextSize = cv::getTextSize(szText, cv::FONT_HERSHEY_SIMPLEX, 0.75, 1, nullptr);
                    cv::rectangle(cvDetectionsFrame,
                                  vDetectedTags[nIter].pBoundingBox->tl() - cv::Point2d(0, cvTextSize.height),
                                  vDetectedTags[nIter].pBoundingBox->tl() + cv::Point2d(cvTextSize.width, 0),
                                  cv::Scalar(0, 0, 0),
                                  cv::FILLED);
                    cv::putText(cvDetectionsFrame, szText, vDetectedTags[nIter].pBoundingBox->tl(), cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(255, 255, 255));
                }
            }
        }
        else
        {
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger,
                      "ArucoDetect: Unable to draw markers on image because it is empty or because it has {} channels. (Should be 1 or 3)",
                      cvDetectionsFrame.channels());
        }
    }
}    // namespace arucotag
 
#endif