pt: reintroduce dynamic pose timer

6 files changed, 89 insertions, 54 deletions

diff --git a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
index 39fb0686..17e2819b 100644
--- a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
+++ b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
@@ -67,6 +67,19 @@
           <string>Camera settings</string>
          </property>
          <layout class="QGridLayout" name="gridLayout_2">
+          <item row="2" column="0">
+           <widget class="QLabel" name="label_41">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="text">
+             <string>Height</string>
+            </property>
+           </widget>
+          </item>
           <item row="0" column="0">
            <widget class="QLabel" name="label_2">
             <property name="sizePolicy">
@@ -105,19 +118,6 @@
             </property>
            </widget>
           </item>
-          <item row="2" column="0">
-           <widget class="QLabel" name="label_41">
-            <property name="sizePolicy">
-             <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
-              <horstretch>0</horstretch>
-              <verstretch>0</verstretch>
-             </sizepolicy>
-            </property>
-            <property name="text">
-             <string>Height</string>
-            </property>
-           </widget>
-          </item>
           <item row="0" column="1">
            <widget class="QComboBox" name="camdevice_combo">
             <property name="sizePolicy">
@@ -144,19 +144,6 @@
             </property>
            </widget>
           </item>
-          <item row="4" column="0">
-           <widget class="QLabel" name="label_4">
-            <property name="sizePolicy">
-             <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
-              <horstretch>0</horstretch>
-              <verstretch>0</verstretch>
-             </sizepolicy>
-            </property>
-            <property name="text">
-             <string>Field of view</string>
-            </property>
-           </widget>
-          </item>
           <item row="2" column="1">
            <widget class="QSpinBox" name="res_y_spin">
             <property name="sizePolicy">
@@ -195,22 +182,36 @@
             </property>
            </widget>
           </item>
-          <item row="3" column="1">
-           <widget class="QSpinBox" name="fps_spin">
+          <item row="4" column="0">
+           <widget class="QLabel" name="label_4">
             <property name="sizePolicy">
-             <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+             <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
               <horstretch>0</horstretch>
               <verstretch>0</verstretch>
              </sizepolicy>
             </property>
-            <property name="toolTip">
-             <string>Desired capture framerate</string>
+            <property name="text">
+             <string>Field of view</string>
             </property>
-            <property name="suffix">
-             <string> Hz</string>
+           </widget>
+          </item>
+          <item row="5" column="0">
+           <widget class="QLabel" name="label_5">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
             </property>
-            <property name="maximum">
-             <number>2000</number>
+            <property name="text">
+             <string>Dynamic pose resolution</string>
+            </property>
+           </widget>
+          </item>
+          <item row="5" column="1">
+           <widget class="QCheckBox" name="dynamic_pose">
+            <property name="text">
+             <string/>
             </property>
            </widget>
           </item>
@@ -230,23 +231,42 @@
             </property>
            </widget>
           </item>
-          <item row="5" column="0">
-           <widget class="QLabel" name="label_5">
+          <item row="3" column="1">
+           <widget class="QSpinBox" name="fps_spin">
             <property name="sizePolicy">
-             <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
+             <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
               <horstretch>0</horstretch>
               <verstretch>0</verstretch>
              </sizepolicy>
             </property>
-            <property name="text">
-             <string>Dynamic pose resolution</string>
+            <property name="toolTip">
+             <string>Desired capture framerate</string>
+            </property>
+            <property name="suffix">
+             <string> Hz</string>
+            </property>
+            <property name="maximum">
+             <number>2000</number>
             </property>
            </widget>
           </item>
-          <item row="5" column="1">
-           <widget class="QCheckBox" name="dynamic_pose">
+          <item row="6" column="0">
+           <widget class="QLabel" name="label_6">
             <property name="text">
-             <string/>
+             <string>Dynamic pose timeout</string>
+            </property>
+           </widget>
+          </item>
+          <item row="6" column="1">
+           <widget class="QSpinBox" name="init_phase_timeout">
+            <property name="suffix">
+             <string> ms</string>
+            </property>
+            <property name="minimum">
+             <number>1</number>
+            </property>
+            <property name="maximum">
+             <number>10000</number>
             </property>
            </widget>
           </item>
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index 2d113ac5..e96e7171 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -86,7 +86,7 @@ void Tracker_PT::run()
             ever_success |= success;
             
             if (success)
