Merge pull request #3 from modelec/cpp

Cpp

CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.27)
-project(detection_pot)
+project(detection_pot VERSION 1.0.0)
 
 set(CMAKE_CXX_STANDARD 17)
 

arucoDetector.cpp

@@ -34,20 +34,20 @@ int main(int argc, char *argv[])
 
     // End argument parser
 
-    const auto robotPose = Type::RobotPose{cv::Point3f(0, 0, 0), 0};
+    const auto robotPose = Type::RobotPose{cv::Point3f(0, 0, 0), CV_PI/2};
 
-    ArucoDetector detector(robotPose, calibrationPath, cameraId, headless);
+    ArucoDetector detector(robotPose, calibrationPath, BLUE, cameraId, headless);
 
     while (true) {
-        const int res = detector.detectArucoTags();
+        const auto res = detector.detectArucoTags();
 
-        if (res == -2)
+        if (res.first == -2)
         {
             std::cerr << "Error: Could not capture frame." << std::endl;
             return -2;
         }
 
-        if (res == 1)
+        if (res.first == 1)
         {
             break;
         }

utils/ArucoDetector.cpp

@@ -1,6 +1,6 @@
 #include "ArucoDetector.h"
 
-ArucoDetector::ArucoDetector(const Type::RobotPose pose, const std::string& calibrationPath, const int cameraId, const bool headless) : robotPose(pose), headless(headless)
+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->transformationMatrix = (cv::Mat_<double>(4, 4) <<
@@ -22,12 +22,17 @@ ArucoDetector::ArucoDetector(const Type::RobotPose pose, const std::string& cali
         cv::namedWindow("ArUco Detection", cv::WINDOW_NORMAL);
     }
 
-    this->addArucoTag(ArucoTag(36, "White flower", 20, FLOWER));
+    auto whiteFlower = ArucoTag(36, "White flower", 20, FLOWER);
-    this->addArucoTag(ArucoTag(13, "Purple flower", 20, FLOWER));
+    whiteFlower.setFlowerObjectRepresentation();
-    this->addArucoTag(ArucoTag(47, "Solar panel", 20, FLOWER));
+    this->addArucoTag(whiteFlower);
+    auto purpleFlower = ArucoTag(13, "Purple flower", 20, FLOWER);
+    purpleFlower.setFlowerObjectRepresentation();
+    this->addArucoTag(purpleFlower);
+
+    this->addArucoTag(ArucoTag(47, "Solar panel", 50, SOLAR_PANEL));
 }
 
-ArucoDetector::ArucoDetector(const float x, const float y, const float z, const float theta, const std::string& calibrationPath, const int cameraId, const bool headless) : ArucoDetector(Type::RobotPose{cv::Point3f(x, y, z), theta}, calibrationPath, cameraId, headless)
+ArucoDetector::ArucoDetector(const float x, const float y, const float z, const float theta, const std::string& calibrationPath, const Team team, const int cameraId, const bool headless) : ArucoDetector(Type::RobotPose{cv::Point3f(x, y, z), theta}, calibrationPath, team, cameraId, headless)
 {
 }
 
@@ -55,14 +60,17 @@ void ArucoDetector::addArucoTag(const ArucoTag& tag)
     this->arucoTags.push_back(tag);
 }
 
-int ArucoDetector::detectArucoTags()
+std::pair<int, std::vector<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;
+
     if (frame.empty()) {
         std::cerr << "Error capturing frame." << std::endl;
-        return -2;
+        result.first = -2;
+        return result;
     }
 
     cv::Mat gray;
@@ -84,7 +92,8 @@ int ArucoDetector::detectArucoTags()
             cv::aruco::drawDetectedMarkers(frame, markerCorners, markerIds);
         }
 
-        for (size_t i = 0; i < markerCorners.size(); i++) {
+        for (size_t i = 0; i < markerCorners.size(); i++)
+        {
             int id = markerIds[i];
 
             if (std::find_if(arucoTags.begin(), arucoTags.end(), [id](const ArucoTag& tag) { return tag.id == id; }) == arucoTags.end())
@@ -122,6 +131,8 @@ int ArucoDetector::detectArucoTags()
 
             cv::Mat transformedTvec = (transformationMatrix * translat);
 
+            result.second.emplace_back(transformedTvec, rotaEuler);
+
             if (tag.type == FLOWER)
             {
                 flowerDetector(tag, transformedTvec, rotaEuler);
@@ -132,29 +143,59 @@ int ArucoDetector::detectArucoTags()
         }
     }
 
+    std::sort(result.second.begin(), result.second.end(), [](const std::pair<cv::Mat, cv::Mat>& a, const std::pair<cv::Mat, cv::Mat>& b)
+    {
+        return a.first.at<double>(2, 0) < b.first.at<double>(2, 0);
+    });
+
     if (!headless)
     {
         cv::imshow("ArUco Detection", frame);
     }
 
-    if (cv::waitKey(10) == 27)  // Press 'Esc' to exit
+    if (cv::waitKey(10) == 27)
-        return 1;
+    {
+        // Press 'Esc' to exit
+        result.first = 1;
+        return result;
+    }
 
-    return 0;
+    result.first = 0;
+    return result;
 }
 
 void ArucoDetector::flowerDetector(const ArucoTag& tag, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix)
 {
     constexpr double distanceToPot = 21;
 
-    const double distanceXFlower = translationMatrix.at<double>(0, 0) + (distanceToPot * sin(rotationMatrix.at<double>(1, 0)));
+    const double distanceXFlower = translationMatrix.at<double>(0, 0);// + (distanceToPot * sin(rotationMatrix.at<double>(1, 0)));
-    const double distanceZFlower = translationMatrix.at<double>(2, 0) + (distanceToPot * cos(rotationMatrix.at<double>(1, 0)));
+    const double distanceZFlower = translationMatrix.at<double>(2, 0);// + (distanceToPot * cos(rotationMatrix.at<double>(1, 0)));
 
     std::cout << tag.name << " Pos : x: " << distanceXFlower << " z: " << distanceZFlower << " " << std::endl;
 }
 
