summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--ftnoir_tracker_pt/point_tracker.cpp22
-rw-r--r--ftnoir_tracker_pt/point_tracker.h5
2 files changed, 15 insertions, 12 deletions
diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp
index afb348c6..640d4528 100644
--- a/ftnoir_tracker_pt/point_tracker.cpp
+++ b/ftnoir_tracker_pt/point_tracker.cpp
@@ -6,6 +6,7 @@
*/
#include "point_tracker.h"
+#include <opencv2/imgproc/imgproc.hpp>
#include <vector>
#include <algorithm>
@@ -50,8 +51,11 @@ PointModel::PointModel(Vec3f M01, Vec3f M02)
float s11 = M01.dot(M01);
float s12 = M01.dot(M02);
float s22 = M02.dot(M02);
- P = 1.0/(s11*s22-s12*s12) * Matx22f(s22, -s12,
- -s12, s11);
+ P = 1.0/(s11*s22-s12*s12) * Matx22f(s22, -s12, -s12, s11);
+
+ cv::Vec6f line;
+ cv::fitLine(std::vector<cv::Point3f>{M01, M02}, line, CV_DIST_L1, 0, 1e-2, 1e-4);
+ d = cv::Vec3f(line[0], line[1], line[3]);
}
#ifdef OPENTRACK_API
@@ -61,12 +65,12 @@ static bool d_vals_sort(const pair<float,int> a, const pair<float,int> b)
}
#endif
-void PointModel::get_d_order(const std::vector<cv::Vec2f>& points, int d_order[]) const
+template<typename vec>
+void PointModel::get_d_order(const std::vector<vec>& points, int d_order[], vec d) const
{
// fit line to orthographically projected points
- Vec2f d(M01[0]-M02[0], M01[1]-M02[1]);
- // get sort indices with respect to d scalar product
vector< pair<float,int> > d_vals;
+ // get sort indices with respect to d scalar product
for (unsigned i = 0; i<points.size(); ++i)
d_vals.push_back(pair<float, int>(d.dot(points[i]), i));
@@ -102,13 +106,9 @@ PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv
// sort points
int point_d_order[PointModel::N_POINTS];
int model_d_order[PointModel::N_POINTS];
- model.get_d_order(points, point_d_order);
+ model.get_d_order(points, point_d_order, cv::Vec2f(points[0][0]-points[1][0], points[0][1]-points[1][1]));
// calculate d and d_order for simple freetrack-like point correspondence
- vector<Vec2f> model_points;
- model_points.push_back(Vec2f(0,0));
- model_points.push_back(Vec2f(model.M01[0], model.M01[1]));
- model_points.push_back(Vec2f(model.M02[0], model.M02[1]));
- model.get_d_order(model_points, model_d_order);
+ model.get_d_order(std::vector<cv::Vec3f>{ Vec3f{0,0,0}, model.M01, model.M02 }, model_d_order, model.d);
// set correspondences
PointOrder p;
for (int i=0; i<PointModel::N_POINTS; ++i)
diff --git a/ftnoir_tracker_pt/point_tracker.h b/ftnoir_tracker_pt/point_tracker.h
index ec852286..c6e73f71 100644
--- a/ftnoir_tracker_pt/point_tracker.h
+++ b/ftnoir_tracker_pt/point_tracker.h
@@ -72,7 +72,10 @@ private:
cv::Vec3f u; // unit vector perpendicular to M01,M02-plane
cv::Matx22f P;
- void get_d_order(const std::vector<cv::Vec2f>& points, int* d_order) const;
+ cv::Vec3f d;
+
+ template<typename vec>
+ void get_d_order(const std::vector<vec>& points, int* d_order, vec d) const;
};
// ----------------------------------------------------------------------------