Merge branch 'unstable' into trackhat-ui

4 files changed, 29 insertions, 11 deletions

diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index bad75238..2f852343 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -37,6 +37,7 @@ Tracker_PT::~Tracker_PT()
     delete video_widget;
     video_widget = NULL;
     if (video_frame->layout()) delete video_frame->layout();
+    camera.stop();
 }
 
 void Tracker_PT::set_command(Command command)
@@ -98,7 +99,7 @@ void Tracker_PT::run()
             ever_success |= success;
             
             if (success)
-                point_tracker.track(points, PointModel(s), get_focal_length(), true);
+                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_settings.h b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
index 4c6bab48..00346267 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
@@ -21,6 +21,9 @@ struct settings_pt : opts
     value<int> fov, camera_mode;
     value<int> model_used;
 
+    value<bool> dynamic_pose;
+    value<int> init_phase_timeout;
+
     settings_pt() :
         opts("tracker-pt"),
         threshold(b, "threshold-primary", 128),
@@ -31,7 +34,9 @@ struct settings_pt : opts
         t_MH_z(b, "model-centroid-z", 0),
         fov(b, "camera-fov", 0),
         camera_mode(b, "camera-mode", 0),
-        model_used(b, "model-used", 0)
+        model_used(b, "model-used", 0),
+        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 344a05c0..1340a11d 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>
@@ -116,7 +116,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:
@@ -136,6 +136,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