tracker/aruco: cycle threshold params if detection fails

3 files changed, 149 insertions, 58 deletions

diff --git a/tracker-aruco/aruco-trackercontrols.ui b/tracker-aruco/aruco-trackercontrols.ui
index 8e6b30a6..0d70d9ed 100644
--- a/tracker-aruco/aruco-trackercontrols.ui
+++ b/tracker-aruco/aruco-trackercontrols.ui
@@ -10,7 +10,7 @@
     <x>0</x>
     <y>0</y>
     <width>453</width>
-    <height>218</height>
+    <height>246</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -81,6 +81,31 @@
            </property>
           </widget>
          </item>
+         <item row="3" column="1">
+          <widget class="QComboBox" name="resolution">
+           <property name="sizePolicy">
+            <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+             <horstretch>0</horstretch>
+             <verstretch>0</verstretch>
+            </sizepolicy>
+           </property>
+           <item>
+            <property name="text">
+             <string>640x480</string>
+            </property>
+           </item>
+           <item>
+            <property name="text">
+             <string>320x240</string>
+            </property>
+           </item>
+           <item>
+            <property name="text">
+             <string>Default (not recommended!)</string>
+            </property>
+           </item>
+          </widget>
+         </item>
          <item row="1" column="1">
           <widget class="QComboBox" name="cameraFPS">
            <property name="sizePolicy">
@@ -140,32 +165,28 @@
            </property>
           </widget>
          </item>
-         <item row="3" column="1">
-          <widget class="QComboBox" name="resolution">
-           <property name="sizePolicy">
-            <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
-             <horstretch>0</horstretch>
-             <verstretch>0</verstretch>
-            </sizepolicy>
+         <item row="2" column="0">
+          <widget class="QLabel" name="label_10">
+           <property name="text">
+            <string>Camera name</string>
            </property>
-           <item>
-            <property name="text">
-             <string>640x480</string>
-            </property>
-           </item>
-           <item>
-            <property name="text">
-             <string>320x240</string>
-            </property>
-           </item>
-           <item>
-            <property name="text">
-             <string>Default (not recommended!)</string>
-            </property>
-           </item>
           </widget>
          </item>
-         <item row="4" column="1">
+         <item row="1" column="0">
+          <widget class="QLabel" name="label_12">
+           <property name="text">
+            <string>Frames per second</string>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="0">
+          <widget class="QLabel" name="label_11">
+           <property name="text">
+            <string>Resolution</string>
+           </property>
+          </widget>
+         </item>
+         <item row="5" column="1">
           <widget class="QPushButton" name="camera_settings">
            <property name="sizePolicy">
             <sizepolicy hsizetype="Preferred" vsizetype="Maximum">
@@ -178,24 +199,20 @@
            </property>
           </widget>
          </item>
-         <item row="1" column="0">
-          <widget class="QLabel" name="label_12">
+         <item row="4" column="0">
+          <widget class="QLabel" name="label">
            <property name="text">
-            <string>Frames per second</string>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="0">
-          <widget class="QLabel" name="label_10">
-           <property name="text">
-            <string>Camera name</string>
+            <string>Model rotation</string>
            </property>
           </widget>
          </item>
-         <item row="3" column="0">
-          <widget class="QLabel" name="label_11">
-           <property name="text">
-            <string>Resolution</string>
+         <item row="4" column="1">
+          <widget class="QComboBox" name="model_rotation">
+           <property name="sizePolicy">
+            <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+             <horstretch>0</horstretch>
+             <verstretch>0</verstretch>
+            </sizepolicy>
            </property>
           </widget>
          </item>
diff --git a/tracker-aruco/ftnoir_tracker_aruco.cpp b/tracker-aruco/ftnoir_tracker_aruco.cpp
index 15955692..53907808 100644
--- a/tracker-aruco/ftnoir_tracker_aruco.cpp
+++ b/tracker-aruco/ftnoir_tracker_aruco.cpp
@@ -24,36 +24,31 @@
 #include <algorithm>
 #include <iterator>
 
-struct resolution_tuple
-{
-    int width;
-    int height;
-};
+constexpr double aruco_tracker::timeout;
+constexpr double aruco_tracker::timeout_backoff_c;
+constexpr const int aruco_tracker::adaptive_sizes[];
 
-static constexpr const resolution_tuple resolution_choices[] =
-{
-    { 640, 480 },
-    { 320, 240 },
-    { 0, 0 }
-};
+constexpr const aruco_tracker::resolution_tuple aruco_tracker::resolution_choices[];
 
 constexpr const double aruco_tracker::RC;
 constexpr const float aruco_tracker::size_min;
 constexpr const float aruco_tracker::size_max;
 
 aruco_tracker::aruco_tracker() :
+    pose{0,0,0, 0,0,0},
     fps(0),
+    no_detection_timeout(0),
     obj_points(4),
     intrinsics(cv::Matx33d::eye()),
     rmat(cv::Matx33d::eye()),
     roi_points(4),
     last_roi(65535, 65535, 0, 0),
