diff options
| -rw-r--r-- | ftnoir_tracker_pt/FTNoIR_PT_Controls.ui | 86 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 2 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp | 2 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h | 5 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/point_tracker.cpp | 54 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/point_tracker.h | 15 |
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 |