3 files changed, 13 insertions, 16 deletions
diff --git a/FTNoIR_Tracker_PT/point_tracker.cpp b/FTNoIR_Tracker_PT/point_tracker.cpp
index 9f0fb59e..263be43a 100644
--- a/FTNoIR_Tracker_PT/point_tracker.cpp
+++ b/FTNoIR_Tracker_PT/point_tracker.cpp
@@ -105,8 +105,8 @@ void PointTracker::reset()
 	dt_valid = 0;
 	reset_velocities();
     // assume identity rotation again
-    X_CM.R = cv::Matx33f::eye();
-    X_CM.t = cv::Vec3f(0, 0, 1);
+    X_CM.R = cv::Matx33d::eye();
+    X_CM.t = cv::Vec3f();
 }
 
 void PointTracker::reset_velocities()
@@ -168,7 +168,7 @@ bool PointTracker::track(const vector<Vec2f>& points, float fov, float dt, int w
 void PointTracker::predict(float dt)
 {
 	// predict with constant velocity
-    Matx33f R;
+    Matx33d R;
 	Rodrigues(dt*v_r, R);
 	X_CM.R = R*X_CM.R;
 	X_CM.t += dt * v_t;
@@ -273,10 +273,7 @@ void PointTracker::POSIT(float fov, int w, int h)
     cv::Rodrigues(rvec, rmat);
 
     // finally, find the closer solution
-    cv::Mat expected(3, 3, CV_64FC1);
-    for (int i = 0; i < 3; i++)
-        for (int j = 0; j < 3; j++)
-            expected.at<double>(i, j) = X_CM.R(i, j);
+    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);
diff --git a/FTNoIR_Tracker_PT/point_tracker.h b/FTNoIR_Tracker_PT/point_tracker.h
index ac43489e..823d75c0 100644
--- a/FTNoIR_Tracker_PT/point_tracker.h
+++ b/FTNoIR_Tracker_PT/point_tracker.h
@@ -21,11 +21,11 @@
 class FrameTrafo
 {
 public:
-    FrameTrafo() : R(cv::Matx33f::eye()), t(0,0,0) {}
-    FrameTrafo(const cv::Matx33f& R, const cv::Vec3f& t) : R(R),t(t) {}
+    FrameTrafo() : R(cv::Matx33d::eye()), t(0,0,0) {}
+    FrameTrafo(const cv::Matx33d& R, const cv::Vec3d& t) : R(R),t(t) {}
 
-    cv::Matx33f R;
-    cv::Vec3f t;
+    cv::Matx33d R;
+    cv::Vec3d t;
 };
 
 inline FrameTrafo operator*(const FrameTrafo& X, const FrameTrafo& Y)
@@ -33,17 +33,17 @@ inline FrameTrafo operator*(const FrameTrafo& X, const FrameTrafo& Y)
 	return FrameTrafo(X.R*Y.R, X.R*Y.t + X.t);
 }
 
-inline FrameTrafo operator*(const cv::Matx33f& X, const FrameTrafo& Y)
+inline FrameTrafo operator*(const cv::Matx33d& X, const FrameTrafo& Y)
 {
 	return FrameTrafo(X*Y.R, X*Y.t);
 }
 
-inline FrameTrafo operator*(const FrameTrafo& X, const cv::Matx33f& Y)
+inline FrameTrafo operator*(const FrameTrafo& X, const cv::Matx33d& Y)
 {
 	return FrameTrafo(X.R*Y, X.t);
 }
 
-inline cv::Vec3f operator*(const FrameTrafo& X, const cv::Vec3f& v)
+inline cv::Vec3f operator*(const FrameTrafo& X, const cv::Vec3d& v)
 {
 	return X.R*v + X.t;
 }
diff --git a/FTNoIR_Tracker_PT/trans_calib.cpp b/FTNoIR_Tracker_PT/trans_calib.cpp
index b2e0ead0..9b75a1b6 100644
--- a/FTNoIR_Tracker_PT/trans_calib.cpp
+++ b/FTNoIR_Tracker_PT/trans_calib.cpp
@@ -40,5 +40,5 @@ void TranslationCalibrator::update(const Matx33f& R_CM_k, const Vec3f& t_CM_k)
 Vec3f TranslationCalibrator::get_estimate()
 {
 	Vec6f x = P.inv() * y;
-    return Vec3f(-x[0], -x[1], -x[2]);
-}
+	return Vec3f(-x[0], -x[1], -x[2]);
+}
+\ No newline at end of file