From 853f8026bdca98ba7fc221ff0e1d582132243d1c Mon Sep 17 00:00:00 2001 From: Joan Sola <jsola@iri.upc.edu> Date: Thu, 28 Apr 2022 11:50:48 +0200 Subject: [PATCH] Check for right-handed axes specification --- src/subscriber_imu.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp index 5a6bdfa..3e96193 100644 --- a/src/subscriber_imu.cpp +++ b/src/subscriber_imu.cpp @@ -66,9 +66,18 @@ SubscriberImu::SubscriberImu(const std::string& _unique_name, imu_x_axis_ == imu_y_axis_ or imu_x_axis_ == imu_z_axis_ or imu_y_axis_ == imu_z_axis_) - //TODO: add check right-handed axes { - throw(std::runtime_error("SubscriberImu: parameters 'imu_x_axis'm, 'imu_y_axis' and 'imu_z_axis' wrongly specified. Should be +/-1, +/-2 or +/-3 without repetitions.")); + throw(std::runtime_error("SubscriberImu: parameters 'imu_x_axis', 'imu_y_axis' and 'imu_z_axis' wrongly specified. Should be +/-1, +/-2 or +/-3 without repetitions.")); + } + // check for right-handed system + Matrix3d R; + R.setZero(); + R(0,imu_x_axis_) = (imu_x_neg_ ? -1 : 1); + R(1,imu_y_axis_) = (imu_y_neg_ ? -1 : 1); + R(2,imu_z_axis_) = (imu_z_neg_ ? -1 : 1); + if (R.determinant() < 0) + { + throw(std::runtime_error("SubscriberImu: parameters 'imu_x_axis', 'imu_y_axis' and 'imu_z_axis' define a left-handed reference system!")); } } -- GitLab