aruco: fix fixating on the prev marker location

3 files changed, 78 insertions, 124 deletions

diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
index 1898d15b..099dec02 100644
--- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui
+++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
@@ -10,7 +10,7 @@
     <x>0</x>
     <y>0</y>
     <width>562</width>
-    <height>214</height>
+    <height>178</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -33,6 +33,42 @@
    <item row="0" column="0">
     <widget class="QFrame" name="frame">
      <layout class="QGridLayout" name="gridLayout_3">
+      <item row="0" column="1">
+       <widget class="QDoubleSpinBox" name="cameraFOV">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
+        <property name="locale">
+         <locale language="English" country="UnitedStates"/>
+        </property>
+        <property name="minimum">
+         <double>35.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>180.000000000000000</double>
+        </property>
+        <property name="value">
+         <double>52.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="0">
+       <widget class="QLabel" name="label_2">
+        <property name="text">
+         <string>Frames per second</string>
+        </property>
+       </widget>
+      </item>
+      <item row="3" column="0">
+       <widget class="QLabel" name="label_4">
+        <property name="text">
+         <string>Resolution</string>
+        </property>
+       </widget>
+      </item>
       <item row="0" column="0">
        <widget class="QLabel" name="label">
         <property name="text">
@@ -75,59 +111,6 @@
         </item>
        </widget>
       </item>
-      <item row="3" column="0">
-       <widget class="QLabel" name="label_4">
-        <property name="text">
-         <string>Resolution</string>
-        </property>
-       </widget>
-      </item>
-      <item row="0" column="1">
-       <widget class="QDoubleSpinBox" name="cameraFOV">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="locale">
-         <locale language="English" country="UnitedStates"/>
-        </property>
-        <property name="minimum">
-         <double>35.000000000000000</double>
-        </property>
-        <property name="maximum">
-         <double>180.000000000000000</double>
-        </property>
-        <property name="value">
-         <double>52.000000000000000</double>
-        </property>
-       </widget>
-      </item>
-      <item row="1" column="0">
-       <widget class="QLabel" name="label_2">
-        <property name="text">
-         <string>Frames per second</string>
-        </property>
-       </widget>
-      </item>
-      <item row="2" column="1">
-       <widget class="QComboBox" name="cameraName">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-       </widget>
-      </item>
-      <item row="2" column="0">
-       <widget class="QLabel" name="label_3">
-        <property name="text">
-         <string>Camera name</string>
-        </property>
-       </widget>
-      </item>
       <item row="3" column="1">
        <widget class="QComboBox" name="resolution">
         <property name="sizePolicy">
@@ -158,26 +141,20 @@
         </item>
        </widget>
       </item>
-      <item row="4" column="0">
-       <widget class="QLabel" name="label_6">
+      <item row="2" column="0">
+       <widget class="QLabel" name="label_3">
         <property name="text">
-         <string>Sun glare removal</string>
+         <string>Camera name</string>
         </property>
        </widget>
       </item>
-      <item row="4" column="1">
-       <widget class="QSlider" name="desaturate_slider">
-        <property name="maximum">
-         <number>100</number>
-        </property>
-        <property name="singleStep">
-         <number>10</number>
-        </property>
-        <property name="pageStep">
-         <number>20</number>
-        </property>
-        <property name="orientation">
-         <enum>Qt::Horizontal</enum>
+      <item row="2" column="1">
+       <widget class="QComboBox" name="cameraName">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
         </property>
        </widget>
       </item>
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index 0974f0f0..fd8a8ce1 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -194,7 +194,6 @@ void Tracker::run()
     auto last_time = cv::getTickCount();
     int cur_fps = 0;
     int last_fps = 0;
