From 6dc31bc869c44c84429f3e52a033cbc09c1d3844 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sat, 17 Oct 2015 16:31:41 +0200 Subject: pt: reformat posit --- ftnoir_tracker_pt/point_tracker.cpp | 240 ++++++++++++++++++------------------ 1 file changed, 120 insertions(+), 120 deletions(-) diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp index cedf1979..383061c5 100644 --- a/ftnoir_tracker_pt/point_tracker.cpp +++ b/ftnoir_tracker_pt/point_tracker.cpp @@ -110,7 +110,7 @@ void PointTracker::track(const std::vector& points, const PointModel& order = find_correspondences(points, model); else order = find_correspondences_previous(points, model, f); - + POSIT(model, order, f); init_phase = false; t.start(); @@ -142,127 +142,127 @@ PointTracker::PointOrder PointTracker::find_correspondences(const std::vector and - cv::Vec2f I0_M0i(order[1][0]*(1.0 + epsilon_1) - order[0][0], - order[2][0]*(1.0 + epsilon_2) - order[0][0]); - cv::Vec2f J0_M0i(order[1][1]*(1.0 + epsilon_1) - order[0][1], - order[2][1]*(1.0 + epsilon_2) - order[0][1]); - - // construct projection of I, J onto M0i plane: I0 and J0 - I0_coeff = model.P * I0_M0i; - J0_coeff = model.P * J0_M0i; - I0 = I0_coeff[0]*model.M01 + I0_coeff[1]*model.M02; - J0 = J0_coeff[0]*model.M01 + J0_coeff[1]*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 = std::sqrt(std::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; - } - - // construct the two solutions - I_1 = I0 + rho*cos(theta)*model.u; - I_2 = I0 - rho*cos(theta)*model.u; - - J_1 = J0 + rho*sin(theta)*model.u; - J_2 = J0 - rho*sin(theta)*model.u; - - float norm_const = 1.0/cv::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; - - 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)); - - // the single translation solution - Z0 = norm_const * focal_length; - - // pick the rotation solution closer to the expected one - // in simple metric d(A,B) = || I - A * B^T || - float R_1_deviation = cv::norm(cv::Matx33f::eye() - R_expected * R_1.t()); - float R_2_deviation = cv::norm(cv::Matx33f::eye() - R_expected * R_2.t()); - - if (R_1_deviation < R_2_deviation) - R_current = &R_1; - else - R_current = &R_2; - - get_row(*R_current, 2, k); - - // check for convergence condition - if (std::abs(epsilon_1 - old_epsilon_1) + std::abs(epsilon_2 - old_epsilon_2) < EPS_THRESHOLD) - break; - old_epsilon_1 = epsilon_1; - old_epsilon_2 = epsilon_2; - } - - // apply results - X_CM.R = *R_current; - X_CM.t[0] = order[0][0] * Z0/focal_length; - X_CM.t[1] = order[0][1] * Z0/focal_length; - X_CM.t[2] = Z0; - - //qDebug() << "iter:" << i; - - return i; + // 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 + cv::Matx33f R_expected = cv::Matx33f::eye(); + + // initial pose = last (predicted) pose + cv::Vec3f k; + get_row(R_expected, 2, k); + float Z0 = 1000.f; + + float old_epsilon_1 = 0; + float old_epsilon_2 = 0; + float epsilon_1 = 1; + float epsilon_2 = 1; + + cv::Vec3f I0, J0; + cv::Vec2f I0_coeff, J0_coeff; + + cv::Vec3f I_1, J_1, I_2, J_2; + cv::Matx33f R_1, R_2; + cv::Matx33f* R_current; + + const int MAX_ITER = 100; + const float EPS_THRESHOLD = 1e-4; + + const cv::Vec2f* order = order_.points; + + int i=1; + for (; i and + cv::Vec2f I0_M0i(order[1][0]*(1.0 + epsilon_1) - order[0][0], + order[2][0]*(1.0 + epsilon_2) - order[0][0]); + cv::Vec2f J0_M0i(order[1][1]*(1.0 + epsilon_1) - order[0][1], + order[2][1]*(1.0 + epsilon_2) - order[0][1]); + + // construct projection of I, J onto M0i plane: I0 and J0 + I0_coeff = model.P * I0_M0i; + J0_coeff = model.P * J0_M0i; + I0 = I0_coeff[0]*model.M01 + I0_coeff[1]*model.M02; + J0 = J0_coeff[0]*model.M01 + J0_coeff[1]*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 = std::sqrt(std::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; + } + + // construct the two solutions + I_1 = I0 + rho*cos(theta)*model.u; + I_2 = I0 - rho*cos(theta)*model.u; + + J_1 = J0 + rho*sin(theta)*model.u; + J_2 = J0 - rho*sin(theta)*model.u; + + float norm_const = 1.0/cv::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; + + 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)); + + // the single translation solution + Z0 = norm_const * focal_length; + + // pick the rotation solution closer to the expected one + // in simple metric d(A,B) = || I - A * B^T || + float R_1_deviation = cv::norm(cv::Matx33f::eye() - R_expected * R_1.t()); + float R_2_deviation = cv::norm(cv::Matx33f::eye() - R_expected * R_2.t()); + + if (R_1_deviation < R_2_deviation) + R_current = &R_1; + else + R_current = &R_2; + + get_row(*R_current, 2, k); + + // check for convergence condition + if (std::abs(epsilon_1 - old_epsilon_1) + std::abs(epsilon_2 - old_epsilon_2) < EPS_THRESHOLD) + break; + old_epsilon_1 = epsilon_1; + old_epsilon_2 = epsilon_2; + } + + // apply results + X_CM.R = *R_current; + X_CM.t[0] = order[0][0] * Z0/focal_length; + X_CM.t[1] = order[0][1] * Z0/focal_length; + X_CM.t[2] = Z0; + + //qDebug() << "iter:" << i; + + return i; } 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]); + 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]); } -- cgit v1.2.3