works for every aruco size

roboticslab-uc3m · Jun 12, 2024 · 31b5a52 · 31b5a52
1 parent 0237ad2
commit 31b5a52
Show file tree

Hide file tree

Showing 2 changed files with 46 additions and 13 deletions.
diff --git a/libraries/YarpPlugins/ArucoCodeDetector/ArucoCodeDetector.cpp b/libraries/YarpPlugins/ArucoCodeDetector/ArucoCodeDetector.cpp
@@ -4,20 +4,42 @@
 
 #include <iostream>
 #include <vector>
+
+#include <yarp/os/LogComponent.h>
+#include <yarp/os/LogStream.h>
 #include <yarp/os/Value.h>
 #include <yarp/cv/Cv.h>
 
-#include <opencv2/core.hpp>
-#include <opencv2/core/version.hpp>
+using namespace roboticslab;
 
+namespace
+{
+    YARP_LOG_COMPONENT(AC, "rl.ArucoCodeDetector")
+}
 
-using namespace roboticslab;
+constexpr auto DEFAULT_ARUCO_SIZE = "ARUCO_ORIGINAL";
 
 bool ArucoCodeDetector::open(yarp::os::Searchable& config) 
 {
     // default params
     detectorParams = cv::aruco::DetectorParameters();
-    dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_ARUCO_ORIGINAL);
+
+    // dictionary depends on aruco size
+    std::string aruco_size = config.check("aruco_size", yarp::os::Value(DEFAULT_ARUCO_SIZE)).asString();
+
+    if (aruco_size == "4X4_1000")
+        dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_1000);
+    else if (aruco_size == "ARUCO_ORIGINAL")
+        dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_ARUCO_ORIGINAL);
+    else if (aruco_size == "6X6_1000")
+        dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_1000);
+    else if (aruco_size == "7X7_1000")
+        dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_7X7_1000);
+    else
+    {
+        yCError(AC) << "unknown aruco_size";
+        return false;
+    }
 
     return true;
 }
@@ -36,18 +58,26 @@ bool ArucoCodeDetector::detect(const yarp::sig::Image& inYarpImg, yarp::os::Bott
 
     for (auto i = 0; i < corners.size(); i++)
     {
-        const auto & corner = corners[i];
+        const auto & tl = corners[i][0];
+        const auto & tr = corners[i][1];
+        const auto & br = corners[i][2];
+        const auto & bl = corners[i][3];
 
         detectedObjects.addDict() = {
-            {"tlx", yarp::os::Value(corner[0].x)},
-            {"tly", yarp::os::Value(corner[0].y)},
-            {"trx", yarp::os::Value(corner[1].x)},
-            {"try", yarp::os::Value(corner[1].y)},
-            {"brx", yarp::os::Value(corner[2].x)},
-            {"bry", yarp::os::Value(corner[2].y)},
-            {"blx", yarp::os::Value(corner[3].x)},
-            {"bly", yarp::os::Value(corner[3].y)}
+            {"tlx", yarp::os::Value(tl.x)},
+            {"tly", yarp::os::Value(tl.y)},
+            {"trx", yarp::os::Value(tr.x)},
+            {"try", yarp::os::Value(tr.y)},
+            {"brx", yarp::os::Value(br.x)},
+            {"bry", yarp::os::Value(br.y)},
+            {"blx", yarp::os::Value(bl.x)},
+            {"bly", yarp::os::Value(bl.y)}
         };
+
+        /*yCDebug(AC) << "Top Left: " << tl.x << ", " << tl.y;
+        yCDebug(AC) << "Top Right: " << tr.x << ", " << tr.y;
+        yCDebug(AC) << "Bottom Right: " << br.x << ", " << br.y;
+        yCDebug(AC) << "Bottom Left: " << bl.x << ", " << bl.y;*/
     }
 
     return true;

diff --git a/libraries/YarpPlugins/ArucoCodeDetector/ArucoCodeDetector.hpp b/libraries/YarpPlugins/ArucoCodeDetector/ArucoCodeDetector.hpp
@@ -6,6 +6,9 @@
 #include <yarp/dev/DeviceDriver.h>
 #include <opencv2/objdetect.hpp>
 #include <opencv2/objdetect/aruco_detector.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/core/version.hpp>
+
 #include "IDetector.hpp"
 
 namespace roboticslab