diff options
| -rw-r--r-- | tracker-pt/point_tracker.cpp | 20 | ||||
| -rw-r--r-- | tracker-pt/point_tracker.h | 3 |
2 files changed, 15 insertions, 8 deletions
diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp index a68e83eb..34699aab 100644 --- a/tracker-pt/point_tracker.cpp +++ b/tracker-pt/point_tracker.cpp @@ -288,7 +288,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; @@ -308,8 +308,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; @@ -404,8 +404,8 @@ 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"; - return -1; + qDebug() << "posit nan R"; + goto fail; } } @@ -414,8 +414,8 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f foca int ret = std::fpclassify(t[i]); if (ret == FP_NAN || ret == FP_INFINITE) { - qDebug() << "posit nan -- T"; - return -1; + qDebug() << "posit nan T"; + goto fail; } } @@ -425,9 +425,14 @@ 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; +fail: + X_CM_expected = {}; + return -1; } #ifdef __clang__ @@ -449,6 +454,7 @@ void PointTracker::reset_state() { prev_positions_valid = false; init_phase = true; + X_CM_expected = {}; } } // ns pt_module diff --git a/tracker-pt/point_tracker.h b/tracker-pt/point_tracker.h index 63d81456..df56842a 100644 --- a/tracker-pt/point_tracker.h +++ b/tracker-pt/point_tracker.h @@ -77,7 +77,8 @@ private: // The POSIT algorithm, returns the number of iterations int POSIT(const PointModel& point_model, const PointOrder& order, f focal_length); - Affine X_CM; // transform from model to camera + Affine X_CM; // transform from model to camera + Affine X_CM_expected; PointOrder prev_positions; Timer t; bool init_phase = true, prev_positions_valid = false; |