From 223ff7abf556d7af4b1aeb63a4dc1664fd9d6161 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Tue, 19 Jul 2016 07:08:44 +0200 Subject: tracker/pt: check for nan before writing pose --- tracker-pt/point_tracker.cpp | 29 +++++++++++++++++++++++------ 1 file changed, 23 insertions(+), 6 deletions(-) (limited to 'tracker-pt/point_tracker.cpp') diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp index 4c1e177f..61c54fa4 100644 --- a/tracker-pt/point_tracker.cpp +++ b/tracker-pt/point_tracker.cpp @@ -6,6 +6,7 @@ */ #include "point_tracker.h" +#include "opentrack-compat/nan.hpp" #include #include @@ -145,8 +146,8 @@ void PointTracker::track(const std::vector& points, const PointModel& mode else order = find_correspondences_previous(points, model, focal_length); - POSIT(model, order, focal_length); - init_phase = false; + const int iters = POSIT(model, order, focal_length); + init_phase = iters != -1; t.start(); } @@ -294,12 +295,28 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, f foc old_epsilon_2 = epsilon_2; } + const f t[3] = { + order[0][0] * Z0/focal_length, + order[0][1] * Z0/focal_length, + Z0 + }; + const mat33& r = *R_current; + + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + if (nanp(r(i, j))) + return -1; + + for (unsigned i = 0; i < 3; i++) + if (nanp(t[i])) + return -1; + QMutexLocker l(&mtx); // 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; + X_CM.R = r; + X_CM.t[0] = t[0]; + X_CM.t[1] = t[1]; + X_CM.t[2] = t[2]; //qDebug() << "iter:" << i; -- cgit v1.2.3