1 files changed, 9 insertions, 24 deletions
diff --git a/FTNoIR_Tracker_PT/point_tracker.cpp b/FTNoIR_Tracker_PT/point_tracker.cpp
index 263be43a..1df70b17 100644
--- a/FTNoIR_Tracker_PT/point_tracker.cpp
+++ b/FTNoIR_Tracker_PT/point_tracker.cpp
@@ -104,9 +104,6 @@ void PointTracker::reset()
 	init_phase = true;
 	dt_valid = 0;
 	reset_velocities();
-    // assume identity rotation again
-    X_CM.R = cv::Matx33d::eye();
-    X_CM.t = cv::Vec3f();
 }
 
 void PointTracker::reset_velocities()
@@ -116,7 +113,7 @@ void PointTracker::reset_velocities()
 }
 
 
-bool PointTracker::track(const vector<Vec2f>& points, float fov, float dt, int w, int h)
+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;
 
@@ -153,7 +150,7 @@ bool PointTracker::track(const vector<Vec2f>& points, float fov, float dt, int w
 	}
 
     // XXX TODO fov
-    POSIT(fov, w, h);
+    POSIT(fov, w, h, headpos);
 	//qDebug()<<"Number of POSIT iterations: "<<n_iter;
 
 	if (!init_phase)
@@ -168,7 +165,7 @@ bool PointTracker::track(const vector<Vec2f>& points, float fov, float dt, int w
 void PointTracker::predict(float dt)
 {
 	// predict with constant velocity
-    Matx33d R;
+	Matx33f R;
 	Rodrigues(dt*v_r, R);
 	X_CM.R = R*X_CM.R;
 	X_CM.t += dt * v_t;
@@ -235,7 +232,7 @@ bool PointTracker::find_correspondences(const vector<Vec2f>& points)
 
 
 
-void PointTracker::POSIT(float fov, int w, int h)
+void PointTracker::POSIT(float fov, int w, int h, const cv::Vec3f& headpos)
 {
     // XXX hack
     this->fov = fov;
@@ -244,9 +241,9 @@ void PointTracker::POSIT(float fov, int w, int h)
     std::vector<cv::Point3f> obj_points;
     std::vector<cv::Point2f> img_points;
 
-    obj_points.push_back(cv::Vec3f(0, 0, 0));
-    obj_points.push_back(point_model->M01);
-    obj_points.push_back(point_model->M02);
+    obj_points.push_back(headpos);
+    obj_points.push_back(point_model->M01 + headpos);
+    obj_points.push_back(point_model->M02 + headpos);
 
     img_points.push_back(p[0]);
     img_points.push_back(p[1]);
@@ -265,29 +262,17 @@ void PointTracker::POSIT(float fov, int w, int h)
 
     cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1);
 
-    bool lastp = !rvec.empty() && !tvec.empty();
+    bool lastp = !rvec.empty() && !tvec.empty() && !init_phase;
 
     cv::solvePnP(obj_points, img_points, intrinsics, dist_coeffs, rvec, tvec, lastp, cv::ITERATIVE);
 
     cv::Mat rmat;
     cv::Rodrigues(rvec, rmat);
 
-    // finally, find the closer solution
-    cv::Mat expected = cv::Mat(X_CM.R);
-    cv::Mat eye = cv::Mat::eye(3, 3, CV_64FC1);
-    double dev1 = norm(eye - expected * rmat.t());
-    double dev2 = norm(eye - expected * rmat);
-
-    if (dev1 > dev2)
-    {
-        rmat = rmat.t();
-        cv::Rodrigues(rmat, rvec);
-    }
-
 	// apply results
     for (int i = 0; i < 3; i++)
     {
-        X_CM.t[i] = tvec.at<double>(i) * 1e-2;
+        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);
     }