summaryrefslogtreecommitdiffhomepage
path: root/tracker-pt/point_tracker.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'tracker-pt/point_tracker.cpp')
-rw-r--r--tracker-pt/point_tracker.cpp29
1 files changed, 23 insertions, 6 deletions
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 <vector>
#include <algorithm>
@@ -145,8 +146,8 @@ void PointTracker::track(const std::vector<vec2>& 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;