-void ArucoDetector::solarPanelDetector(ArucoTag tag, cv::Mat translationMatrix, cv::Mat rotationMatrix)
+void ArucoDetector::solarPanelDetector(const ArucoTag& tag, cv::Mat translationMatrix, const cv::Mat& rotationMatrix)
 {
     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);
+
+    const auto rotationBaseTable = (-yaw) + robotPose.theta;
+
+    std::cout << " Rotation: " << rotationBaseTable * (180 / CV_PI) << std::endl;
+
+    if (rotationBaseTable > 70 && rotationBaseTable <= 110)
+    {
+        std::cout << "Mid" << std::endl;
+    }
+    else if (rotationBaseTable > 30 && rotationBaseTable <= 70)
+    {
+        std::cout << "Blue side" << std::endl;
+    }
+    else if (rotationBaseTable > 110 && rotationBaseTable <= 150)
+    {
+        std::cout << "Yellow side" << std::endl;
+    }
+
+    // BLUE => 90, YELLOW => -90
 }
 

utils/ArucoDetector.h

@@ -19,14 +19,18 @@ class ArucoDetector {
 
     cv::Mat transformationMatrix;
 
-public:
+    Team team;
-    ArucoDetector(Type::RobotPose pose, const std::string& calibrationPath, int cameraId = 0, bool headless = false);
 
-    ArucoDetector(float x, float y, float z, float theta, const std::string& calibrationPath, int cameraId = 0, bool headless = false);
+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(float x, float y, float z, float theta, const std::string& calibrationPath, Team team, int cameraId = 0, bool headless = false);
 
     ~ArucoDetector();
 
-    int detectArucoTags();
+    std::pair<int, std::vector<std::pair<cv::Mat, cv::Mat>>> detectArucoTags();
 
     void readCameraParameters(const std::string& path);
 
@@ -34,6 +38,6 @@ public:
 
     void flowerDetector(const ArucoTag& type, const cv::Mat& translationMatrix, const cv::Mat& rotationMatrix);
 
-    void solarPanelDetector(ArucoTag type, cv::Mat translationMatrix, cv::Mat rotationMatrix);
+    void solarPanelDetector(const ArucoTag& type, cv::Mat translationMatrix, const cv::Mat& rotationMatrix);
 
 };

utils/ArucoTag.h

@@ -29,9 +29,10 @@ public:
 
     void setFlowerObjectRepresentation()
     {
-        this->objectRepresenation.ptr<cv::Vec3f>(0)[0] = cv::Vec3f(-length/2.f, length/2.f, 0);
+        this->objectRepresenation = cv::Mat(4, 1, CV_32FC3);
-        this->objectRepresenation.ptr<cv::Vec3f>(0)[1] = cv::Vec3f(length/2.f, length/2.f, 0);
+        this->objectRepresenation.ptr<cv::Vec3f>(0)[0] = cv::Vec3f(-19/2.f, 19/2.f, 0);
-        this->objectRepresenation.ptr<cv::Vec3f>(0)[2] = cv::Vec3f(length/2.f, -length/2.f, 0);
+        this->objectRepresenation.ptr<cv::Vec3f>(0)[1] = cv::Vec3f(19/2.f, 19/2.f, 0);
-        this->objectRepresenation.ptr<cv::Vec3f>(0)[3] = cv::Vec3f(-length/2.f, -length/2.f, 0);
+        this->objectRepresenation.ptr<cv::Vec3f>(0)[2] = cv::Vec3f(18.7f/2.f, -19/2.f, 0);
+        this->objectRepresenation.ptr<cv::Vec3f>(0)[3] = cv::Vec3f(-18.7f/2.f, -19/2.f, 0);
     }
 };

utils/utils.h

@@ -17,3 +17,14 @@ namespace Type
         float theta; // rotation around the y axis
     };
 }
+
+enum Team
+{
+    BLUE,
+    YELLOW
+};
+
+double distance(cv::Mat a)
+{
+
+}