imu_transformer: Fix transformation of the orientation of IMU

imu_transformer/CMakeLists.txt

@@ -16,7 +16,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES imu_transformer_nodelet
-  CATKIN_DEPENDS roscpp message_filters nodelet sensor_msgs tf2 tf2_ros geometry_msgs
+  CATKIN_DEPENDS roscpp message_filters nodelet sensor_msgs tf2 tf2_ros geometry_msgs topic_tools
 )
 
 include_directories(
@@ -33,6 +33,11 @@ target_link_libraries(imu_transformer_node ${catkin_LIBRARIES})
 if(CATKIN_ENABLE_TESTING)
   find_package(roslaunch REQUIRED)
   roslaunch_add_file_check(launch)
+
+  find_package(tf2_geometry_msgs REQUIRED)
+  include_directories(${tf2_geometry_msgs_INCLUDE_DIRS})
+  catkin_add_gtest(test_imu_transforms test/test_imu_transforms.cpp)
+  target_link_libraries(test_imu_transforms ${catkin_LIBRARIES} ${tf2_geometry_msgs_LIBRARIES})
 endif()
 
 install(TARGETS imu_transformer_node imu_transformer_nodelet

imu_transformer/include/imu_transformer/tf2_sensor_msgs.h

@@ -108,15 +108,19 @@ namespace tf2
 
     transformCovariance(imu_in.linear_acceleration_covariance, imu_out.linear_acceleration_covariance, r);
 
-    Eigen::Quaternion<double> orientation = r * Eigen::Quaternion<double>(
+    // Orientation expresses attitude of the new frame_id in a fixed world frame. This is why the transform here applies
+    // in the opposite direction.
+    Eigen::Quaternion<double> orientation = Eigen::Quaternion<double>(
         imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z) * r.inverse();
 
     imu_out.orientation.w = orientation.w();
     imu_out.orientation.x = orientation.x();
     imu_out.orientation.y = orientation.y();
     imu_out.orientation.z = orientation.z();
 
-    transformCovariance(imu_in.orientation_covariance, imu_out.orientation_covariance, r);
+    // Orientation is measured relative to the fixed world frame, so it doesn't change when applying a static
+    // transform to the sensor frame.
+    imu_out.orientation_covariance = imu_in.orientation_covariance;
 
   }
 

imu_transformer/package.xml

@@ -29,6 +29,7 @@
   <exec_depend>tf</exec_depend>
 
   <test_depend>roslaunch</test_depend>
+  <test_depend>tf2_geometry_msgs</test_depend>
 
   <export>
     <nodelet plugin="${prefix}/nodelets.xml"/>

imu_transformer/test/test_imu_transforms.cpp

@@ -0,0 +1,275 @@
+/**
+ * \file
+ * \brief 
+ * \author Martin Pecka
+ * SPDX-License-Identifier: BSD-3-Clause
+ * SPDX-FileCopyrightText: Czech Technical University in Prague
+ */
+
+#include "gtest/gtest.h"
+
+#include <imu_transformer/tf2_sensor_msgs.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/MagneticField.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+void compareCovariances(const boost::array<double, 9>& c1, const boost::array<double, 9>& c2)
+{
+  for (size_t i = 0; i < 9; ++i)
+    EXPECT_NEAR(c1[i], c2[i], 1e-6) << "Wrong value at position " << i;
+}
+
+TEST(Covariance, Transform)
+{
+  boost::array<double, 9> in = {1, 0, 0, 0, 2, 0, 0, 0, 3};
+  boost::array<double, 9> expectedOut = {1, 0, 0, 0, 2, 0, 0, 0, 3};
+  boost::array<double, 9> out{};
+  Eigen::Quaterniond q(1, 0, 0, 0);
+  tf2::transformCovariance(in, out, q);
+  compareCovariances(expectedOut, out);
+
+  q = Eigen::Quaterniond(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(0, 0, 1)));
+  expectedOut = {2, 0, 0, 0, 1, 0, 0, 0, 3};
+  tf2::transformCovariance(in, out, q);
+  compareCovariances(expectedOut, out);
+
+  q = q.inverse();
+  tf2::transformCovariance(in, out, q);
+  compareCovariances(expectedOut, out);
+
+  q = Eigen::Quaterniond(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 0, 0)));
+  expectedOut = {1, 0, 0, 0, 3, 0, 0, 0, 2};
+  tf2::transformCovariance(in, out, q);
+  compareCovariances(expectedOut, out);
+
+  q = q.inverse();
+  tf2::transformCovariance(in, out, q);
+  compareCovariances(expectedOut, out);
+
+  q = Eigen::Quaterniond(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(0, 1, 0)));
+  expectedOut = {3, 0, 0, 0, 2, 0, 0, 0, 1};
+  tf2::transformCovariance(in, out, q);
+  compareCovariances(expectedOut, out);
+
+  q = q.inverse();
+  tf2::transformCovariance(in, out, q);
+  compareCovariances(expectedOut, out);
+
+  q = Eigen::Quaterniond(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1)));
+  expectedOut = {2.5, -0.5, 3, 1, 0, -1, -1.5, 2, 0.5};
+  tf2::transformCovariance(in, out, q);
+  compareCovariances(expectedOut, out);
+
+  q = q.inverse();
+  expectedOut = {1.5, -1, 1, 2, 2, -1.5, -0.5, 3, -0.5};
+  tf2::transformCovariance(in, out, q);
+  compareCovariances(expectedOut, out);
+}
+
+TEST(Imu, GetTimestamp)
+{
+  sensor_msgs::Imu msg;
+  msg.header.stamp.sec = 1;
+  msg.header.stamp.nsec = 2;
+
+  EXPECT_EQ(msg.header.stamp, tf2::getTimestamp(msg));
+}
+
+TEST(Imu, GetFrameId)
+{
+  sensor_msgs::Imu msg;
+  msg.header.frame_id = "test";
+
+  EXPECT_EQ(msg.header.frame_id, tf2::getFrameId(msg));
+}
+
+void prepareImuMsg(sensor_msgs::Imu& msg)
+{
+  msg.header.frame_id = "test2";
+  msg.header.stamp.sec = 1;
+  msg.angular_velocity.x = 1;
+  msg.angular_velocity.y = 2;
+  msg.angular_velocity.z = 3;
+  msg.angular_velocity_covariance = {1, 0, 0, 0, 2, 0, 0, 0, 3};
+  msg.linear_acceleration.x = 1;
+  msg.linear_acceleration.y = 2;
+  msg.linear_acceleration.z = 3;
+  msg.linear_acceleration_covariance = {1, 0, 0, 0, 2, 0, 0, 0, 3};
+  msg.orientation.w = 1;
+  msg.orientation_covariance = {1, 0, 0, 0, 2, 0, 0, 0, 3};
+}
+
+void prepareTf(geometry_msgs::TransformStamped& tf)
+{
+  tf.header.frame_id = "test";
+  tf.header.stamp.sec = 1;
+  tf.child_frame_id = "test2";
+  tf.transform.translation.x = 1e6;
+  tf.transform.translation.y = 2e6;
+  tf.transform.translation.z = -3e6;
+  tf.transform.rotation.w = 1;
+}
+
+TEST(Imu, DoTransformYaw)
+{
+  // Q = +90 degrees yaw
+
+  sensor_msgs::Imu msg;
+  prepareImuMsg(msg);
+
+  geometry_msgs::TransformStamped tf;
+  prepareTf(tf);
+
+  tf2::Quaternion q;
+  q.setRPY(0, 0, M_PI_2);
+  tf2::convert(q, tf.transform.rotation);
+
+  sensor_msgs::Imu out;
+  tf2::doTransform(msg, out, tf);
+
+  tf2::Quaternion rot;
+
+  EXPECT_EQ("test", out.header.frame_id);
+  EXPECT_EQ(msg.header.stamp, out.header.stamp);
+  EXPECT_NEAR(-msg.angular_velocity.y, out.angular_velocity.x, 1e-6);
+  EXPECT_NEAR(msg.angular_velocity.x, out.angular_velocity.y, 1e-6);
+  EXPECT_NEAR(msg.angular_velocity.z, out.angular_velocity.z, 1e-6);
+  EXPECT_NEAR(-msg.linear_acceleration.y, out.linear_acceleration.x, 1e-6);
+  EXPECT_NEAR(msg.linear_acceleration.x, out.linear_acceleration.y, 1e-6);
+  EXPECT_NEAR(msg.linear_acceleration.z, out.linear_acceleration.z, 1e-6);
+  // Transforming orientation means expressing the attitude of the new frame in the same world frame (i.e. you have
+  // data in imu frame and want to ask what is the world-referenced orientation of the base_link frame that is attached
+  // to this IMU). This is why the orientation change goes the other way than the transform.
+  tf2::convert(out.orientation, rot);
+  EXPECT_NEAR(0, rot.angleShortestPath(q.inverse()), 1e-6);
+
+  compareCovariances({2, 0, 0, 0, 1, 0, 0, 0, 3}, out.angular_velocity_covariance);
+  compareCovariances({2, 0, 0, 0, 1, 0, 0, 0, 3}, out.linear_acceleration_covariance);
+  // Orientation covariance stays as it is measured regarding the fixed world frame 
+  compareCovariances(msg.orientation_covariance, out.orientation_covariance);
+}
+
+TEST(Imu, DoTransformEnuNed)
+{
+  // Q = ENU->NED transform
+
+  sensor_msgs::Imu msg;
+  prepareImuMsg(msg);
+
+  geometry_msgs::TransformStamped tf;
+  prepareTf(tf);
+
+  tf2::Quaternion q;
+  q.setRPY(M_PI, 0, M_PI_2);
+  tf2::convert(q, tf.transform.rotation);
+
+  sensor_msgs::Imu out;
+  tf2::doTransform(msg, out, tf);
+
+  tf2::Quaternion rot;
+
+  EXPECT_EQ("test", out.header.frame_id);
+  EXPECT_EQ(msg.header.stamp, out.header.stamp);
+  EXPECT_NEAR(msg.angular_velocity.y, out.angular_velocity.x, 1e-6);
+  EXPECT_NEAR(msg.angular_velocity.x, out.angular_velocity.y, 1e-6);
+  EXPECT_NEAR(-msg.angular_velocity.z, out.angular_velocity.z, 1e-6);
+  EXPECT_NEAR(msg.linear_acceleration.y, out.linear_acceleration.x, 1e-6);
+  EXPECT_NEAR(msg.linear_acceleration.x, out.linear_acceleration.y, 1e-6);
+  EXPECT_NEAR(-msg.linear_acceleration.z, out.linear_acceleration.z, 1e-6);
+  // Transforming orientation means expressing the attitude of the new frame in the same world frame (i.e. you have
+  // data in imu frame and want to ask what is the world-referenced orientation of the base_link frame that is attached
+  // to this IMU). This is why the orientation change goes the other way than the transform.
+  tf2::convert(out.orientation, rot);
+  EXPECT_NEAR(0, rot.angleShortestPath(q.inverse()), 1e-6);
+
+  compareCovariances({2, 0, 0, 0, 1, 0, 0, 0, 3}, out.angular_velocity_covariance);
+  compareCovariances({2, 0, 0, 0, 1, 0, 0, 0, 3}, out.linear_acceleration_covariance);
+  // Orientation covariance stays as it is measured regarding the fixed world frame 
+  compareCovariances(msg.orientation_covariance, out.orientation_covariance);
+}
+
+TEST(Mag, GetTimestamp)
+{
+  sensor_msgs::MagneticField msg;
+  msg.header.stamp.sec = 1;
+  msg.header.stamp.nsec = 2;
+
+  EXPECT_EQ(msg.header.stamp, tf2::getTimestamp(msg));
+}
+
+TEST(Mag, GetFrameId)
+{
+  sensor_msgs::MagneticField msg;
+  msg.header.frame_id = "test";
+
+  EXPECT_EQ(msg.header.frame_id, tf2::getFrameId(msg));
+}
+
+void prepareMagMsg(sensor_msgs::MagneticField& msg)
+{
+  msg.header.frame_id = "test2";
+  msg.header.stamp.sec = 1;
+  msg.magnetic_field.x = 1;
+  msg.magnetic_field.y = 2;
+  msg.magnetic_field.z = 3;
+  msg.magnetic_field_covariance = {1, 0, 0, 0, 2, 0, 0, 0, 3};
+}
+
+TEST(Mag, DoTransformYaw)
+{
+  // Q = +90 degrees yaw
+
+  sensor_msgs::MagneticField msg;
+  prepareMagMsg(msg);
+
+  geometry_msgs::TransformStamped tf;
+  prepareTf(tf);
+
+  tf2::Quaternion q;
+  q.setRPY(0, 0, M_PI_2);
+  tf2::convert(q, tf.transform.rotation);
+
+  sensor_msgs::MagneticField out;
+  tf2::doTransform(msg, out, tf);
+
+  EXPECT_EQ("test", out.header.frame_id);
+  EXPECT_EQ(msg.header.stamp, out.header.stamp);
+  EXPECT_NEAR(-msg.magnetic_field.y, out.magnetic_field.x, 1e-6);
+  EXPECT_NEAR(msg.magnetic_field.x, out.magnetic_field.y, 1e-6);
+  EXPECT_NEAR(msg.magnetic_field.z, out.magnetic_field.z, 1e-6);
+
+  compareCovariances({2, 0, 0, 0, 1, 0, 0, 0, 3}, out.magnetic_field_covariance);
+}
+
+TEST(Mag, DoTransformEnuNed)
+{
+  // Q = ENU->NED transform
+
+  sensor_msgs::MagneticField msg;
+  prepareMagMsg(msg);
+
+  geometry_msgs::TransformStamped tf;
+  prepareTf(tf);
+
+  tf2::Quaternion q;
+  q.setRPY(M_PI, 0, M_PI_2);
+  tf2::convert(q, tf.transform.rotation);
+
+  sensor_msgs::MagneticField out;
+  tf2::doTransform(msg, out, tf);
+
+  EXPECT_EQ("test", out.header.frame_id);
+  EXPECT_EQ(msg.header.stamp, out.header.stamp);
+  EXPECT_NEAR(msg.magnetic_field.y, out.magnetic_field.x, 1e-6);
+  EXPECT_NEAR(msg.magnetic_field.x, out.magnetic_field.y, 1e-6);
+  EXPECT_NEAR(-msg.magnetic_field.z, out.magnetic_field.z, 1e-6);
+
+  compareCovariances({2, 0, 0, 0, 1, 0, 0, 0, 3}, out.magnetic_field_covariance);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

imu_transformer/test/test_imu_transforms.launch

@@ -0,0 +1,22 @@
+<launch>
+    <!-- This launch file serves as a visual confirmation that the transformation of IMU orientation works as expected.
+     A world frame is set up, in which imu_link is placed with 90 deg yaw. Then, imu_link_ned is set up which corresponds
+     to the same link but after applying ENU-NED frame conversion. Frame world is set as fixed frame, and both IMU
+     visualizers in the rviz view are set to display orientation regarding the fixed frame (which is the world frame).
+     So in this case, the axes displayed by the rviz IMU visualizers should match the imu_link and imu_link_ned
+     axes visualized from TF. Please note that the frames have non-zero translation in rviz so that it is easier to
+     distinguish them. But the translation has no effect on the computations, since IMU data are only transformed by
+     rotation.
+
+     Another confirmation of the correctness of the implementation can be got when running "tf_echo world imu_link_ned".
+     The orientation in the output of this command should match the orientation that is output in imu_out/data - which
+     should be orientation of the imu_link_ned frame in the world frame. -->
+    <arg name="rviz" default="true" />
+    <node name="tf1" pkg="tf2_ros" type="static_transform_publisher" args="0.7 0 0 1.5708 0 0 world imu_link" />
+    <node name="tf2" pkg="tf2_ros" type="static_transform_publisher" args="0.5 0.5 0 1.5708 0 3.1416 imu_link imu_link_ned" />
+    <node name="transformer" pkg="imu_transformer" type="imu_transformer_node">
+        <param name="target_frame" value="imu_link_ned" />
+    </node>
+    <node name="msg" pkg="rostopic" type="rostopic" args="pub -r10 /imu_in/data sensor_msgs/Imu &quot;{header: {frame_id: 'imu_link'}, orientation: {z: 0.707, w: 0.707}}&quot;" />
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(dirname)/test_imu_transforms.rviz" if="$(arg rviz)" />
+</launch>