summaryrefslogtreecommitdiffhomepage
path: root/FTNoIR_Tracker_PT/point_tracker.cpp
diff options
context:
space:
mode:
authorPatrick Ruoff <c14-radioactive@19e81ba0-9b1a-49c3-bd6c-561e1906d5fb>2012-09-17 16:32:04 +0000
committerPatrick Ruoff <c14-radioactive@19e81ba0-9b1a-49c3-bd6c-561e1906d5fb>2012-09-17 16:32:04 +0000
commitcc3bc1c6d14de535f87e6601ed562b505d902fce (patch)
tree215fff88ae282a197b80b9db926430380c36c946 /FTNoIR_Tracker_PT/point_tracker.cpp
parentf5df7884cf82b87fce50d3ff68c6a4bfa85064e9 (diff)
added pointtracker
created VC9 solution and project files for available projects git-svn-id: svn+ssh://svn.code.sf.net/p/facetracknoir/code@143 19e81ba0-9b1a-49c3-bd6c-561e1906d5fb
Diffstat (limited to 'FTNoIR_Tracker_PT/point_tracker.cpp')
-rw-r--r--FTNoIR_Tracker_PT/point_tracker.cpp231
1 files changed, 231 insertions, 0 deletions
diff --git a/FTNoIR_Tracker_PT/point_tracker.cpp b/FTNoIR_Tracker_PT/point_tracker.cpp
new file mode 100644
index 00000000..8a32326d
--- /dev/null
+++ b/FTNoIR_Tracker_PT/point_tracker.cpp
@@ -0,0 +1,231 @@
+/* Copyright (c) 2012 Patrick Ruoff
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ */
+
+#include "point_tracker.h"
+
+#include <vector>
+#include <algorithm>
+#include <cmath>
+
+#include <QDebug>
+
+using namespace cv;
+using namespace boost;
+using namespace std;
+
+const float PI = 3.14159265358979323846f;
+
+// ----------------------------------------------------------------------------
+static void get_row(const Matx33f& m, int i, Vec3f& v)
+{
+ v[0] = m(i,0);
+ v[1] = m(i,1);
+ v[2] = m(i,2);
+}
+
+static void set_row(Matx33f& m, int i, const Vec3f& v)
+{
+ m(i,0) = v[0];
+ m(i,1) = v[1];
+ m(i,2) = v[2];
+}
+
+// ----------------------------------------------------------------------------
+PointModel::PointModel(Vec3f M01, Vec3f M02)
+ : M01(M01), M02(M02)
+{
+ // calculate u
+ u = M01.cross(M02);
+ u /= norm(u);
+
+ // calculate projection matrix on M01,M02 plane
+ 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);
+
+ // calculate d and d_order for simple freetrack-like point correspondence
+ vector<Vec2f> points;
+ points.push_back(Vec2f(0,0));
+ points.push_back(Vec2f(M01[0], M01[1]));
+ points.push_back(Vec2f(M02[0], M02[1]));
+ // fit line to orthographically projected points
+ // ERROR: yields wrong results with colinear points?!
+ /*
+ Vec4f line;
+ fitLine(points, line, CV_DIST_L2, 0, 0.01, 0.01);
+ d[0] = line[0]; d[1] = line[1];
+ */
+ // TODO: fix this
+ d = Vec2f(M01[0]-M02[0], M01[1]-M02[1]);
+
+ // sort model points
+ get_d_order(points, d_order);
+}
+
+void PointModel::get_d_order(const std::vector<cv::Vec2f>& points, int d_order[]) const
+{
+ // get sort indices with respect to d scalar product
+ vector< pair<float,int> > d_vals;
+ for (int i = 0; i<points.size(); ++i)
+ d_vals.push_back(pair<float, int>(d.dot(points[i]), i));
+
+ struct
+ {
+ bool operator()(const pair<float, int>& a, const pair<float, int>& b) { return a.first < b.first; }
+ } comp;
+ sort(d_vals.begin(), d_vals.end(), comp);
+
+ for (int i = 0; i<points.size(); ++i)
+ d_order[i] = d_vals[i].second;
+}
+
+
+// ----------------------------------------------------------------------------
+PointTracker::PointTracker()
+{
+ X_CM.t[2] = 1000; // default position: 1 m away from cam
+}
+
+bool PointTracker::track(const vector<Vec2f>& points, float f, float dt)
+{
+ if (!point_model) return false;
+ if (!find_correspondences(points)) return false;
+ POSIT(f);
+ return true;
+}
+
+bool PointTracker::find_correspondences(const vector<Vec2f>& points)
+{
+ if (points.size() != PointModel::N_POINTS) return false;
+
+ // sort points
+ int point_d_order[PointModel::N_POINTS];
+ point_model->get_d_order(points, point_d_order);
+
+ // set correspondences
+ for (int i=0; i<PointModel::N_POINTS; ++i)
+ {
+ p[point_model->d_order[i]] = points[point_d_order[i]];
+ }
+ return true;
+}
+
+void PointTracker::POSIT(float f)
+{
+ float old_epsilon_1 = 0;
+ float old_epsilon_2 = 0;
+ float epsilon_1 = 1;
+ float epsilon_2 = 1;
+
+ Vec3f I0, J0;
+ Vec2f I0_coeff, J0_coeff;
+
+ Vec3f I_1, J_1, I_2, J_2;
+ Matx33f R_1, R_2;
+ Matx33f* R_current;
+
+ //TODO: do extrapolation or reinit here!
+ Vec3f k;
+ get_row(X_CM.R, 2, k);
+ float Z0 = X_CM.t[2];
+ Matx33f R_expected = Matx33f::eye();
+ //Matx33f R_expected = X_CM.R;
+
+ const int MAX_ITER = 100;
+ const float EPS_THRESHOLD = 1e-3;
+
+ int i=1;
+ for (; i<MAX_ITER; ++i)
+ {
+ epsilon_1 = k.dot(point_model->M01)/Z0;
+ epsilon_2 = k.dot(point_model->M02)/Z0;
+
+ // vector of scalar products <I0, M0i> and <J0, M0i>
+ Vec2f I0_M0i(p[1][0]*(1.0 + epsilon_1) - p[0][0],
+ p[2][0]*(1.0 + epsilon_2) - p[0][0]);
+ Vec2f J0_M0i(p[1][1]*(1.0 + epsilon_1) - p[0][1],
+ p[2][1]*(1.0 + epsilon_2) - p[0][1]);
+
+ // construct projection of I, J onto M0i plane: I0 and J0
+ I0_coeff = point_model->P * I0_M0i;
+ J0_coeff = point_model->P * J0_M0i;
+ I0 = I0_coeff[0]*point_model->M01 + I0_coeff[1]*point_model->M02;
+ J0 = J0_coeff[0]*point_model->M01 + J0_coeff[1]*point_model->M02;
+
+ // calculate u component of I, J
+ float II0 = I0.dot(I0);
+ float IJ0 = I0.dot(J0);
+ float JJ0 = J0.dot(J0);
+ float rho, theta;
+ if (JJ0 == II0) {
+ rho = sqrt(abs(2*IJ0));
+ 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) );
+ if (JJ0 - II0 < 0) theta += PI;
+ theta /= 2;
+ }
+
+ // construct the two solutions
+ I_1 = I0 + rho*cos(theta)*point_model->u;
+ I_2 = I0 - rho*cos(theta)*point_model->u;
+
+ J_1 = J0 + rho*sin(theta)*point_model->u;
+ J_2 = J0 - rho*sin(theta)*point_model->u;
+
+ float norm_const = 1.0/norm(I_1); // all have the same norm
+
+ // create rotation matrices
+ I_1 *= norm_const; J_1 *= norm_const;
+ I_2 *= norm_const; J_2 *= norm_const;
+
+ set_row(R_1, 0, I_1);
+ set_row(R_1, 1, J_1);
+ set_row(R_1, 2, I_1.cross(J_1));
+
+ set_row(R_2, 0, I_2);
+ set_row(R_2, 1, J_2);
+ set_row(R_2, 2, I_2.cross(J_2));
+
+ // the single translation solution
+ Z0 = norm_const * f;
+
+ // pick the rotation solution closer to the expected one
+ // in simple metric d(A,B) = || I - A * B^T ||
+ float R_1_deviation = norm(Matx33f::eye() - R_expected * R_1.t());
+ float R_2_deviation = norm(Matx33f::eye() - R_expected * R_2.t());
+
+ if (R_1_deviation < R_2_deviation)
+ R_current = &R_1;
+ else
+ R_current = &R_2;
+
+ get_row(*R_current, 2, k);
+
+ // check for convergence condition
+ if (abs(epsilon_1 - old_epsilon_1) + abs(epsilon_2 - old_epsilon_2) < EPS_THRESHOLD)
+ break;
+ old_epsilon_1 = epsilon_1;
+ old_epsilon_2 = epsilon_2;
+ }
+
+ X_CM.R = *R_current;
+ X_CM.t[0] = p[0][0] * Z0/f;
+ X_CM.t[1] = p[0][1] * Z0/f;
+ X_CM.t[2] = Z0;
+
+ //qDebug()<<"iter: "<<i;
+ //qDebug()<<"t: "<<X_CM.t[0]<<' '<<X_CM.t[1]<<' '<<X_CM.t[2];
+ //Vec3f r;
+ //Rodrigues(X_CM.R, r);
+ //qDebug()<<"r: "<<r[0]<<' '<<r[1]<<' '<<r[2]<<'\n';
+}