-                point_tracker.track(points, PointModel(s), get_focal_length(), s.dynamic_pose);
+                point_tracker.track(points, PointModel(s), get_focal_length(), s.dynamic_pose, s.init_phase_timeout);
             
             {
                 Affine X_CM = pose();
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
index e7f1384d..696f0b1b 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
@@ -75,6 +75,7 @@ TrackerDialog_PT::TrackerDialog_PT()
     tie_setting(s.active_model_panel, ui.model_tabs);
     
     tie_setting(s.dynamic_pose, ui.dynamic_pose);
+	tie_setting(s.init_phase_timeout, ui.init_phase_timeout);
 
     connect( ui.tcalib_button,SIGNAL(toggled(bool)), this,SLOT(startstop_trans_calib(bool)) );
 
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
index 4e241bc3..4d54eb68 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
@@ -32,6 +32,7 @@ struct settings_pt : opts
     value<int> fov;
     
     value<bool> dynamic_pose;
+    value<int> init_phase_timeout;
 
     settings_pt() :
         opts("tracker-pt"),
@@ -60,7 +61,8 @@ struct settings_pt : opts
         cap_y(b, "cap-y", 0),
         cap_z(b, "cap-z", 0),
         fov(b, "camera-fov", 56),
-        dynamic_pose(b, "dynamic-pose-resolution", true)
+        dynamic_pose(b, "dynamic-pose-resolution", true),
+        init_phase_timeout(b, "init-phase-timeout", 500)
     {}
 };
 
diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp
index e4c999ad..bae89dbe 100644
--- a/ftnoir_tracker_pt/point_tracker.cpp
+++ b/ftnoir_tracker_pt/point_tracker.cpp
@@ -62,7 +62,7 @@ void PointModel::get_d_order(const std::vector<cv::Vec2f>& points, int d_order[]
 }
 
 
-PointTracker::PointTracker()
+PointTracker::PointTracker() : init_phase(true)
 {
 }
 
@@ -96,6 +96,7 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const vecto
             // if one point is closest to more than one model point, fallback
             if (point_taken[min_idx])
             {
+                init_phase = true;
                 return find_correspondences(points, model);
             }
             point_taken[min_idx] = true;
@@ -104,16 +105,24 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const vecto
     return p;
 }
 
-void PointTracker::track(const vector<Vec2f>& points, const PointModel& model, float f, bool dynamic_pose)
+void PointTracker::track(const vector<Vec2f>& points, const PointModel& model, float f, bool dynamic_pose, int init_phase_timeout)
 {
     PointOrder order;
-    
-    if (!dynamic_pose)
-       order = find_correspondences(points, model);
-    else
-        order = find_correspondences_previous(points, model, f);
+
+	if (t.elapsed_ms() > init_phase_timeout)
+	{
+		t.start();
+		init_phase = true;
+	}
+
+    if (!dynamic_pose || init_phase)
+        order = find_correspondences(points, model);
+	else
+		order = find_correspondences_previous(points, model, f);
     
     POSIT(model, order, f);
+	init_phase = false;
+    t.start();
 }
 
 PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv::Vec2f>& points, const PointModel& model)
diff --git a/ftnoir_tracker_pt/point_tracker.h b/ftnoir_tracker_pt/point_tracker.h
index 3b9573ff..935e2eaa 100644
--- a/ftnoir_tracker_pt/point_tracker.h
+++ b/ftnoir_tracker_pt/point_tracker.h
@@ -15,7 +15,7 @@
 #   include <memory>
 #endif
 #include <vector>
-
+#include "opentrack/timer.hpp"
 #include "ftnoir_tracker_pt_settings.h"
 
 #include <QObject>
@@ -119,7 +119,7 @@ public:
     // track the pose using the set of normalized point coordinates (x pos in range -0.5:0.5)
     // f : (focal length)/(sensor width)
     // dt : time since last call
-    void track(const std::vector<cv::Vec2f>& projected_points, const PointModel& model, float f, bool dynamic_pose);
+    void track(const std::vector<cv::Vec2f>& projected_points, const PointModel& model, float f, bool dynamic_pose, int init_phase_timeout);
     Affine pose() const { return X_CM; }
     cv::Vec2f project(const cv::Vec3f& v_M, float f);
 private:
@@ -139,6 +139,9 @@ private:
     int POSIT(const PointModel& point_model, const PointOrder& order, float focal_length);  // The POSIT algorithm, returns the number of iterations
 
     Affine X_CM; // trafo from model to camera
+
+	Timer t;
+	bool init_phase;
 };
 
 #endif //POINTTRACKER_H