Check for right-handed axes specification

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!"));
     }
 }