aruco: wait fraction of second between switching box sizes

1 files changed, 21 insertions, 11 deletions

diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 34bffa0a..9f85fcce 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -124,6 +124,8 @@ void Tracker::run()
     
     std::vector<int> box_sizes { 5, 7, 9, 11 };
     int box_idx = 0;
+    double failed = 0;
+    const double max_failed = .334;
 
     while (!stop)
     {
@@ -164,6 +166,7 @@ void Tracker::run()
             if (detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false),
                 markers.size() == 1 && markers[0].size() == 4)
             {
+                failed = 0;
                 auto& m = markers.at(0);
                 for (int i = 0; i < 4; i++)
                 {
@@ -174,11 +177,27 @@ void Tracker::run()
                 roi_valid = true;
             }
         }
+        
+        auto time = cv::getTickCount();
+
+        if ((long) (time / freq) != (long) (last_time / freq))
+        {
+            last_fps = cur_fps;
+            cur_fps = 0;
+            last_time = time;
+        }
+        
+        const double dt = (time - last_time) / freq;
 
         if (!roi_valid)
         {
-            box_idx++;
-            box_idx %= box_sizes.size();
+            failed += dt;
+            if (failed > max_failed)
+            {
+                box_idx++;
+                box_idx %= box_sizes.size();
+                failed = 0;
+            }
             detector.setThresholdParams(box_sizes[box_idx], 5);
             detector.setMinMaxSize(size_min, size_max);
             detector.detect(grayscale, markers, cv::Mat(), cv::Mat(), -1, false);
@@ -190,15 +209,6 @@ void Tracker::run()
                 cv::line(color, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), scale, 8);
         }
 
-        auto time = cv::getTickCount();
-
-        if ((long) (time / freq) != (long) (last_time / freq))
-        {
-            last_fps = cur_fps;
-            cur_fps = 0;
-            last_time = time;
-        }
-
         cur_fps++;
 
         char buf[128];