diff options
Diffstat (limited to 'tracker-pt/point_tracker.cpp')
| -rw-r--r-- | tracker-pt/point_tracker.cpp | 174 |
1 files changed, 62 insertions, 112 deletions
diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp index 63c6f47c..bda412e2 100644 --- a/tracker-pt/point_tracker.cpp +++ b/tracker-pt/point_tracker.cpp @@ -8,14 +8,16 @@ #include "point_tracker.h" #include "compat/math-imports.hpp" -using namespace types; - #include <vector> #include <algorithm> #include <cmath> #include <QDebug> +namespace pt_module { + +using namespace numeric_types; + static void get_row(const mat33& m, int i, vec3& v) { v[0] = m(i,0); @@ -48,6 +50,9 @@ void PointModel::set_model(const pt_settings& s) { switch (s.active_model_panel) { + default: + eval_once(qDebug() << "pt: wrong model type selected"); + [[fallthrough]]; case Clip: M01 = vec3(0, s.clip_ty, -s.clip_tz); M02 = vec3(0, -s.clip_by, -s.clip_bz); @@ -65,18 +70,19 @@ void PointModel::set_model(const pt_settings& s) void PointModel::get_d_order(const vec2* points, unsigned* d_order, const vec2& d) const { + constexpr unsigned cnt = PointModel::N_POINTS; // fit line to orthographically projected points using t = std::pair<f,unsigned>; - t d_vals[3]; + t d_vals[cnt]; // get sort indices with respect to d scalar product - for (unsigned i = 0; i < PointModel::N_POINTS; ++i) + for (unsigned i = 0; i < cnt; ++i) d_vals[i] = t(d.dot(points[i]), i); std::sort(d_vals, - d_vals + 3u, + d_vals + 3, [](const t& a, const t& b) { return a.first < b.first; }); - for (unsigned i = 0; i < PointModel::N_POINTS; ++i) + for (unsigned i = 0; i < cnt; ++i) d_order[i] = d_vals[i].second; } @@ -87,38 +93,34 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const vec2* const PointModel& model, const pt_camera_info& info) { - const double fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y); + const f fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y); PointTracker::PointOrder p; p[0] = project(vec3(0,0,0), fx); p[1] = project(model.M01, fx); p[2] = project(model.M02, fx); - const int diagonal = int(std::sqrt(f(info.res_x*info.res_x + info.res_y*info.res_y))); - constexpr int div = 100; - const int max_dist = diagonal / div; // 8 pixels for 640x480 + constexpr unsigned sz = PointModel::N_POINTS; // set correspondences by minimum distance to projected model point - bool point_taken[PointModel::N_POINTS]; - for (unsigned i=0; i<PointModel::N_POINTS; ++i) + bool point_taken[sz]; + for (unsigned i=0; i < sz; ++i) // NOLINT(modernize-loop-convert) point_taken[i] = false; - for (unsigned i=0; i<PointModel::N_POINTS; ++i) + for (unsigned i=0; i < sz; ++i) { f min_sdist = 0; unsigned min_idx = 0; // find closest point to projected model point i - for (unsigned j=0; j<PointModel::N_POINTS; ++j) + for (unsigned j=0; j < sz; ++j) { vec2 d = p[i]-points[j]; f sdist = d.dot(d); - if (sdist < min_sdist || j==0) + if (sdist < min_sdist || j == 0) { min_idx = j; min_sdist = sdist; } } - if (min_sdist > max_dist) - return find_correspondences(points, model); // if one point is closest to more than one model point, fallback if (point_taken[min_idx]) @@ -133,119 +135,61 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const vec2* return p; } -bool PointTracker::maybe_use_old_point_order(const PointOrder& order, const pt_camera_info& info) -{ - constexpr f std_width = 640, std_height = 480; - - PointOrder scaled_order; - - const f cx = std_width / info.res_x; - const f cy = std_height / info.res_y; - - for (unsigned k = 0; k < 3; k++) - { - // note, the .y component is actually scaled by width - scaled_order[k][0] = std_width * cx * order[k][0]; - scaled_order[k][1] = std_width * cy * order[k][1]; - } - - f sum = 0; - - for (unsigned k = 0; k < 3; k++) - { - vec2 tmp = prev_scaled_order[k] - scaled_order[k]; - sum += std::sqrt(tmp.dot(tmp)); - } - - // CAVEAT don't increase too much, it visibly loses precision - constexpr f max_dist = f(.13); - - const bool validp = sum < max_dist; - - prev_order_valid &= validp; - - if (!prev_order_valid) - { - prev_order = order; - prev_scaled_order = scaled_order; - } - -#if 0 - { - static Timer tt; - static int cnt1 = 0, cnt2 = 0; - if (tt.elapsed_ms() >= 1000) - { - tt.start(); - if (cnt1 + cnt2) - { - qDebug() << "old-order" << ((cnt1 * 100) / f(cnt1 + cnt2)) << "nsamples" << (cnt1 + cnt2); - cnt1 = 0, cnt2 = 0; - } - } - if (validp) - cnt1++; - else - cnt2++; - } -#endif - - prev_order_valid = validp; - - return validp; -} - void PointTracker::track(const std::vector<vec2>& points, const PointModel& model, const pt_camera_info& info, int init_phase_timeout) { - const double fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y); + const f fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y); PointOrder order; - if (init_phase_timeout > 0 && t.elapsed_ms() > init_phase_timeout) + if (init_phase_timeout <= 0 || t.elapsed_ms() > init_phase_timeout || init_phase) { - t.start(); init_phase = true; - } - - if (!(init_phase_timeout > 0 && !init_phase)) order = find_correspondences(points.data(), model); + } else order = find_correspondences_previous(points.data(), model, info); - if (maybe_use_old_point_order(order, info) || - POSIT(model, order, fx) != -1) + if (POSIT(model, order, fx) != -1) { init_phase = false; t.start(); } + else + reset_state(); } PointTracker::PointOrder PointTracker::find_correspondences(const vec2* points, const PointModel& model) { - static const Affine a(mat33::eye(), vec3(0, 0, 1)); + constexpr unsigned cnt = PointModel::N_POINTS; // We do a simple freetrack-like sorting in the init phase... - unsigned point_d_order[PointModel::N_POINTS]; - unsigned model_d_order[PointModel::N_POINTS]; - // sort points + unsigned point_d_order[cnt]; + unsigned model_d_order[cnt]; + // calculate d and d_order for simple freetrack-like point correspondence vec2 d(model.M01[0]-model.M02[0], model.M01[1]-model.M02[1]); + // sort points model.get_d_order(points, point_d_order, d); - // calculate d and d_order for simple freetrack-like point correspondence - vec2 pts[3] = { - vec2(0, 0), - vec2(model.M01[0], model.M01[1]), - vec2(model.M02[0], model.M02[1]) + vec2 pts[cnt] { + { 0, 0 }, + { model.M01[0], model.M01[1] }, + { model.M02[0], model.M02[1] }, }; model.get_d_order(pts, model_d_order, d); + // set correspondences PointOrder p; - for (unsigned i = 0; i < PointModel::N_POINTS; ++i) + for (unsigned i = 0; i < cnt; ++i) p[model_d_order[i]] = points[point_d_order[i]]; return p; } +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wfloat-equal" +#endif + int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f focal_length) { // POSIT algorithm for coplanar points as presented in @@ -254,7 +198,7 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca // The expected rotation used for resolving the ambiguity in POSIT: // In every iteration step the rotation closer to R_expected is taken - static const mat33 R_expected(X_CM.R); + const mat33& R_expected{X_CM_expected.R}; // initial pose = last (predicted) pose vec3 k; @@ -263,8 +207,7 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca f old_epsilon_1 = 0; f old_epsilon_2 = 0; - f epsilon_1 = 1; - f epsilon_2 = 1; + f epsilon_1, epsilon_2; vec3 I0, J0; vec2 I0_coeff, J0_coeff; @@ -275,8 +218,8 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca constexpr int max_iter = 100; - int i=1; - for (; i<max_iter; ++i) + int i; + for (i = 1; i < max_iter; ++i) { epsilon_1 = k.dot(model.M01)/Z0; epsilon_2 = k.dot(model.M02)/Z0; @@ -301,14 +244,14 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca // CAVEAT don't change to comparison with an epsilon -sh 20160423 if (JJ0 == II0) { rho = sqrt(fabs(2*IJ0)); - theta = -M_PI/4; + 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) ); // avoid branch misprediction - theta += (JJ0 - II0 < 0) * M_PI; + theta += (JJ0 - II0 < 0) * pi; theta *= f(.5); } @@ -319,7 +262,7 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca J_1 = J0 + rho*sin(theta)*model.u; J_2 = J0 - rho*sin(theta)*model.u; - f norm_const = 1/cv::norm(I_1); // all have the same norm + f norm_const = (f)(1/cv::norm(I_1)); // all have the same norm // create rotation matrices I_1 *= norm_const; J_1 *= norm_const; @@ -338,8 +281,8 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca // pick the rotation solution closer to the expected one // in simple metric d(A,B) = || I - A * B^T || - f R_1_deviation = cv::norm(mat33::eye() - R_expected * R_1.t()); - f R_2_deviation = cv::norm(mat33::eye() - R_expected * R_2.t()); + f R_1_deviation = (f)(cv::norm(mat33::eye() - R_expected * R_1.t())); + f R_2_deviation = (f)(cv::norm(mat33::eye() - R_expected * R_2.t())); if (R_1_deviation < R_2_deviation) R_current = &R_1; @@ -351,7 +294,7 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca // check for convergence condition const f delta = fabs(epsilon_1 - old_epsilon_1) + fabs(epsilon_2 - old_epsilon_2); - if (!(delta > constants::eps)) + if (delta < eps) break; old_epsilon_1 = epsilon_1; @@ -371,17 +314,17 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca int ret = std::fpclassify(r(i, j)); if (ret == FP_NAN || ret == FP_INFINITE) { - qDebug() << "posit nan -- R"; + qDebug() << "posit nan R"; return -1; } } - for (unsigned i = 0; i < 3; i++) + for (unsigned i = 0; i < 3; i++) // NOLINT(modernize-loop-convert) { int ret = std::fpclassify(t[i]); if (ret == FP_NAN || ret == FP_INFINITE) { - qDebug() << "posit nan -- T"; + qDebug() << "posit nan T"; return -1; } } @@ -392,11 +335,17 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca X_CM.t[1] = t[1]; X_CM.t[2] = t[2]; + X_CM_expected = X_CM; + //qDebug() << "iter:" << i; return i; } +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + vec2 PointTracker::project(const vec3& v_M, f focal_length) { return project(v_M, focal_length, X_CM); @@ -410,7 +359,8 @@ vec2 PointTracker::project(const vec3& v_M, f focal_length, const Affine& X_CM) void PointTracker::reset_state() { - prev_order_valid = false; init_phase = true; + X_CM_expected = {}; } +} // ns pt_module |