pt, aruco: fix calibration

2 files changed, 9 insertions, 5 deletions

diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index b86e035c..81cfb895 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -457,10 +457,18 @@ void TrackerControls::toggleCalibrate()
 {
     if (!calib_timer.isActive())
     {
+        s.headpos_x = 0;
+        s.headpos_y = 0;
+        s.headpos_z = 0;
         calibrator.reset();
         calib_timer.start();
     } else {
         cleanupCalib();
+        
+        auto pos = calibrator.get_estimate();
+        s.headpos_x = pos(0);
+        s.headpos_y = pos(1);
+        s.headpos_z = pos(2);
     }
 }
 
@@ -478,10 +486,6 @@ void TrackerControls::update_tracker_calibration()
         cv::Vec3d t;
         tracker->getRT(r, t);
         calibrator.update(r, t);
-        auto pos = calibrator.get_estimate() * .1;
-        s.headpos_x = pos(0);
-        s.headpos_y = pos(1);
-        s.headpos_z = pos(2);
     }
 }
 
diff --git a/ftnoir_tracker_aruco/trans_calib.cpp b/ftnoir_tracker_aruco/trans_calib.cpp
index 369de449..03b25e6b 100644
--- a/ftnoir_tracker_aruco/trans_calib.cpp
+++ b/ftnoir_tracker_aruco/trans_calib.cpp
@@ -40,5 +40,5 @@ void TranslationCalibrator::update(const Matx33d& R_CM_k, const Vec3d& 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]);
 }