From b15fc98d35ab86d83e94040d8a6c34cc0fc40722 Mon Sep 17 00:00:00 2001
From: ackimixs <ackimixs@protonmail.com>
Date: Tue, 16 Jan 2024 23:03:33 +0100
Subject: [PATCH] add argument to the main function for headless and some
 optimization

---
 arucoDetector.cpp | 98 +++++++++++++++++++++++++++++++++++------------
 1 file changed, 74 insertions(+), 24 deletions(-)

diff --git a/arucoDetector.cpp b/arucoDetector.cpp
index 9d62040..a341bca 100644
--- a/arucoDetector.cpp
+++ b/arucoDetector.cpp
@@ -1,16 +1,45 @@
 #include <opencv2/opencv.hpp>
 #include <opencv2/aruco.hpp>
 
-int main()
+#include "utils/utils.h"
+
+int main(int argc, char *argv[])
 {
-    cv::VideoCapture cap(2);  // Open default camera (change the argument if using a different camera)
+    if (argc < 2) {
+        std::cout << "Usage: " << argv[0] << " <video capture device>" << std::endl;
+        return 1;
+    }
+
+    bool headless = false;
+
+    for (int i = 0; i < argc; i++)
+    {
+        if (std::string(argv[i]) == "--headless")
+        {
+            std::cout << "Running in headless mode." << std::endl;
+            headless = true;
+        }
+    }
+
+    int cameraId = std::stoi(argv[1]);
+
+    if (cameraId < 0)
+    {
+        std::cerr << "Error: Camera ID must be a positive integer." << std::endl;
+        return -1;
+    }
+
+    cv::VideoCapture cap(cameraId);
 
     if (!cap.isOpened()) {
         std::cerr << "Error opening camera." << std::endl;
         return -1;
     }
 
-    cv::namedWindow("ArUco Detection", cv::WINDOW_NORMAL);
+    if (!headless)
+    {
+        cv::namedWindow("ArUco Detection", cv::WINDOW_NORMAL);
+    }
 
     cv::Mat cameraMatrix, distCoeffs;
     cv::FileStorage fs("../calibration_images/calibration_results.yaml", cv::FileStorage::READ);
@@ -19,11 +48,11 @@ int main()
         fs["distCoeffs"] >> distCoeffs;
         fs.release();
     } else {
-        std::cerr << "Error reading calibration file." << std::endl;
+        std::cerr << "Error reading calibration file." << std::endl << "See the calibration --help for more information." << std::endl;
         return -1;
     }
 
-    float markerLength = 0.006;
+    float markerLength = 0.02;
 
     // Set coordinate system
     cv::Mat objPoints(4, 1, CV_32FC3);
@@ -37,8 +66,8 @@ int main()
     cv::aruco::DetectorParameters detectorParams = cv::aruco::DetectorParameters();
     cv::aruco::Dictionary dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
 
-    // 4.9
-    // cv::aruco::ArucoDetector detector(dictionary, detectorParams);
+    // opencv 4.9
+    cv::aruco::ArucoDetector detector(dictionary, detectorParams);
 
     while (true) {
         cv::Mat frame;
@@ -55,13 +84,17 @@ int main()
         std::vector<int> markerIds;
         std::vector<std::vector<cv::Point2f>> markerCorners;
 
-        cv::aruco::detectMarkers(frame, &dictionary, markerCorners, markerIds, &detectorParams);
+        // cv::aruco::detectMarkers(frame, &dictionary, markerCorners, markerIds, &detectorParams);
 
-        // 4.9
-        // detector.detectMarkers(frame, markerCorners, markerIds);
+        // opencv 4.9
+        detector.detectMarkers(frame, markerCorners, markerIds);
 
         if (!markerIds.empty()) {
-            cv::aruco::drawDetectedMarkers(frame, markerCorners, markerIds);
+            std::cout << "Detected " << markerIds.size() << " markers." << std::endl;
+            if (!headless)
+            {
+                cv::aruco::drawDetectedMarkers(frame, markerCorners, markerIds);
+            }
 
             size_t nMarkers = markerCorners.size();
 
@@ -69,30 +102,47 @@ int main()
                 cv::Mat rvec, tvec;
 
                 solvePnP(objPoints, markerCorners.at(i), cameraMatrix, distCoeffs, rvec, tvec);
-                drawFrameAxes(frame, cameraMatrix, distCoeffs, rvec, tvec, markerLength/2.f);
 
-                // Extract rotation matrix from rotation vector
-                cv::Mat R;
-                Rodrigues(rvec, R);
 
-                // Calculate Euler angles (in degrees)
-                cv::Mat euler;
-                Rodrigues(R, euler);
 
-                // Access elements of the Euler angles matrix
-                double yaw = euler.at<double>(2, 0) * (180.0 / CV_PI);
+                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 distance = tvec.at<double>(2, 0);
+                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" << ", " << yaw << " degrees" << std::endl;
+                // 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;
             }
         }
 
-        cv::imshow("ArUco Detection", frame);
+        if (!headless)
+        {
+            cv::imshow("ArUco Detection", frame);
+        }
 
-        if (cv::waitKey(30) == 27)  // Press 'Esc' to exit
+        if (cv::waitKey(10) == 27)  // Press 'Esc' to exit
             break;
     }