From 0e25fafc470086988ff8b1cd89906438388e3c09 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 12 Jun 2018 17:10:19 +0200 Subject: [PATCH] Fixes display of IMU correction angle. (#1193) When the 'imu_calibration' quaternion has a negative real part, correction angles above 180 degrees were displayed. This fixes the issue. --- .../mapping/internal/optimization/optimization_problem_3d.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc index 3854bf5..3d8d5d7 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc @@ -561,7 +561,8 @@ void OptimizationProblem3D::Solve( LOG(INFO) << "Gravity was: " << trajectory_data.gravity_constant; const auto& imu_calibration = trajectory_data.imu_calibration; LOG(INFO) << "IMU correction was: " - << common::RadToDeg(2. * std::acos(imu_calibration[0])) + << common::RadToDeg(2. * + std::acos(std::abs(imu_calibration[0]))) << " deg (" << imu_calibration[0] << ", " << imu_calibration[1] << ", " << imu_calibration[2] << ", " << imu_calibration[3] << ")";