From 36d7e7106e858dbe234f109cb068b1e6f84c811b Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Tue, 2 Jun 2015 22:36:15 +0200
Subject: aruco: wait fraction of second between switching box sizes

---
 ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 32 ++++++++++++++++++---------
 1 file 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];
-- 
cgit v1.2.3