From 0383bd9685bce8528c7ae7bd42762037552b6803 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Mon, 15 Dec 2014 21:50:46 +0100 Subject: pt, aruco: fix calibration --- ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 12 ++++++++---- ftnoir_tracker_aruco/trans_calib.cpp | 2 +- 2 files changed, 9 insertions(+), 5 deletions(-) (limited to 'ftnoir_tracker_aruco') 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]); } -- cgit v1.2.3