pt: reformat posit

1 files changed, 120 insertions, 120 deletions

diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp
index cedf1979..383061c5 100644
--- a/ftnoir_tracker_pt/point_tracker.cpp
+++ b/ftnoir_tracker_pt/point_tracker.cpp
@@ -110,7 +110,7 @@ void PointTracker::track(const std::vector<cv::Vec2f>& points, const PointModel&
         order = find_correspondences(points, model);
 	else
 		order = find_correspondences_previous(points, model, f);
-    
+
     POSIT(model, order, f);
 	init_phase = false;
     t.start();
@@ -142,127 +142,127 @@ PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv
 
 int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float focal_length)
 {
-	// 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 
-        cv::Matx33f R_expected = cv::Matx33f::eye();
-	
-	// initial pose = last (predicted) pose
-        cv::Vec3f k;
-        get_row(R_expected, 2, k);
-        float Z0 = 1000.f;
-
-	float old_epsilon_1 = 0;
-	float old_epsilon_2 = 0;
-	float epsilon_1 = 1;
-	float epsilon_2 = 1;
-
-        cv::Vec3f I0, J0;
-        cv::Vec2f I0_coeff, J0_coeff;
-
-        cv::Vec3f I_1, J_1, I_2, J_2;
-        cv::Matx33f R_1, R_2;
-        cv::Matx33f* R_current;
-
-	const int MAX_ITER = 100;
-	const float EPS_THRESHOLD = 1e-4;
-    
-        const cv::Vec2f* order = order_.points;
-
-	int i=1;
-	for (; i<MAX_ITER; ++i)
-	{
-		epsilon_1 = k.dot(model.M01)/Z0;
-		epsilon_2 = k.dot(model.M02)/Z0;
-
-		// vector of scalar products <I0, M0i> and <J0, M0i>
-                cv::Vec2f I0_M0i(order[1][0]*(1.0 + epsilon_1) - order[0][0],
-                                 order[2][0]*(1.0 + epsilon_2) - order[0][0]);
-                cv::Vec2f J0_M0i(order[1][1]*(1.0 + epsilon_1) - order[0][1],
-                                 order[2][1]*(1.0 + epsilon_2) - order[0][1]);
-
-		// construct projection of I, J onto M0i plane: I0 and J0
-		I0_coeff = model.P * I0_M0i;
-		J0_coeff = model.P * J0_M0i;
-		I0 = I0_coeff[0]*model.M01 + I0_coeff[1]*model.M02;
-		J0 = J0_coeff[0]*model.M01 + J0_coeff[1]*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 = std::sqrt(std::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)*model.u;	
-		I_2 = I0 - rho*cos(theta)*model.u;
-
-		J_1 = J0 + rho*sin(theta)*model.u;
-		J_2 = J0 - rho*sin(theta)*model.u;
-
-		float norm_const = 1.0/cv::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 * focal_length;
-
-		// pick the rotation solution closer to the expected one
-		// in simple metric d(A,B) = || I - A * B^T ||
-                float R_1_deviation = cv::norm(cv::Matx33f::eye() - R_expected * R_1.t());
-                float R_2_deviation = cv::norm(cv::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 (std::abs(epsilon_1 - old_epsilon_1) + std::abs(epsilon_2 - old_epsilon_2) < EPS_THRESHOLD)
-			break;
-		old_epsilon_1 = epsilon_1;
-		old_epsilon_2 = epsilon_2;
-	}	
-
-	// 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;
-
-        //qDebug() << "iter:" << i;
-
-	return i;
+    // 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
+    cv::Matx33f R_expected = cv::Matx33f::eye();
+
+    // initial pose = last (predicted) pose
+    cv::Vec3f k;
+    get_row(R_expected, 2, k);
+    float Z0 = 1000.f;
+
+    float old_epsilon_1 = 0;
+    float old_epsilon_2 = 0;
+    float epsilon_1 = 1;
+    float epsilon_2 = 1;
+
+    cv::Vec3f I0, J0;
+    cv::Vec2f I0_coeff, J0_coeff;
+
+    cv::Vec3f I_1, J_1, I_2, J_2;
+    cv::Matx33f R_1, R_2;
+    cv::Matx33f* R_current;
+
+    const int MAX_ITER = 100;
+    const float EPS_THRESHOLD = 1e-4;
+
+    const cv::Vec2f* order = order_.points;
+
+    int i=1;
+    for (; i<MAX_ITER; ++i)
+    {
+        epsilon_1 = k.dot(model.M01)/Z0;
+        epsilon_2 = k.dot(model.M02)/Z0;
+
+        // vector of scalar products <I0, M0i> and <J0, M0i>
+        cv::Vec2f I0_M0i(order[1][0]*(1.0 + epsilon_1) - order[0][0],
+                order[2][0]*(1.0 + epsilon_2) - order[0][0]);
+        cv::Vec2f J0_M0i(order[1][1]*(1.0 + epsilon_1) - order[0][1],
+                order[2][1]*(1.0 + epsilon_2) - order[0][1]);
+
+        // construct projection of I, J onto M0i plane: I0 and J0
+        I0_coeff = model.P * I0_M0i;
+        J0_coeff = model.P * J0_M0i;
+        I0 = I0_coeff[0]*model.M01 + I0_coeff[1]*model.M02;
+        J0 = J0_coeff[0]*model.M01 + J0_coeff[1]*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 = std::sqrt(std::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)*model.u;
+        I_2 = I0 - rho*cos(theta)*model.u;
+
+        J_1 = J0 + rho*sin(theta)*model.u;
+        J_2 = J0 - rho*sin(theta)*model.u;
+
+        float norm_const = 1.0/cv::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 * focal_length;
+
+        // pick the rotation solution closer to the expected one
+        // in simple metric d(A,B) = || I - A * B^T ||
+        float R_1_deviation = cv::norm(cv::Matx33f::eye() - R_expected * R_1.t());
+        float R_2_deviation = cv::norm(cv::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 (std::abs(epsilon_1 - old_epsilon_1) + std::abs(epsilon_2 - old_epsilon_2) < EPS_THRESHOLD)
+            break;
+        old_epsilon_1 = epsilon_1;
+        old_epsilon_2 = epsilon_2;
+    }
+
+    // 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;
+
+    //qDebug() << "iter:" << i;
+
+    return i;
 }
 
 cv::Vec2f PointTracker::project(const cv::Vec3f& v_M, float f)
 {
-        cv::Vec3f v_C = X_CM * v_M;
-        return cv::Vec2f(f*v_C[0]/v_C[2], f*v_C[1]/v_C[2]);
+    cv::Vec3f v_C = X_CM * v_M;
+    return cv::Vec2f(f*v_C[0]/v_C[2], f*v_C[1]/v_C[2]);
 }