diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp index 5a6bdfa7581f98124e6d201f60dd6dcbf324364c..3e96193786fd034e017a08c773efe308d0a7e6af 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!")); } }