-    cv::Point2f last_centroid;
 
     while (!stop)
     {
@@ -203,42 +202,9 @@ void Tracker::run()
             continue;
         auto tm = cv::getTickCount();
         
-        const double c = s.desaturate * 16. / 100.;
-        
-        if (std::abs(c) > 1e-3)
-        {
-            const int w=color.cols, h=color.rows;
-            
-            cv::Mat hsv;
-            cv::cvtColor(color, hsv, cv::COLOR_BGR2HSV);
-            vector<cv::Mat> channels;
-            cv::split(hsv, channels);
-            cv::Mat sat = channels[1];
-            cv::Mat val = channels[2];
-            
-            struct ops {
-                static double sig(double x)
-                {
-                    double x_ = -6 + x * 2 * 6;
-                    return 1./(1.+exp(-x_));
-                }
-            };
-            
-            for (int i = 0; i < h; i++)
-                for (int j = 0; j < w; j++)
-                {
-                    const double sat_ij = sat.at<unsigned char>(i, j)/255.;
-                    val.at<unsigned char>(i, j) *= std::max(0., 1. - c*ops::sig(sat_ij));
-                }
-            
-            channels[1] = sat;
-            channels[2] = val;
-            cv::merge(channels, hsv);
-            cv::cvtColor(hsv, color, cv::COLOR_HSV2BGR);
-        }
         cv::Mat grayscale;
         cv::cvtColor(color, grayscale, cv::COLOR_BGR2GRAY);
-
+        
         const int scale = frame.cols > 480 ? 2 : 1;
         detector.setThresholdParams(scale > 1 ? 11 : 7, 4);
 
@@ -333,9 +299,35 @@ void Tracker::run()
             
             cv::Vec3d rvec, tvec;
             
-            cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, cv::ITERATIVE);
+            cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, cv::P3P);
             
             std::vector<cv::Point2f> roi_projection(4);
+            
+            {
+                std::vector<cv::Point2f> repr2;
+                std::vector<cv::Point3f> centroid;
+                centroid.push_back(cv::Point3f(0, 0, 0));
+                cv::projectPoints(centroid, rvec, tvec, intrinsics, dist_coeffs, repr2);
+    
+                {
+                    auto s = cv::Scalar(255, 0, 255);
+                    cv::circle(frame, repr2.at(0), 4, s, -1);
+                }
+            }
+            
+            for (int i = 0; i < 4; i++)
+            {
+                obj_points.at<float>(i, 0) -= s.headpos_x;
+                obj_points.at<float>(i, 1) -= s.headpos_y;
+                obj_points.at<float>(i, 2) -= s.headpos_z;
+            }
+            
+            {
+                cv::Mat rvec_, tvec_;
+                cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec_, tvec_, false, cv::P3P);
+                tvec = tvec_;
+            }
+            
             cv::Mat roi_points = obj_points * c_search_window;
             cv::projectPoints(roi_points, rvec, tvec, intrinsics, dist_coeffs, roi_projection);
             
@@ -389,21 +381,9 @@ void Tracker::run()
                 r = rmat;
                 t = tvec;
             }
-
-            std::vector<cv::Point2f> repr2;
-            std::vector<cv::Point3f> centroid;
-            centroid.push_back(cv::Point3f(0, 0, 0));
-            cv::projectPoints(centroid, rvec, tvec, intrinsics, dist_coeffs, repr2);
-
-            {
-                auto s = cv::Scalar(255, 0, 255);
-                cv::circle(frame, repr2.at(0), 4, s, -1);
-            }
             
             if (roi_valid)
                 cv::rectangle(frame, last_roi, cv::Scalar(255, 0, 255), 1);
-
-            last_centroid = repr2[0];
         }
         else
             last_roi = cv::Rect(65535, 65535, 0, 0);
@@ -490,7 +470,6 @@ TrackerControls::TrackerControls()
     tie_setting(s.headpos_x, ui.cx);
     tie_setting(s.headpos_y, ui.cy);
     tie_setting(s.headpos_z, ui.cz);
-    tie_setting(s.desaturate, ui.desaturate_slider);
     connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
     connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
     connect(ui.btn_calibrate, SIGNAL(clicked()), this, SLOT(toggleCalibrate()));
@@ -540,6 +519,6 @@ void TrackerControls::doOK()
 
 void TrackerControls::doCancel()
 {
-    s.b->revert();
+    s.b->reload();
     this->close();
 }
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
index 03fff844..3d37dacd 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
@@ -28,7 +28,6 @@ struct settings {
     pbundle b;
     value<double> fov, headpos_x, headpos_y, headpos_z;
     value<int> camera_index, force_fps, resolution;
-    value<int> desaturate;
     settings() :
         b(bundle("aruco-tracker")),
         fov(b, "field-of-view", 56),
@@ -37,15 +36,14 @@ struct settings {
         headpos_z(b, "headpos-z", 0),
         camera_index(b, "camera-index", 0),
         force_fps(b, "force-fps", 0),
-        resolution(b, "force-resolution", 0),
-        desaturate(b, "desaturate", 0)
+        resolution(b, "force-resolution", 0)
     {}
 };
 
 class Tracker : protected QThread, public ITracker
 {
     Q_OBJECT
-    static constexpr double c_search_window = 2.9;
+    static constexpr double c_search_window = 2.2;
 public:
     Tracker();
     ~Tracker() override;