From 267010ba42b00cfd1ecc73c86d99c647ff191175 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Wed, 8 Jan 2014 19:40:06 +0100 Subject: use levmarq instead of coplanar POSIT implemented in numerically unstable fashion Signed-off-by: Stanislaw Halik --- FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui | 13 +- FTNoIR_Tracker_PT/camera.cpp | 8 +- FTNoIR_Tracker_PT/camera.h | 6 +- FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp | 4 +- FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp | 2 +- FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h | 4 +- FTNoIR_Tracker_PT/point_tracker.cpp | 191 ++++++------------------- FTNoIR_Tracker_PT/point_tracker.h | 33 ++++- 8 files changed, 92 insertions(+), 169 deletions(-) diff --git a/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui b/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui index bdbed955..461253cf 100644 --- a/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui +++ b/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui @@ -515,23 +515,26 @@ - F/W + FOV - f_dspin + fov_dspin - + The camera's focal length devided by its sensor width 2 + + 1.000000000000000 + - 0.100000000000000 + 1.000000000000000 @@ -1733,7 +1736,7 @@ res_x_spin res_y_spin fps_spin - f_dspin + fov_dspin camroll_combo campitch_spin camyaw_spin diff --git a/FTNoIR_Tracker_PT/camera.cpp b/FTNoIR_Tracker_PT/camera.cpp index 754533c5..8986be60 100644 --- a/FTNoIR_Tracker_PT/camera.cpp +++ b/FTNoIR_Tracker_PT/camera.cpp @@ -108,11 +108,11 @@ void Camera::set_device_index(int index) } } -void Camera::set_f(float f) +void Camera::set_fov(float f) { - if (cam_desired.f != f) + if (cam_desired.fov != f) { - cam_desired.f = f; + cam_desired.fov = f; _set_f(); } } @@ -208,7 +208,7 @@ void CVCamera::_set_index() void CVCamera::_set_f() { - cam_info.f = cam_desired.f; + cam_info.fov = cam_desired.fov; } void CVCamera::_set_fps() diff --git a/FTNoIR_Tracker_PT/camera.h b/FTNoIR_Tracker_PT/camera.h index ea68c387..6768e419 100644 --- a/FTNoIR_Tracker_PT/camera.h +++ b/FTNoIR_Tracker_PT/camera.h @@ -25,12 +25,12 @@ void get_camera_device_names(std::vector& device_names); // ---------------------------------------------------------------------------- struct CamInfo { - CamInfo() : res_x(0), res_y(0), fps(0), f(1) {} + CamInfo() : res_x(0), res_y(0), fps(0), fov(56) {} int res_x; int res_y; int fps; - float f; // (focal length) / (sensor width) + float fov; }; // ---------------------------------------------------------------------------- @@ -48,7 +48,7 @@ public: // calls corresponding template methods and reinitializes frame rate calculation void set_device_index(int index); - void set_f(float f); + void set_fov(float f); void set_fps(int fps); void set_res(int x_res, int y_res); diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp index 6bcad861..a3e8919b 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp @@ -84,7 +84,7 @@ void Tracker::run() { frame = frame_rotation.rotate_frame(frame); const std::vector& points = point_extractor.extract_points(frame, dt, false); - tracking_valid = point_tracker.track(points, camera.get_info().f, dt); + tracking_valid = point_tracker.track(points, camera.get_info().fov, dt, frame.cols, frame.rows, t_MH); video_widget->update_image(frame); } #ifdef PT_PERF_LOG @@ -104,7 +104,7 @@ void Tracker::apply(settings& s) camera.set_device_index(s.cam_index); camera.set_res(s.cam_res_x, s.cam_res_y); camera.set_fps(s.cam_fps); - camera.set_f(s.cam_f); + camera.set_fov(s.cam_fov); frame_rotation.rotation = static_cast(static_cast(s.cam_roll)); point_extractor.threshold_val = s.threshold; point_extractor.threshold_secondary_val = s.threshold_secondary; diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp index c103b78c..4ae20f48 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp @@ -46,7 +46,7 @@ TrackerDialog::TrackerDialog() tie_setting(s.reset_time, ui.reset_spin); tie_setting(s.cam_index, ui.camdevice_combo); - tie_setting(s.cam_f, ui.f_dspin); + tie_setting(s.cam_fov, ui.fov_dspin); tie_setting(s.cam_res_x, ui.res_x_spin); tie_setting(s.cam_res_y, ui.res_y_spin); tie_setting(s.cam_fps, ui.fps_spin); diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h index 564e1264..e0dfa2e6 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_settings.h @@ -28,7 +28,7 @@ struct settings threshold_secondary, min_point_size, max_point_size; - value cam_f; + value cam_fov; value m01_x, m01_y, m01_z; value m02_x, m02_y, m02_z; @@ -57,7 +57,7 @@ struct settings threshold_secondary(b, "threshold-secondary", 128), min_point_size(b, "min-point-size", 10), max_point_size(b, "max-point-size", 50), - cam_f(b, "camera-focal-length", 1), + cam_fov(b, "camera-fov", 56), m01_x(b, "m_01-x", 0), m01_y(b, "m_01-y", 0), m01_z(b, "m_01-z", 0), diff --git a/FTNoIR_Tracker_PT/point_tracker.cpp b/FTNoIR_Tracker_PT/point_tracker.cpp index dfefdaf8..1df70b17 100644 --- a/FTNoIR_Tracker_PT/point_tracker.cpp +++ b/FTNoIR_Tracker_PT/point_tracker.cpp @@ -17,23 +17,6 @@ using namespace cv; using namespace boost; using namespace std; -const float PI = 3.14159265358979323846f; - -// ---------------------------------------------------------------------------- -static void get_row(const Matx33f& m, int i, Vec3f& v) -{ - v[0] = m(i,0); - v[1] = m(i,1); - v[2] = m(i,2); -} - -static void set_row(Matx33f& m, int i, const Vec3f& v) -{ - m(i,0) = v[0]; - m(i,1) = v[1]; - m(i,2) = v[2]; -} - // ---------------------------------------------------------------------------- PointModel::PointModel(Vec3f M01, Vec3f M02) : M01(M01), @@ -108,9 +91,11 @@ PointTracker::PointTracker() dt_reset(1), v_t(0,0,0), v_r(0,0,0), - dynamic_pose_resolution(true) + dynamic_pose_resolution(true), + fov(0), + _w(0), + _h(0) { - X_CM.t[2] = 1000; // default position: 1 m away from cam; } void PointTracker::reset() @@ -128,7 +113,7 @@ void PointTracker::reset_velocities() } -bool PointTracker::track(const vector& points, float f, float dt) +bool PointTracker::track(const vector& points, float fov, float dt, int w, int h, const cv::Vec3f& headpos) { if (!dynamic_pose_resolution) init_phase = true; @@ -140,12 +125,7 @@ bool PointTracker::track(const vector& points, float f, float dt) reset(); } - bool no_model = -#ifdef OPENTRACK_API - point_model.get() == NULL; -#else - !point_model; -#endif + bool no_model = !point_model; // if there is a pointtracking problem, reset the velocities if (no_model || points.size() != PointModel::N_POINTS) @@ -161,7 +141,7 @@ bool PointTracker::track(const vector& points, float f, float dt) predict(dt_valid); // if there is a point correspondence problem something has gone wrong, do a reset - if (!find_correspondences(points, f)) + if (!find_correspondences(points)) { //qDebug()<<"Error in finding point correspondences!"; X_CM = X_CM_old; // undo prediction @@ -169,7 +149,8 @@ bool PointTracker::track(const vector& points, float f, float dt) return false; } - int n_iter = POSIT(f); + // XXX TODO fov + POSIT(fov, w, h, headpos); //qDebug()<<"Number of POSIT iterations: "<& points, float f) +bool PointTracker::find_correspondences(const vector& points) { if (init_phase) { // We do a simple freetrack-like sorting in the init phase... @@ -215,9 +196,9 @@ bool PointTracker::find_correspondences(const vector& points, float f) else { // ... otherwise we look at the distance to the projection of the expected model points // project model points under current pose - p_exp[0] = project(Vec3f(0,0,0), f); - p_exp[1] = project(point_model->M01, f); - p_exp[2] = project(point_model->M02, f); + p_exp[0] = project(Vec3f(0,0,0)); + p_exp[1] = project(point_model->M01); + p_exp[2] = project(point_model->M02); // set correspondences by minimum distance to projected model point bool point_taken[PointModel::N_POINTS]; @@ -251,130 +232,48 @@ bool PointTracker::find_correspondences(const vector& points, float f) -int PointTracker::POSIT(float f) +void PointTracker::POSIT(float fov, int w, int h, const cv::Vec3f& headpos) { - // POSIT algorithm for coplanar points as presented in - // [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"] - // we use the same notation as in the paper here - - // The expected rotation used for resolving the ambiguity in POSIT: - // In every iteration step the rotation closer to R_expected is taken - Matx33f R_expected; - if (init_phase) - R_expected = Matx33f::eye(); // in the init phase, we want to be close to the default pose = no rotation - else - R_expected = X_CM.R; // later we want to be close to the last (predicted) rotation - - // initial pose = last (predicted) pose - Vec3f k; - get_row(R_expected, 2, k); - float Z0 = init_phase ? 1000 : X_CM.t[2]; - - float old_epsilon_1 = 0; - float old_epsilon_2 = 0; - float epsilon_1 = 1; - float epsilon_2 = 1; - - Vec3f I0, J0; - Vec2f I0_coeff, J0_coeff; - - Vec3f I_1, J_1, I_2, J_2; - Matx33f R_1, R_2; - Matx33f* R_current; - - const int MAX_ITER = 100; - const float EPS_THRESHOLD = 1e-4; - - int i=1; - for (; iM01)/Z0; - epsilon_2 = k.dot(point_model->M02)/Z0; - - // vector of scalar products and - Vec2f I0_M0i(p[1][0]*(1.0 + epsilon_1) - p[0][0], - p[2][0]*(1.0 + epsilon_2) - p[0][0]); - Vec2f J0_M0i(p[1][1]*(1.0 + epsilon_1) - p[0][1], - p[2][1]*(1.0 + epsilon_2) - p[0][1]); - - // construct projection of I, J onto M0i plane: I0 and J0 - I0_coeff = point_model->P * I0_M0i; - J0_coeff = point_model->P * J0_M0i; - I0 = I0_coeff[0]*point_model->M01 + I0_coeff[1]*point_model->M02; - J0 = J0_coeff[0]*point_model->M01 + J0_coeff[1]*point_model->M02; - - // calculate u component of I, J - float II0 = I0.dot(I0); - float IJ0 = I0.dot(J0); - float JJ0 = J0.dot(J0); - float rho, theta; - if (JJ0 == II0) { - rho = sqrt(abs(2*IJ0)); - theta = -PI/4; - if (IJ0<0) theta *= -1; - } - else { - rho = sqrt(sqrt( (JJ0-II0)*(JJ0-II0) + 4*IJ0*IJ0 )); - theta = atan( -2*IJ0 / (JJ0-II0) ); - if (JJ0 - II0 < 0) theta += PI; - theta /= 2; - } + // XXX hack + this->fov = fov; + _w = w; + _h = h; + std::vector obj_points; + std::vector img_points; - // construct the two solutions - I_1 = I0 + rho*cos(theta)*point_model->u; - I_2 = I0 - rho*cos(theta)*point_model->u; + obj_points.push_back(headpos); + obj_points.push_back(point_model->M01 + headpos); + obj_points.push_back(point_model->M02 + headpos); - J_1 = J0 + rho*sin(theta)*point_model->u; - J_2 = J0 - rho*sin(theta)*point_model->u; + img_points.push_back(p[0]); + img_points.push_back(p[1]); + img_points.push_back(p[2]); - float norm_const = 1.0/norm(I_1); // all have the same norm - - // create rotation matrices - I_1 *= norm_const; J_1 *= norm_const; - I_2 *= norm_const; J_2 *= norm_const; + const float HT_PI = 3.1415926535; - set_row(R_1, 0, I_1); - set_row(R_1, 1, J_1); - set_row(R_1, 2, I_1.cross(J_1)); - - set_row(R_2, 0, I_2); - set_row(R_2, 1, J_2); - set_row(R_2, 2, I_2.cross(J_2)); + const float focal_length_w = 0.5 * w / tan(fov * HT_PI / 180); + const float focal_length_h = 0.5 * h / tan(fov * h / w * HT_PI / 180.0); - // the single translation solution - Z0 = norm_const * f; + cv::Mat intrinsics = cv::Mat::eye(3, 3, CV_32FC1); + intrinsics.at (0, 0) = focal_length_w; + intrinsics.at (1, 1) = focal_length_h; + intrinsics.at (0, 2) = w/2; + intrinsics.at (1, 2) = h/2; - // pick the rotation solution closer to the expected one - // in simple metric d(A,B) = || I - A * B^T || - float R_1_deviation = norm(Matx33f::eye() - R_expected * R_1.t()); - float R_2_deviation = norm(Matx33f::eye() - R_expected * R_2.t()); + cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1); - if (R_1_deviation < R_2_deviation) - R_current = &R_1; - else - R_current = &R_2; + bool lastp = !rvec.empty() && !tvec.empty() && !init_phase; - get_row(*R_current, 2, k); + cv::solvePnP(obj_points, img_points, intrinsics, dist_coeffs, rvec, tvec, lastp, cv::ITERATIVE); - // check for convergence condition - if (abs(epsilon_1 - old_epsilon_1) + abs(epsilon_2 - old_epsilon_2) < EPS_THRESHOLD) - break; - old_epsilon_1 = epsilon_1; - old_epsilon_2 = epsilon_2; - } + cv::Mat rmat; + cv::Rodrigues(rvec, rmat); // apply results - X_CM.R = *R_current; - X_CM.t[0] = p[0][0] * Z0/f; - X_CM.t[1] = p[0][1] * Z0/f; - X_CM.t[2] = Z0; - - return i; - - //Rodrigues(X_CM.R, r); - //qDebug()<<"iter: "<(i); + for (int j = 0; j < 3; j++) + X_CM.R(i, j) = rmat.at(i, j); + } } diff --git a/FTNoIR_Tracker_PT/point_tracker.h b/FTNoIR_Tracker_PT/point_tracker.h index 11034100..e05e8f98 100644 --- a/FTNoIR_Tracker_PT/point_tracker.h +++ b/FTNoIR_Tracker_PT/point_tracker.h @@ -90,7 +90,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 - bool track(const std::vector& points, float f, float dt); + bool track(const std::vector& points, float fov, float dt, int w, int h, const cv::Vec3f &headpos); boost::shared_ptr point_model; bool dynamic_pose_resolution; @@ -98,15 +98,35 @@ public: FrameTrafo get_pose() const { return X_CM; } void reset(); + float fov; + int _w, _h; protected: - inline cv::Vec2f project(const cv::Vec3f& v_M, float f) + cv::Vec2f project(const cv::Vec3f& v_M) { - 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]); + if (!rvec.empty() && !tvec.empty() && fov > 0) + { + const float HT_PI = 3.1415926535; + const int w = _w, h = _h; + const float focal_length_w = 0.5 * w / tan(fov * HT_PI / 180); + const float focal_length_h = 0.5 * h / tan(fov * h / w * HT_PI / 180.0); + + cv::Mat intrinsics = cv::Mat::eye(3, 3, CV_32FC1); + intrinsics.at (0, 0) = focal_length_w; + intrinsics.at (1, 1) = focal_length_h; + intrinsics.at (0, 2) = w/2; + intrinsics.at (1, 2) = h/2; + std::vector xs; + xs.push_back(v_M); + cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1); + std::vector rets(1); + cv::projectPoints(xs, rvec, tvec, intrinsics, dist_coeffs, rets); + return rets[0]; + } + return cv::Vec2f(); } - bool find_correspondences(const std::vector& points, float f); + bool find_correspondences(const std::vector& points); cv::Vec2f p[PointModel::N_POINTS]; // the points in model order cv::Vec2f p_exp[PointModel::N_POINTS]; // the expected point positions @@ -116,7 +136,7 @@ protected: void reset_velocities(); - int POSIT(float f); // The POSIT algorithm, returns the number of iterations + void POSIT(float fov, int w, int h, const cv::Vec3f &headpos); // The POSIT algorithm, returns the number of iterations bool init_phase; float dt_valid; // time since last valid tracking result @@ -124,6 +144,7 @@ protected: cv::Vec3f v_r; FrameTrafo X_CM; // trafo from model to camera FrameTrafo X_CM_old; + cv::Mat rvec, tvec; }; #endif //POINTTRACKER_H -- cgit v1.2.3