-    stop(false)
+    adaptive_size_pos(0),
+    stop(false),
+    use_otsu(false)
 {
     // param 2 ignored for Otsu thresholding. it's required to use our fork of Aruco.
-    detector.setThresholdParams(7, -1);
-    detector.setDesiredSpeed(3);
-    detector._thresMethod = aruco::MarkerDetector::FIXED_THRES;
+    set_detector_params();
 }
 
 aruco_tracker::~aruco_tracker()
@@ -75,8 +70,6 @@ void aruco_tracker::start_tracker(QFrame* videoframe)
     videoframe->setLayout(layout.data());
     videoWidget->show();
     start();
-    for (int i = 0; i < 6; i++)
-        pose[i] = 0;
 }
 
 void aruco_tracker::getRT(cv::Matx33d& r_, cv::Vec3d& t_)
@@ -323,6 +316,35 @@ void aruco_tracker::set_roi_from_projection()
     clamp_last_roi();
 }
 
+void aruco_tracker::set_detector_params()
+{
+    detector.setDesiredSpeed(3);
+    detector.setThresholdParams(adaptive_sizes[adaptive_size_pos], adaptive_thres);
+    if (use_otsu)
+        detector._thresMethod = aruco::MarkerDetector::FIXED_THRES;
+    else
+        detector._thresMethod = aruco::MarkerDetector::ADPT_THRES;
+}
+
+void aruco_tracker::cycle_detection_params()
+{
+    if (!use_otsu)
+        use_otsu = true;
+    else
+    {
+        use_otsu = false;
+
+        adaptive_size_pos++;
+        adaptive_size_pos %= sizeof(adaptive_sizes)/sizeof(*adaptive_sizes);
+    }
+
+    set_detector_params();
+
+    qDebug() << "aruco: switched thresholding params"
+             << "otsu:" << use_otsu
+             << "size:" << adaptive_sizes[adaptive_size_pos];
+}
+
 void aruco_tracker::run()
 {
     cv::setNumThreads(0);
@@ -336,6 +358,7 @@ void aruco_tracker::run()
         return;
 
     fps_timer.start();
+    last_detection_timer.start();
 
     while (!stop)
     {
@@ -365,15 +388,33 @@ void aruco_tracker::run()
             if (!cv::solvePnP(obj_points, markers[0], intrinsics, cv::noArray(), rvec, tvec, false, cv::SOLVEPNP_ITERATIVE))
                 goto fail;
 
+            {
+                const double dt = last_detection_timer.elapsed_seconds();
+                last_detection_timer.start();
+                no_detection_timeout -= dt * timeout_backoff_c;
+                no_detection_timeout = std::max(0., no_detection_timeout);
+            }
+
             set_last_roi();
             draw_centroid();
             set_rmat();
         }
         else
+        {
 fail:
             // no marker found, reset search region
             last_roi = cv::Rect(65535, 65535, 0, 0);
 
+            const double dt = last_detection_timer.elapsed_seconds();
+            last_detection_timer.start();
+            no_detection_timeout += dt;
+            if (no_detection_timeout > timeout)
+            {
+                no_detection_timeout = 0;
+                cycle_detection_params();
+            }
+        }
+
         draw_ar(ok);
 
         if (frame.rows > 0)
diff --git a/tracker-aruco/ftnoir_tracker_aruco.h b/tracker-aruco/ftnoir_tracker_aruco.h
index aa3c59f7..6e7db3f6 100644
--- a/tracker-aruco/ftnoir_tracker_aruco.h
+++ b/tracker-aruco/ftnoir_tracker_aruco.h
@@ -28,6 +28,7 @@
 #include <opencv2/core.hpp>
 #include <opencv2/videoio.hpp>
 
+
 using namespace options;
 
 struct settings : opts {
@@ -83,6 +84,10 @@ private:
     void set_last_roi();
     void set_rmat();
     void set_roi_from_projection();
+    void set_detector_params();
+    void cycle_detection_params();
+
+    cv::Point3f rotate_model(float x, float y, settings::rot mode);
 
     cv::VideoCapture camera;
     QMutex camera_mtx;
@@ -90,7 +95,7 @@ private:
     qshared<cv_video_widget> videoWidget;
     qshared<QHBoxLayout> layout;
     settings s;
-    double pose[6], fps;
+    double pose[6], fps, no_detection_timeout;
     cv::Mat frame, grayscale, color;
     cv::Matx33d r;
     std::vector<cv::Point3f> obj_points;
@@ -105,9 +110,37 @@ private:
     cv::Vec3d euler;
     std::vector<cv::Point3f> roi_points;
     cv::Rect last_roi;
-    Timer fps_timer;
-
+    Timer fps_timer, last_detection_timer;
+    unsigned adaptive_size_pos;
     volatile bool stop;
+    bool use_otsu;
+
+    struct resolution_tuple
+    {
+        int width;
+        int height;
+    };
+
+    static constexpr const int adaptive_sizes[] =
+    {
+        7,
+        9,
+        //11,
+        13,
+        //5,
+    };
+
+    static constexpr int adaptive_thres = 6;
+
+    static constexpr const resolution_tuple resolution_choices[] =
+    {
+        { 640, 480 },
+        { 320, 240 },
+        { 0, 0 }
+    };
+
+    static constexpr double timeout = 1;
+    static constexpr double timeout_backoff_c = 4./11;
 
     static constexpr const float size_min = 0.05;
     static constexpr const float size_max = 0.3;