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 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) (limited to 'ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp') 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); } } -- cgit v1.2.3