update

CMakeLists.txt

@@ -8,6 +8,7 @@ include_directories( ${OpenCV_INCLUDE_DIRS} )
 
 set(COMMON_SOURCES
         utils/utils.h
+        utils/utils.cpp
 )
 
 set(calibrationSources

arucoDetector.cpp

@@ -51,6 +51,17 @@ int main(int argc, char *argv[])
         {
             break;
         }
+
+        for (auto p : res.second)
+        {
+            if (p.first.type == FLOWER)
+            {
+                ArucoDetector::flowerDetector(p.first, p.second.first, p.second.first);
+            } else if (p.first.type == SOLAR_PANEL)
+            {
+                ArucoDetector::solarPanelDetector(p.first, p.second.first, p.second.first, robotPose);
+            }
+        }
     }
 
     return 0;

calibration_images/calibration_results.yaml

@@ -4,12 +4,12 @@ cameraMatrix: !!opencv-matrix
    rows: 3
    cols: 3
    dt: d
-   data: [ 1.5210854147825523e+03, 0., 9.5264072920750630e+02, 0.,
-       1.5220010501373945e+03, 5.5175450881770780e+02, 0., 0., 1. ]
+   data: [ 1.5045814501202790e+03, 0., 9.6442035005869900e+02, 0.,
+       1.5066446935120762e+03, 5.3637144548900244e+02, 0., 0., 1. ]
 distCoeffs: !!opencv-matrix
    rows: 1
    cols: 5
    dt: d
-   data: [ 3.6818238470785941e-01, -2.4288949326716387e+00,
-       2.4611682433000304e-03, 6.8682976250353874e-04,
-       4.9441917218577958e+00 ]
+   data: [ 3.9626454172776632e-01, -2.5689007335445475e+00,
+       -4.2189376866905628e-03, 2.2079481537779400e-03,
+       4.5390098277697222e+00 ]

utils/ArucoDetector.cpp

@@ -1,8 +1,11 @@
 #include "ArucoDetector.h"
 
