From 06a5c5d3e373b016d8a8917285c9b8a14e85f719 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Fri, 3 Jul 2015 10:20:18 +0200 Subject: pt: reintroduce dynamic pose timer --- ftnoir_tracker_pt/FTNoIR_PT_Controls.ui | 106 +++++++++++++++---------- ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 2 +- ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp | 1 + ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h | 4 +- ftnoir_tracker_pt/point_tracker.cpp | 23 ++++-- ftnoir_tracker_pt/point_tracker.h | 7 +- 6 files changed, 89 insertions(+), 54 deletions(-) (limited to 'ftnoir_tracker_pt') 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 @@ Camera settings + + + + + 0 + 0 + + + + Height + + + @@ -105,19 +118,6 @@ - - - - - 0 - 0 - - - - Height - - - @@ -144,19 +144,6 @@ - - - - - 0 - 0 - - - - Field of view - - - @@ -195,22 +182,36 @@ - - + + - + 0 0 - - Desired capture framerate + + Field of view - - Hz + + + + + + + 0 + 0 + - - 2000 + + Dynamic pose resolution + + + + + + + @@ -230,23 +231,42 @@ - - + + - + 0 0 - - Dynamic pose resolution + + Desired capture framerate + + + Hz + + + 2000 - - + + - + Dynamic pose timeout + + + + + + + ms + + + 1 + + + 10000 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 fov; value dynamic_pose; + value 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& 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& points, const PointModel& model, float f, bool dynamic_pose) +void PointTracker::track(const vector& 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& 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 #endif #include - +#include "opentrack/timer.hpp" #include "ftnoir_tracker_pt_settings.h" #include @@ -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& projected_points, const PointModel& model, float f, bool dynamic_pose); + void track(const std::vector& 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 -- cgit v1.2.3