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

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';

+}