use levmarq instead of coplanar POSIT implemented in numerically unstable fashion

Signed-off-by: Stanislaw Halik <sthalik@misaki.pl>

1 files changed, 45 insertions, 146 deletions

diff --git a/FTNoIR_Tracker_PT/point_tracker.cpp b/FTNoIR_Tracker_PT/point_tracker.cpp
index dfefdaf8..1df70b17 100644
--- a/FTNoIR_Tracker_PT/point_tracker.cpp
+++ b/FTNoIR_Tracker_PT/point_tracker.cpp
@@ -17,23 +17,6 @@ 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), 

@@ -108,9 +91,11 @@ PointTracker::PointTracker()
 	  dt_reset(1), 

 	  v_t(0,0,0),

 	  v_r(0,0,0), 

-	  dynamic_pose_resolution(true)

+      dynamic_pose_resolution(true),

+      fov(0),

+      _w(0),

+      _h(0)

 {

-	X_CM.t[2] = 1000;	// default position: 1 m away from cam;

 }

 

 void PointTracker::reset()

@@ -128,7 +113,7 @@ void PointTracker::reset_velocities()
 }

 

 

-bool PointTracker::track(const vector<Vec2f>& points, float f, float dt)

+bool PointTracker::track(const vector<Vec2f>& points, float fov, float dt, int w, int h, const cv::Vec3f& headpos)

 {

 	if (!dynamic_pose_resolution) init_phase = true;

 

@@ -140,12 +125,7 @@ bool PointTracker::track(const vector<Vec2f>& points, float f, float dt)
 		reset();

 	}

 

-    bool no_model =

-#ifdef OPENTRACK_API

-        point_model.get() == NULL;

-#else

-        !point_model;

-#endif

+    bool no_model = !point_model;

 

 	// if there is a pointtracking problem, reset the velocities

     if (no_model || points.size() != PointModel::N_POINTS)

@@ -161,7 +141,7 @@ bool PointTracker::track(const vector<Vec2f>& points, float f, float dt)
 		predict(dt_valid);

 

 	// if there is a point correspondence problem something has gone wrong, do a reset

-	if (!find_correspondences(points, f))

+    if (!find_correspondences(points))

 	{

 		//qDebug()<<"Error in finding point correspondences!";

 		X_CM = X_CM_old;	// undo prediction

@@ -169,7 +149,8 @@ bool PointTracker::track(const vector<Vec2f>& points, float f, float dt)
 		return false;

 	}

 

-	int n_iter = POSIT(f);

+    // XXX TODO fov

+    POSIT(fov, w, h, headpos);

 	//qDebug()<<"Number of POSIT iterations: "<<n_iter;

 

 	if (!init_phase)

@@ -198,7 +179,7 @@ void PointTracker::update_velocities(float dt)
 	v_t = (X_CM.t - X_CM_old.t)/dt;

 }

 

-bool PointTracker::find_correspondences(const vector<Vec2f>& points, float f)

+bool PointTracker::find_correspondences(const vector<Vec2f>& points)

 {

 	if (init_phase) {

 		// We do a simple freetrack-like sorting in the init phase...

@@ -215,9 +196,9 @@ bool PointTracker::find_correspondences(const vector<Vec2f>& points, float f)
 	else {

 		// ... otherwise we look at the distance to the projection of the expected model points 

 		// project model points under current pose

-		p_exp[0] = project(Vec3f(0,0,0), f);

-		p_exp[1] = project(point_model->M01, f);

-		p_exp[2] = project(point_model->M02, f);

+        p_exp[0] = project(Vec3f(0,0,0));

+        p_exp[1] = project(point_model->M01);

+        p_exp[2] = project(point_model->M02);

 

 		// set correspondences by minimum distance to projected model point

 		bool point_taken[PointModel::N_POINTS];

@@ -251,130 +232,48 @@ bool PointTracker::find_correspondences(const vector<Vec2f>& points, float f)
 

 

 

-int PointTracker::POSIT(float f)

+void PointTracker::POSIT(float fov, int w, int h, const cv::Vec3f& headpos)

 {

-	// POSIT algorithm for coplanar points as presented in

-	// [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"]

-	// we use the same notation as in the paper here

-

-	// The expected rotation used for resolving the ambiguity in POSIT:

-	// In every iteration step the rotation closer to R_expected is taken 

-	Matx33f R_expected;	

-	if (init_phase)

-		R_expected = Matx33f::eye(); // in the init phase, we want to be close to the default pose = no rotation

-	else 

-		R_expected = X_CM.R; // later we want to be close to the last (predicted) rotation

-	

-	// initial pose = last (predicted) pose

-	Vec3f k;

-    get_row(R_expected, 2, k);

-    float Z0 = init_phase ? 1000 : X_CM.t[2];

-

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

-

-	const int MAX_ITER = 100;

-	const float EPS_THRESHOLD = 1e-4;

-

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

-		}

+    // XXX hack

+    this->fov = fov;

+    _w = w;

+    _h = h;

+    std::vector<cv::Point3f> obj_points;

+    std::vector<cv::Point2f> img_points;

 

-		// construct the two solutions

-		I_1 = I0 + rho*cos(theta)*point_model->u;	

-		I_2 = I0 - rho*cos(theta)*point_model->u;

+    obj_points.push_back(headpos);

+    obj_points.push_back(point_model->M01 + headpos);

+    obj_points.push_back(point_model->M02 + headpos);

 

-		J_1 = J0 + rho*sin(theta)*point_model->u;

-		J_2 = J0 - rho*sin(theta)*point_model->u;

+    img_points.push_back(p[0]);

+    img_points.push_back(p[1]);

+    img_points.push_back(p[2]);

 

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

+    const float HT_PI = 3.1415926535;

 

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

+    const float focal_length_w = 0.5 * w / tan(fov * HT_PI / 180);

+    const float focal_length_h = 0.5 * h / tan(fov * h / w * HT_PI / 180.0);

 

-		// the single translation solution

-		Z0 = norm_const * f;

+    cv::Mat intrinsics = cv::Mat::eye(3, 3, CV_32FC1);

+    intrinsics.at<float> (0, 0) = focal_length_w;

+    intrinsics.at<float> (1, 1) = focal_length_h;

+    intrinsics.at<float> (0, 2) = w/2;

+    intrinsics.at<float> (1, 2) = h/2;

 

-		// 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());

+    cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1);

 

-		if (R_1_deviation < R_2_deviation)

-			R_current = &R_1;

-		else

-			R_current = &R_2;

+    bool lastp = !rvec.empty() && !tvec.empty() && !init_phase;

 

-		get_row(*R_current, 2, k);

+    cv::solvePnP(obj_points, img_points, intrinsics, dist_coeffs, rvec, tvec, lastp, cv::ITERATIVE);

 

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

-	}	

+    cv::Mat rmat;

+    cv::Rodrigues(rvec, rmat);

 

 	// apply results

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

-

-	return i;

-

-	//Rodrigues(X_CM.R, r);

-	//qDebug()<<"iter: "<<i;

-	//qDebug()<<"t: "<<X_CM.t[0]<<' '<<X_CM.t[1]<<' '<<X_CM.t[2];

-	//Vec3f r;

-	//

-	//qDebug()<<"r: "<<r[0]<<' '<<r[1]<<' '<<r[2]<<'\n';

+    for (int i = 0; i < 3; i++)

+    {

+        X_CM.t[i] = tvec.at<double>(i);

+        for (int j = 0; j < 3; j++)

+            X_CM.R(i, j) = rmat.at<double>(i, j);

+    }

 }