From c52858aef24fc73ed0c88706edce8d87ec885caa Mon Sep 17 00:00:00 2001
From: ackimixs <ackimixs@protonmail.com>
Date: Wed, 10 Apr 2024 12:59:02 +0200
Subject: [PATCH] send the position of the tag in the robot xyz base | log
 rotation matrix

---
 aruco/ArucoDetector.cpp | 10 +++++++---
 utils/utils.cpp         |  6 +++---
 utils/utils.h           |  8 +-------
 3 files changed, 11 insertions(+), 13 deletions(-)

diff --git a/aruco/ArucoDetector.cpp b/aruco/ArucoDetector.cpp
index 0400f35..a5c3389 100644
--- a/aruco/ArucoDetector.cpp
+++ b/aruco/ArucoDetector.cpp
@@ -189,6 +189,8 @@ std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> Ar
             cv::Mat rotationMatrix;
             cv::Rodrigues(rvec, rotationMatrix);
 
+            std::cout << rvec << std::endl;
+
             // Extract Euler angles from the rotation matrix
             double roll, pitch, yaw;
             pitch = asin(rotationMatrix.at<double>(2, 0));
@@ -196,6 +198,8 @@ std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> Ar
             yaw = atan2(-rotationMatrix.at<double>(1, 0), rotationMatrix.at<double>(0, 0));
             // Angles can be used to calculate the distance to the center of the flower.
 
+            std::cout << roll, pitch, yaw << std::endl;
+
             cv::Mat rotaEuler = (cv::Mat_<double>(3, 1) << roll, pitch, yaw);
 
             // Apply the homogeneous transformation to tvec
@@ -205,10 +209,10 @@ std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> Ar
         }
     }
 
-    /*std::sort(result.second.begin(), result.second.end(), [this](const std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>& a, const std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>& b)
+    std::sort(result.second.begin(), result.second.end(), [this](const std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>& a, const std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>& b)
     {
-        return // TODO sort with the tvec
-    });*/
+        return distanceBetweenRobotAndTag(a.second.first) < distanceBetweenRobotAndTag(b.second.first);
+    });
 
     if (!headless)
     {
diff --git a/utils/utils.cpp b/utils/utils.cpp
index ae23a01..b257d27 100644
--- a/utils/utils.cpp
+++ b/utils/utils.cpp
@@ -1,6 +1,6 @@
 #include "utils.h"
 
-double distanceBetweenRobotAndTag(const Type::RobotPose& robotPose, const cv::Mat& a)
+double distanceBetweenRobotAndTag(const cv::Mat& a)
 {
-    return sqrt(pow(robotPose.position.x + a.at<double>(0, 0), 2) + pow(robotPose.position.y + a.at<double>(1, 0), 2) + pow(robotPose.position.z + a.at<double>(2, 0), 2));
-}
+    return sqrt(pow(a.at<double>(0, 0), 2) + pow(a.at<double>(1, 0), 2) + pow(a.at<double>(2, 0), 2));
+}
\ No newline at end of file
diff --git a/utils/utils.h b/utils/utils.h
index de7e0a6..0793b5c 100644
--- a/utils/utils.h
+++ b/utils/utils.h
@@ -10,12 +10,6 @@ namespace Type
         float pitch;
         float yaw;
     };
-
-    struct RobotPose
-    {
-        cv::Point3f position;
-        float theta; // rotation around the y axis
-    };
 }
 
 enum Team
@@ -24,4 +18,4 @@ enum Team
     YELLOW
 };
 
-double distanceBetweenRobotAndTag(const Type::RobotPose& robotPose, const cv::Mat& a);
\ No newline at end of file
+double distanceBetweenRobotAndTag(const cv::Mat& a);
\ No newline at end of file