From 90d5a1a2375e7e344a384f0bd57b7c5072917d71 Mon Sep 17 00:00:00 2001
From: ackimixs <ackimixs@protonmail.com>
Date: Wed, 17 Jan 2024 22:44:41 +0100
Subject: [PATCH] main aruco detector files

---
 arucoDetector.cpp | 130 ++++++----------------------------------------
 1 file changed, 17 insertions(+), 113 deletions(-)

diff --git a/arucoDetector.cpp b/arucoDetector.cpp
index a341bca..6ff7f0b 100644
--- a/arucoDetector.cpp
+++ b/arucoDetector.cpp
@@ -1,12 +1,13 @@
 #include <opencv2/opencv.hpp>
-#include <opencv2/aruco.hpp>
 
-#include "utils/utils.h"
+#include "utils/ArucoDetector.h"
 
 int main(int argc, char *argv[])
 {
-    if (argc < 2) {
-        std::cout << "Usage: " << argv[0] << " <video capture device>" << std::endl;
+    // Settup argument parser
+
+    if (argc < 3) {
+        std::cout << "Usage: " << argv[0] << " <video capture device> <path/to/calibration_results.yaml>" << std::endl;
         return 1;
     }
 
@@ -21,7 +22,7 @@ int main(int argc, char *argv[])
         }
     }
 
-    int cameraId = std::stoi(argv[1]);
+    const int cameraId = std::stoi(argv[1]);
 
     if (cameraId < 0)
     {
@@ -29,125 +30,28 @@ int main(int argc, char *argv[])
         return -1;
     }
 
-    cv::VideoCapture cap(cameraId);
+    const std::string calibrationPath = argv[2];
 
-    if (!cap.isOpened()) {
-        std::cerr << "Error opening camera." << std::endl;
-        return -1;
-    }
+    // End argument parser
 
-    if (!headless)
-    {
-        cv::namedWindow("ArUco Detection", cv::WINDOW_NORMAL);
-    }
+    const auto robotPose = Type::RobotPose{cv::Point3f(0, 0, 0), 0};
 
-    cv::Mat cameraMatrix, distCoeffs;
-    cv::FileStorage fs("../calibration_images/calibration_results.yaml", cv::FileStorage::READ);
-    if (fs.isOpened()) {
-        fs["cameraMatrix"] >> cameraMatrix;
-        fs["distCoeffs"] >> distCoeffs;
-        fs.release();
-    } else {
-        std::cerr << "Error reading calibration file." << std::endl << "See the calibration --help for more information." << std::endl;
-        return -1;
-    }
-
-    float markerLength = 0.02;
-
-    // Set coordinate system
-    cv::Mat objPoints(4, 1, CV_32FC3);
-    objPoints.ptr<cv::Vec3f>(0)[0] = cv::Vec3f(-markerLength/2.f, markerLength/2.f, 0);
-    objPoints.ptr<cv::Vec3f>(0)[1] = cv::Vec3f(markerLength/2.f, markerLength/2.f, 0);
-    objPoints.ptr<cv::Vec3f>(0)[2] = cv::Vec3f(markerLength/2.f, -markerLength/2.f, 0);
-    objPoints.ptr<cv::Vec3f>(0)[3] = cv::Vec3f(-markerLength/2.f, -markerLength/2.f, 0);
-
-
-
-    cv::aruco::DetectorParameters detectorParams = cv::aruco::DetectorParameters();
-    cv::aruco::Dictionary dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
-
-    // opencv 4.9
-    cv::aruco::ArucoDetector detector(dictionary, detectorParams);
+    ArucoDetector detector(robotPose, calibrationPath, cameraId, headless);
 
     while (true) {
-        cv::Mat frame;
-        cap >> frame;  // Capture frame from the camera
+        const int res = detector.detectArucoTags();
 
-        if (frame.empty()) {
-            std::cerr << "Error capturing frame." << std::endl;
-            break;
-        }
-
-        cv::Mat gray;
-        cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
-
-        std::vector<int> markerIds;
-        std::vector<std::vector<cv::Point2f>> markerCorners;
-
-        // cv::aruco::detectMarkers(frame, &dictionary, markerCorners, markerIds, &detectorParams);
-
-        // opencv 4.9
-        detector.detectMarkers(frame, markerCorners, markerIds);
-
-        if (!markerIds.empty()) {
-            std::cout << "Detected " << markerIds.size() << " markers." << std::endl;
-            if (!headless)
-            {
-                cv::aruco::drawDetectedMarkers(frame, markerCorners, markerIds);
-            }
-
-            size_t nMarkers = markerCorners.size();
-
-            for (size_t i = 0; i < nMarkers; i++) {
-                cv::Mat rvec, tvec;
-
-                solvePnP(objPoints, markerCorners.at(i), cameraMatrix, distCoeffs, rvec, tvec);
-
-
-
-                if (!headless)
-                {
-                    drawFrameAxes(frame, cameraMatrix, distCoeffs, rvec, tvec, markerLength/2.f);
-                    // draw::drawCenterPoints(frame, markerCorners, 100);
-                }
-
-                // Convert rotation vector to rotation matrix
-                cv::Mat rotationMatrix;
-                cv::Rodrigues(rvec, rotationMatrix);
-
-                // Extract Euler angles from the rotation matrix
-                double roll, pitch, yaw;
-                pitch = asin(rotationMatrix.at<double>(2, 0));
-                roll = atan2(-rotationMatrix.at<double>(2, 1), rotationMatrix.at<double>(2, 2));
-                yaw = atan2(-rotationMatrix.at<double>(1, 0), rotationMatrix.at<double>(0, 0));
-
-                // Convert angles from radians to degrees
-                roll *= (180.0 / CV_PI);
-                pitch *= (180.0 / CV_PI);
-                yaw *= (180.0 / CV_PI);
-
-                // Distance to ArUco marker (assuming tagLength is known)
-                double distanceX = tvec.at<double>(0, 0);
-                double distanceY = tvec.at<double>(2, 0);
-                double distanceZ = tvec.at<double>(1, 0);
-
-                // Print the distance and yaw angle
-                // std::cout << "Distance to ArUco marker " << markerIds[i] << ": " << distance << " meters" << ", pitch : " << pitch << ", roll : " << roll << ", yaw : " << yaw << std::endl;
-                std::cout << "Distance to ArUco marker " << markerIds[i] << ": " << distanceX << " " << distanceY << " " << distanceZ << std::endl;
-            }
-        }
-
-        if (!headless)
+        if (res == -2)
         {
-            cv::imshow("ArUco Detection", frame);
+            std::cerr << "Error: Could not capture frame." << std::endl;
+            return -2;
         }
 
-        if (cv::waitKey(10) == 27)  // Press 'Esc' to exit
+        if (res == 1)
+        {
             break;
+        }
     }
 
-    cap.release();
-    cv::destroyAllWindows();
-
     return 0;
 }
\ No newline at end of file