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 +- ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp | 6 +----- 3 files changed, 10 insertions(+), 10 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]); } diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp index 98dbfb7b..635e21c5 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp @@ -89,7 +89,7 @@ void TrackerDialog::startstop_trans_calib(bool start) } else { - qDebug()<<"TrackerDialog:: Stoppping translation calibration"; + qDebug()<<"TrackerDialog:: Stopping translation calibration"; trans_calib_running = false; { auto tmp = trans_calib.get_estimate(); @@ -139,10 +139,6 @@ void TrackerDialog::trans_calib_step() Affine X_CM; tracker->pose(&X_CM); trans_calib.update(X_CM.R, X_CM.t); - cv::Vec3f t_MH = trans_calib.get_estimate(); - s.t_MH_x = t_MH[0]; - s.t_MH_y = t_MH[1]; - s.t_MH_z = t_MH[2]; } } -- cgit v1.2.3