From 689c297be28e2f5050e234bdc69594c53166f194 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 12 Jan 2014 19:54:04 +0100 Subject: Revert "use levmarq instead of coplanar POSIT implemented in numerically unstable fashion" This reverts commit 0bc59a0a7a42bcf65ebf3814e5f25fb17d735faa. --- 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, 169 insertions(+), 92 deletions(-) diff --git a/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui b/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui index 461253cf..bdbed955 100644 --- a/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui +++ b/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui @@ -515,26 +515,23 @@ - FOV + F/W - fov_dspin + f_dspin - + The camera's focal length devided by its sensor width 2 - - 1.000000000000000 - - 1.000000000000000 + 0.100000000000000 @@ -1736,7 +1733,7 @@ res_x_spin res_y_spin fps_spin - fov_dspin + f_dspin camroll_combo campitch_spin camyaw_spin diff --git a/FTNoIR_Tracker_PT/camera.cpp b/FTNoIR_Tracker_PT/camera.cpp index 8986be60..754533c5 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_fov(float f) +void Camera::set_f(float f) { - if (cam_desired.fov != f) + if (cam_desired.f != f) { - cam_desired.fov = f; + cam_desired.f = f; _set_f(); } } @@ -208,7 +208,7 @@ void CVCamera::_set_index() void CVCamera::_set_f() { - cam_info.fov = cam_desired.fov; + cam_info.f = cam_desired.f; } void CVCamera::_set_fps() diff --git a/FTNoIR_Tracker_PT/camera.h b/FTNoIR_Tracker_PT/camera.h index 6768e419..ea68c387 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), fov(56) {} + CamInfo() : res_x(0), res_y(0), fps(0), f(1) {} int res_x; int res_y; int fps; - float fov; + float f; // (focal length) / (sensor width) }; // ---------------------------------------------------------------------------- @@ -48,7 +48,7 @@ public: // calls corresponding template methods and reinitializes frame rate calculation void set_device_index(int index); - void set_fov(float f); + void set_f(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 740cf3da..53d9c9e4 100644 --- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp +++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp @@ -83,7 +83,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().fov, dt, frame.cols, frame.rows, t_MH); + tracking_valid = point_tracker.track(points, camera.get_info().f, dt); video_widget->update_image(frame); } #ifdef PT_PERF_LOG @@ -103,7 +103,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_fov(s.cam_fov); + camera.set_f(s.cam_f); 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 4ae20f48..c103b78c 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_fov, ui.fov_dspin); + tie_setting(s.cam_f, ui.f_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 b2ab8854..109090b3 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_fov; + value cam_f; 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_fov(b, "camera-fov", 56), + cam_f(b, "camera-focal-length", 1), 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 1df70b17..dfefdaf8 100644 --- a/FTNoIR_Tracker_PT/point_tracker.cpp +++ b/FTNoIR_Tracker_PT/point_tracker.cpp @@ -17,6 +17,23 @@ 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), @@ -91,11 +108,9 @@ PointTracker::PointTracker() dt_reset(1), v_t(0,0,0), v_r(0,0,0), - dynamic_pose_resolution(true), - fov(0), - _w(0), - _h(0) + dynamic_pose_resolution(true) { + X_CM.t[2] = 1000; // default position: 1 m away from cam; } void PointTracker::reset() @@ -113,7 +128,7 @@ void PointTracker::reset_velocities() } -bool PointTracker::track(const vector& points, float fov, float dt, int w, int h, const cv::Vec3f& headpos) +bool PointTracker::track(const vector& points, float f, float dt) { if (!dynamic_pose_resolution) init_phase = true; @@ -125,7 +140,12 @@ bool PointTracker::track(const vector& points, float fov, float dt, int w reset(); } - bool no_model = !point_model; + bool no_model = +#ifdef OPENTRACK_API + point_model.get() == NULL; +#else + !point_model; +#endif // if there is a pointtracking problem, reset the velocities if (no_model || points.size() != PointModel::N_POINTS) @@ -141,7 +161,7 @@ bool PointTracker::track(const vector& points, float fov, float dt, int w predict(dt_valid); // if there is a point correspondence problem something has gone wrong, do a reset - if (!find_correspondences(points)) + if (!find_correspondences(points, f)) { //qDebug()<<"Error in finding point correspondences!"; X_CM = X_CM_old; // undo prediction @@ -149,8 +169,7 @@ bool PointTracker::track(const vector& points, float fov, float dt, int w return false; } - // XXX TODO fov - POSIT(fov, w, h, headpos); + int n_iter = POSIT(f); //qDebug()<<"Number of POSIT iterations: "<& points) +bool PointTracker::find_correspondences(const vector& points, float f) { if (init_phase) { // We do a simple freetrack-like sorting in the init phase... @@ -196,9 +215,9 @@ bool PointTracker::find_correspondences(const vector& points) 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)); - p_exp[1] = project(point_model->M01); - p_exp[2] = project(point_model->M02); + 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); // set correspondences by minimum distance to projected model point bool point_taken[PointModel::N_POINTS]; @@ -232,48 +251,130 @@ bool PointTracker::find_correspondences(const vector& points) -void PointTracker::POSIT(float fov, int w, int h, const cv::Vec3f& headpos) +int PointTracker::POSIT(float f) { - // XXX hack - this->fov = fov; - _w = w; - _h = h; - std::vector obj_points; - std::vector img_points; + // 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; + } - obj_points.push_back(headpos); - obj_points.push_back(point_model->M01 + headpos); - obj_points.push_back(point_model->M02 + headpos); + // construct the two solutions + I_1 = I0 + rho*cos(theta)*point_model->u; + I_2 = I0 - rho*cos(theta)*point_model->u; - img_points.push_back(p[0]); - img_points.push_back(p[1]); - img_points.push_back(p[2]); + J_1 = J0 + rho*sin(theta)*point_model->u; + J_2 = J0 - rho*sin(theta)*point_model->u; - const float HT_PI = 3.1415926535; + 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 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); + 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)); - 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; + // the single translation solution + Z0 = norm_const * f; - cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1); + // 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()); - bool lastp = !rvec.empty() && !tvec.empty() && !init_phase; + if (R_1_deviation < R_2_deviation) + R_current = &R_1; + else + R_current = &R_2; - cv::solvePnP(obj_points, img_points, intrinsics, dist_coeffs, rvec, tvec, lastp, cv::ITERATIVE); + get_row(*R_current, 2, k); - cv::Mat rmat; - cv::Rodrigues(rvec, rmat); + // 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; + } // apply results - for (int i = 0; i < 3; i++) - { - X_CM.t[i] = tvec.at(i); - for (int j = 0; j < 3; j++) - X_CM.R(i, j) = rmat.at(i, j); - } + 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: "<& points, float fov, float dt, int w, int h, const cv::Vec3f &headpos); + bool track(const std::vector& points, float f, float dt); boost::shared_ptr point_model; bool dynamic_pose_resolution; @@ -98,35 +98,15 @@ public: FrameTrafo get_pose() const { return X_CM; } void reset(); - float fov; - int _w, _h; protected: - cv::Vec2f project(const cv::Vec3f& v_M) + inline cv::Vec2f project(const cv::Vec3f& v_M, float f) { - 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(); + 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]); } - bool find_correspondences(const std::vector& points); + bool find_correspondences(const std::vector& points, float f); cv::Vec2f p[PointModel::N_POINTS]; // the points in model order cv::Vec2f p_exp[PointModel::N_POINTS]; // the expected point positions @@ -136,7 +116,7 @@ protected: void reset_velocities(); - void POSIT(float fov, int w, int h, const cv::Vec3f &headpos); // The POSIT algorithm, returns the number of iterations + int POSIT(float f); // The POSIT algorithm, returns the number of iterations bool init_phase; float dt_valid; // time since last valid tracking result @@ -144,7 +124,6 @@ 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