-ArucoDetector::ArucoDetector(const Type::RobotPose pose, const std::string& calibrationPath, const Team team, const int cameraId, const bool headless) : robotPose(pose), headless(headless), team(team)
+ArucoDetector::ArucoDetector(const Type::RobotPose& pose, const std::string& calibrationPath, const Team team, const int cameraId, const bool headless) : robotPose(pose), headless(headless), team(team)
 {
-    this->detector = cv::aruco::ArucoDetector(getPredefinedDictionary(cv::aruco::DICT_4X4_50), cv::aruco::DetectorParameters());
+    // this->detector = cv::aruco::ArucoDetector(getPredefinedDictionary(cv::aruco::DICT_4X4_50), cv::aruco::DetectorParameters());
+    this->dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
+    this->parameters = cv::aruco::DetectorParameters();
+
     this->transformationMatrix = (cv::Mat_<double>(4, 4) <<
         cos(pose.theta), 0, sin(pose.theta), pose.position.x,
         0, 1, 0, pose.position.y,
@@ -60,12 +63,12 @@ void ArucoDetector::addArucoTag(const ArucoTag& tag)
     this->arucoTags.push_back(tag);
 }
 
-std::pair<int, std::vector<std::pair<cv::Mat, cv::Mat>>> ArucoDetector::detectArucoTags()
+std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> ArucoDetector::detectArucoTags()
 {
     cv::Mat frame;
     cap >> frame;  // Capture frame from the camera
 
-    std::pair<int, std::vector<std::pair<cv::Mat, cv::Mat>>> result;
+    std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> result;
 
     if (frame.empty()) {
         std::cerr << "Error capturing frame." << std::endl;
@@ -79,10 +82,10 @@ std::pair<int, std::vector<std::pair<cv::Mat, cv::Mat>>> ArucoDetector::detectAr
     std::vector<int> markerIds;
     std::vector<std::vector<cv::Point2f>> markerCorners;
 
-    // cv::aruco::detectMarkers(frame, &dictionary, markerCorners, markerIds, &detectorParams);
+    cv::aruco::detectMarkers(frame, &this->dictionary, markerCorners, markerIds, &parameters);
 
-    // opencv 4.9
-    detector.detectMarkers(frame, markerCorners, markerIds);
+    // opencv 4.8
+    // detector.detectMarkers(frame, markerCorners, markerIds);
 
     if (!markerIds.empty())
     {
@@ -131,21 +134,13 @@ std::pair<int, std::vector<std::pair<cv::Mat, cv::Mat>>> ArucoDetector::detectAr
 
             cv::Mat transformedTvec = (transformationMatrix * translat);
 
-            result.second.emplace_back(transformedTvec, rotaEuler);
-
-            if (tag.type == FLOWER)
-            {
-                flowerDetector(tag, transformedTvec, rotaEuler);
-            } else if (tag.type == SOLAR_PANEL)
-            {
-                solarPanelDetector(tag, transformedTvec, rotaEuler);
-            }
+            result.second.emplace_back(tag, std::make_pair(transformedTvec, rotaEuler));
         }
     }
 
-    std::sort(result.second.begin(), result.second.end(), [](const std::pair<cv::Mat, cv::Mat>& a, const 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 a.first.at<double>(2, 0) < b.first.at<double>(2, 0);
+        return distanceBetweenRobotAndTag(robotPose, a.second.first) < distanceBetweenRobotAndTag(robotPose, b.second.first);
     });
 
     if (!headless)
@@ -174,7 +169,7 @@ void ArucoDetector::flowerDetector(const ArucoTag& tag, const cv::Mat& translati
     std::cout << tag.name << " Pos : x: " << distanceXFlower << " z: " << distanceZFlower << " " << std::endl;
 }
 
-void ArucoDetector::solarPanelDetector(const ArucoTag& tag, cv::Mat translationMatrix, const cv::Mat& rotationMatrix)
+void ArucoDetector::solarPanelDetector(const ArucoTag& tag, cv::Mat translationMatrix, const cv::Mat& rotationMatrix, const Type::RobotPose& robotPose)
 {
     std::cout << tag.name << " Pos : x: " << translationMatrix.at<double>(0, 0) << " z: " << translationMatrix.at<double>(2, 0) << " " << std::endl;
     const auto yaw = rotationMatrix.at<double>(2, 0);

utils/ArucoDetector.h

@@ -13,7 +13,9 @@ class ArucoDetector {
 
     cv::VideoCapture cap;
 
-    cv::aruco::ArucoDetector detector;
+    cv::aruco::Dictionary dictionary;
+    cv::aruco::DetectorParameters parameters;
+    // cv::aruco::ArucoDetector detector;
 
     bool headless;
 
@@ -22,22 +24,20 @@ class ArucoDetector {
     Team team;
 
 public:
-    // TODO triée les tag dans la liste en fonction de la distance
-
-    ArucoDetector(Type::RobotPose pose, const std::string& calibrationPath, Team team, int cameraId = 0, bool headless = false);
+    ArucoDetector(const Type::RobotPose& pose, const std::string& calibrationPath, Team team, int cameraId = 0, bool headless = false);
 
     ArucoDetector(float x, float y, float z, float theta, const std::string& calibrationPath, Team team, int cameraId = 0, bool headless = false);
 
     ~ArucoDetector();
 
-    std::pair<int, std::vector<std::pair<cv::Mat, cv::Mat>>> detectArucoTags();
+    std::pair<int, std::vector<std::pair<ArucoTag, std::pair<cv::Mat, cv::Mat>>>> detectArucoTags();
 
     void readCameraParameters(const std::string& path);
 
     void addArucoTag(const ArucoTag& tag);
 
-    void flowerDetector(const ArucoTag& type, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix);
+    static void flowerDetector(const ArucoTag& type, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix);
 
-    void solarPanelDetector(const ArucoTag& type, cv::Mat translationMatrix, const cv::Mat& rotationMatrix);
+    static void solarPanelDetector(const ArucoTag& type, cv::Mat translationMatrix, const cv::Mat& rotationMatrix, const Type::RobotPose& robotPose);
 
 };

utils/utils.cpp

@@ -0,0 +1,6 @@
+#include "utils.h"
+
+double distanceBetweenRobotAndTag(const Type::RobotPose robotPose, 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));
+}

utils/utils.h

@@ -24,7 +24,4 @@ enum Team
     YELLOW
 };
 
-double distance(cv::Mat a)
-{
-
-}
+double distanceBetweenRobotAndTag(Type::RobotPose robotPose, const cv::Mat& a);