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