From 04814bca27225540202d6de96b78bdbb4b64bd3a Mon Sep 17 00:00:00 2001
From: Thomas Bonnier <thomas.bonnier@ecam.fr>
Date: Tue, 19 Nov 2024 11:51:59 +0100
Subject: [PATCH] enhancing with intrinsic matrix camera.

---
 test/src/QRcode.cpp | 51 ++++++++++++++++++++++++++++++++-------------
 1 file changed, 36 insertions(+), 15 deletions(-)

diff --git a/test/src/QRcode.cpp b/test/src/QRcode.cpp
index cfd7c94..5d1fe77 100644
--- a/test/src/QRcode.cpp
+++ b/test/src/QRcode.cpp
@@ -3,14 +3,18 @@
 #include <opencv2/aruco.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
+#include <thread>
+
+cv::Mat cameraMatrix, distCoeffs;
 
 void processFrame(const cv::Mat& frame)
 {
     // Create ArUco marker detector
     cv::Ptr<cv::aruco::Dictionary> dictionary = 
-        cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); // Change dictionary type here
+        cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); 
     std::vector<int> markerIds;
     std::vector<std::vector<cv::Point2f>> markerCorners;
+    std::vector<cv::Vec3d> rvecs, tvecs;
 
     // Detect markers in the image
     cv::aruco::detectMarkers(frame, dictionary, markerCorners, markerIds);
@@ -19,10 +23,12 @@ void processFrame(const cv::Mat& frame)
     if (!markerIds.empty())
     {
         cv::aruco::drawDetectedMarkers(frame, markerCorners, markerIds);
+        cv::aruco::estimatePoseSingleMarkers(markerCorners, 0.05, cameraMatrix, distCoeffs, rvecs, tvecs);
         ROS_INFO("Detected %zu ArUco markers", markerIds.size());
         for (size_t i = 0; i < markerIds.size(); ++i)
         {
             ROS_INFO("Marker ID: %d", markerIds[i]);
+            cv::aruco::drawAxis(frame, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
         }
     }
     else
@@ -35,25 +41,19 @@ void processFrame(const cv::Mat& frame)
     cv::waitKey(1);
 }
 
-int main(int argc, char** argv)
+void frameProcessingThread()
 {
-    ros::init(argc, argv, "qr_code_detector");
-    ros::NodeHandle nh;
-
     cv::VideoCapture cap(0); // Open the default camera
     if (!cap.isOpened())
     {
-        ROS_ERROR("Could not open the camera");
-        return -1;
+        ROS_ERROR("Unable to open camera");
+        return;
     }
 
-    cv::namedWindow("ArUco Detection");
-
     while (ros::ok())
     {
         cv::Mat frame;
-        cap >> frame; // Capture a new frame from the camera
-
+        cap >> frame; // Capture a new frame
         if (frame.empty())
         {
             ROS_ERROR("Captured empty frame");
@@ -61,9 +61,30 @@ int main(int argc, char** argv)
         }
 
         processFrame(frame);
-        ros::spinOnce();
     }
-
-    cv::destroyWindow("ArUco Detection");
-    return 0;
 }
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "aruco_detector");
+    ros::NodeHandle nh;
+
+    // Load camera parameters
+    cv::FileStorage fs("camera_parameters.yml", cv::FileStorage::READ);
+    if (!fs.isOpened())
+    {
+        ROS_ERROR("Unable to open camera parameters file");
+        return -1;
+    }
+    fs["camera_matrix"] >> cameraMatrix;
+    fs["distortion_coefficients"] >> distCoeffs;
+    fs.release();
+
+    // Start frame processing thread
+    std::thread processingThread(frameProcessingThread);
+
+    ros::spin();
+
+    processingThread.join();
+    return 0;
+}
\ No newline at end of file