Improve detection, draw marker borders

1 files changed, 9 insertions, 3 deletions

diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 366b5b51..84a3de73 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -184,14 +184,14 @@ void Tracker::run()
     
     aruco::MarkerDetector detector;
     detector.setDesiredSpeed(3);
-    cv::Mat color, grayscale, rvec, tvec, tmp;
+    detector.setThresholdParams(11, 6);
+    cv::Mat color, grayscale, rvec, tvec;
   
     while (!stop)
     {
         if (!camera.read(color))
             break;
-        cv::cvtColor(color, tmp, cv::COLOR_BGR2GRAY);
-        cv::GaussianBlur(tmp, grayscale, cv::Size(5, 5), 0);
+        cv::cvtColor(color, grayscale, cv::COLOR_BGR2GRAY);
         const float focal_length_w = 0.5 * grayscale.cols / tan(0.5 * fov * HT_PI / 180);
         const float focal_length_h = 0.5 * grayscale.rows / tan(0.5 * fov * grayscale.rows / grayscale.cols * HT_PI / 180.0);
         cv::Mat intrinsics = cv::Mat::eye(3, 3, CV_32FC1);
@@ -209,6 +209,12 @@ void Tracker::run()
         
         detector.detect(grayscale, markers, cv::Mat(), cv::Mat(), -1, false);
         
+        if (markers.size() == 1 && markers[0].size() == 4) {
+            const aruco::Marker& m = markers.at(0);
+            for (int i = 0; i < 4; i++)
+                cv::line(color, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), 4);
+        }
+        
         frame = color;
         
         if (frame.rows > 0 && !fresh)