From 253c206c171b5f064addccda266a1998039409ad Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 14 Dec 2014 00:19:17 +0100 Subject: pt: set correct focal length from camera fov Issue: #96 Having user-supplied camera fov, we can prevent yaw and pitch occuring by itself when moving horizontally and vertically. Note PointExtractor::extract_points(Mat& frame) should enable same value for fx and fy: c[0] = (mx/m - W/2)/W; c[1] = -(my/m - H/2)/W; --- ftnoir_tracker_pt/FTNoIR_PT_Controls.ui | 31 +++++++++++++++++++++++++- ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 9 +++++++- ftnoir_tracker_pt/ftnoir_tracker_pt.h | 3 +++ ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp | 2 ++ ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h | 5 ++++- ftnoir_tracker_pt/point_tracker.cpp | 7 +++--- ftnoir_tracker_pt/point_tracker.h | 6 ++--- 7 files changed, 53 insertions(+), 10 deletions(-) diff --git a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui index 63ed7f86..109e50cb 100644 --- a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui +++ b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui @@ -9,7 +9,7 @@ 0 0 - 453 + 460 621 @@ -273,6 +273,35 @@ + + + + + 0 + 0 + + + + Field of view + + + + + + + ° + + + + + + 10 + + + 90 + + + diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp index 05e7b067..3abaa35e 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp @@ -48,6 +48,13 @@ void Tracker::reset_command(Command command) commands &= ~command; } +float Tracker::get_focal_length() +{ + static constexpr float pi = 3.1415926f; + const float fov = static_cast(s.fov) * pi / 180.f; + return 0.5f / tan(0.5f * fov); +} + void Tracker::run() { #ifdef PT_PERF_LOG @@ -85,7 +92,7 @@ void Tracker::run() 4); } if (points.size() == PointModel::N_POINTS) - point_tracker.track(points, model); + point_tracker.track(points, model, get_focal_length()); video_widget->update_image(frame); } #ifdef PT_PERF_LOG diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.h b/ftnoir_tracker_pt/ftnoir_tracker_pt.h index 2f922e17..3dd15618 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.h +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.h @@ -57,6 +57,9 @@ private: }; void set_command(Command command); void reset_command(Command command); + + float get_focal_length(); + volatile int commands; CVCamera camera; diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp index 2bf8ba75..8f6edc2f 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp @@ -65,6 +65,8 @@ TrackerDialog::TrackerDialog() tie_setting(s.t_MH_x, ui.tx_spin); tie_setting(s.t_MH_y, ui.ty_spin); tie_setting(s.t_MH_z, ui.tz_spin); + + tie_setting(s.fov, ui.fov); 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 c95dc3e5..804de22e 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h @@ -33,6 +33,8 @@ struct settings value clip_ty, clip_tz, clip_by, clip_bz; value active_model_panel, cap_x, cap_y, cap_z; + + value fov; settings() : b(bundle("tracker-pt")), @@ -60,7 +62,8 @@ struct settings active_model_panel(b, "active-model-panel", 0), cap_x(b, "cap-x", 0), cap_y(b, "cap-y", 0), - cap_z(b, "cap-z", 0) + cap_z(b, "cap-z", 0), + fov(b, "camera-fov", 56) {} }; diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp index 3b1a49e2..9f04af30 100644 --- a/ftnoir_tracker_pt/point_tracker.cpp +++ b/ftnoir_tracker_pt/point_tracker.cpp @@ -83,16 +83,15 @@ void PointModel::get_d_order(const std::vector& points, int d_order[], vec } -// ---------------------------------------------------------------------------- PointTracker::PointTracker() { X_CM.t[2] = 1000; // default position: 1 m away from cam; } -void PointTracker::track(const vector& points, const PointModel& model) +void PointTracker::track(const vector& points, const PointModel& model, float f) { const PointOrder& order = find_correspondences(points, model); - POSIT(model, order); + POSIT(model, order, f); } PointTracker::PointOrder PointTracker::find_correspondences(const std::vector& points, const PointModel& model) @@ -117,7 +116,7 @@ PointTracker::PointOrder PointTracker::find_correspondences(const std::vector& projected_points, const PointModel& model); + void track(const std::vector& projected_points, const PointModel& model, float f); Affine pose() const { return X_CM; } + private: // the points in model order typedef struct { cv::Vec2f points[PointModel::N_POINTS]; } PointOrder; - static constexpr float focal_length = 1.0f; PointOrder find_correspondences(const std::vector& projected_points, const PointModel &model); - int POSIT(const PointModel& point_model, const PointOrder& order); // The POSIT algorithm, returns the number of iterations + 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 }; -- cgit v1.2.3