bring back dynamic pose resolution following user outrageopentrack-2.3-rc9

Uses a simpler method without computing point velocities.

Issues: #112, #126

6 files changed, 124 insertions, 40 deletions

diff --git a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
index 4342cfac..0e6048c3 100644
--- a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
+++ b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
@@ -83,21 +83,30 @@
             </property>
            </widget>
           </item>
-          <item row="0" column="1">
-           <widget class="QComboBox" name="camdevice_combo">
+          <item row="1" column="1">
+           <widget class="QSpinBox" name="res_x_spin">
             <property name="sizePolicy">
              <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
               <horstretch>0</horstretch>
               <verstretch>0</verstretch>
              </sizepolicy>
             </property>
-            <property name="minimumContentsLength">
+            <property name="toolTip">
+             <string>Desired capture width</string>
+            </property>
+            <property name="suffix">
+             <string> px</string>
+            </property>
+            <property name="maximum">
+             <number>2000</number>
+            </property>
+            <property name="singleStep">
              <number>10</number>
             </property>
            </widget>
           </item>
-          <item row="1" column="0">
-           <widget class="QLabel" name="label_36">
+          <item row="2" column="0">
+           <widget class="QLabel" name="label_41">
             <property name="sizePolicy">
              <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
               <horstretch>0</horstretch>
@@ -105,34 +114,38 @@
              </sizepolicy>
             </property>
             <property name="text">
-             <string>Width</string>
+             <string>Height</string>
             </property>
            </widget>
           </item>
-          <item row="1" column="1">
-           <widget class="QSpinBox" name="res_x_spin">
+          <item row="0" column="1">
+           <widget class="QComboBox" name="camdevice_combo">
             <property name="sizePolicy">
              <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
               <horstretch>0</horstretch>
               <verstretch>0</verstretch>
              </sizepolicy>
             </property>
-            <property name="toolTip">
-             <string>Desired capture width</string>
-            </property>
-            <property name="suffix">
-             <string> px</string>
+            <property name="minimumContentsLength">
+             <number>10</number>
             </property>
-            <property name="maximum">
-             <number>2000</number>
+           </widget>
+          </item>
+          <item row="1" column="0">
+           <widget class="QLabel" name="label_36">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
             </property>
-            <property name="singleStep">
-             <number>10</number>
+            <property name="text">
+             <string>Width</string>
             </property>
            </widget>
           </item>
-          <item row="2" column="0">
-           <widget class="QLabel" name="label_41">
+          <item row="4" column="0">
+           <widget class="QLabel" name="label_4">
             <property name="sizePolicy">
              <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
               <horstretch>0</horstretch>
@@ -140,7 +153,7 @@
              </sizepolicy>
             </property>
             <property name="text">
-             <string>Height</string>
+             <string>Field of view</string>
             </property>
            </widget>
           </item>
@@ -201,19 +214,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="4" column="1">
            <widget class="QSpinBox" name="fov">
             <property name="suffix">
@@ -230,6 +230,26 @@
             </property>
            </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="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>
          </layout>
         </widget>
        </item>
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index fc5e5b99..23cc72c7 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -77,7 +77,7 @@ void Tracker::run()
             std::vector<cv::Vec2f> points = point_extractor.extract_points(frame);
             
             if (points.size() == PointModel::N_POINTS)
-                point_tracker.track(points, PointModel(s), get_focal_length());
+                point_tracker.track(points, PointModel(s), get_focal_length(), s.dynamic_pose);
             
             {
                 Affine CM = pose();
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
index 2c1ec5d8..7be05bb7 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
@@ -69,6 +69,8 @@ TrackerDialog::TrackerDialog()
     tie_setting(s.fov, ui.fov);
     
     tie_setting(s.active_model_panel, ui.model_tabs);
+    
+    tie_setting(s.dynamic_pose, ui.dynamic_pose);
 
     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 bce89058..57752ed6 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
@@ -34,6 +34,8 @@ struct settings
     value<int> active_model_panel, cap_x, cap_y, cap_z;
     
     value<int> fov;
+    
+    value<bool> dynamic_pose;
 
     settings() :
         b(bundle("tracker-pt")),
@@ -62,7 +64,8 @@ struct settings
         cap_x(b, "cap-x", 0),
         cap_y(b, "cap-y", 0),
         cap_z(b, "cap-z", 0),
-        fov(b, "camera-fov", 56)
+        fov(b, "camera-fov", 56),
+        dynamic_pose(b, "dynamic-pose-resolution", false)
     {}
 };
 
diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp
index 0f996316..324119a9 100644
--- a/ftnoir_tracker_pt/point_tracker.cpp
+++ b/ftnoir_tracker_pt/point_tracker.cpp
@@ -67,9 +67,53 @@ PointTracker::PointTracker()
 	X_CM.t[2] = 1000;	// default position: 1 m away from cam;
 }
 
-void PointTracker::track(const vector<Vec2f>& points, const PointModel& model, float f)
+PointTracker::PointOrder PointTracker::find_correspondences_previous(const vector<Vec2f>& points, const PointModel& model, float f)
 {
-    const PointOrder& order = find_correspondences(points, model);
+    PointTracker::PointOrder p;
+    p.points[0] = project(Vec3f(0,0,0), f);
+    p.points[1] = project(model.M01, f);
+    p.points[2] = project(model.M02, f);
+
+    // set correspondences by minimum distance to projected model point
+    bool point_taken[PointModel::N_POINTS];
+    for (int i=0; i<PointModel::N_POINTS; ++i)
+            point_taken[i] = false;
+
+    for (int i=0; i<PointModel::N_POINTS; ++i)
+    {
+            float min_sdist = 0;
+            int min_idx = 0;
+            // find closest point to projected model point i
+            for (int j=0; j<PointModel::N_POINTS; ++j)
+            {
+                    Vec2f d = p.points[i]-points[j];
+                    float sdist = d.dot(d);
+                    if (sdist < min_sdist || j==0)
+                    {
+                            min_idx = j;
+                            min_sdist = sdist;
+                    }
+            }
+            // if one point is closest to more than one model point, fallback
+            if (point_taken[min_idx])
+            {
+                return find_correspondences(points, model);
+            }
+            point_taken[min_idx] = true;
+            p.points[i] = points[min_idx];
+    }
+    return p;
+}
+
+void PointTracker::track(const vector<Vec2f>& points, const PointModel& model, float f, bool dynamic_pose)
+{
+    PointOrder order;
+    
+    if (!dynamic_pose)
+       order = find_correspondences(points, model);
+    else
+        order = find_correspondences_previous(points, model, f);
+    
     POSIT(model, order, f);
 }
 
@@ -222,3 +266,9 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
 	//
 	//qDebug()<<"r: "<<r[0]<<' '<<r[1]<<' '<<r[2]<<'\n';
 }
+
+cv::Vec2f PointTracker::project(const cv::Vec3f& v_M, float f)
+{
+        cv::Vec3f v_C = X_CM * v_M;
+        return cv::Vec2f(f*v_C[0]/v_C[2], f*v_C[1]/v_C[2]);
+}
diff --git a/ftnoir_tracker_pt/point_tracker.h b/ftnoir_tracker_pt/point_tracker.h
index 61d2a826..10bd2cef 100644
--- a/ftnoir_tracker_pt/point_tracker.h
+++ b/ftnoir_tracker_pt/point_tracker.h
@@ -119,14 +119,23 @@ 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);
+    void track(const std::vector<cv::Vec2f>& projected_points, const PointModel& model, float f, bool dynamic_pose);
     Affine pose() const { return X_CM; }
-    
+    cv::Vec2f project(const cv::Vec3f& v_M, float f);
 private:
     // the points in model order
-    typedef struct { cv::Vec2f points[PointModel::N_POINTS]; } PointOrder;
+    struct PointOrder
+    {
+        cv::Vec2f points[PointModel::N_POINTS];
+        PointOrder()
+        {
+            for (int i = 0; i < PointModel::N_POINTS; i++)
+                points[i] = cv::Vec2f(0, 0);
+        }
+    };
 
     PointOrder find_correspondences(const std::vector<cv::Vec2f>& projected_points, const PointModel &model);
+    PointOrder find_correspondences_previous(const std::vector<cv::Vec2f>& points, const PointModel &model, float f);
     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