From b0916ad14c8826f4bdba87cd17db0065bc6d6b84 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 29 Mar 2025 12:36:43 -0700 Subject: [PATCH] imu_to_tf: fixed fixed_frame to base_frame orientation when IMU's frame z axis is not up accordingly to base frame --- rtabmap_util/src/nodelets/imu_to_tf.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/rtabmap_util/src/nodelets/imu_to_tf.cpp b/rtabmap_util/src/nodelets/imu_to_tf.cpp index 49b22592..42d39713 100644 --- a/rtabmap_util/src/nodelets/imu_to_tf.cpp +++ b/rtabmap_util/src/nodelets/imu_to_tf.cpp @@ -87,8 +87,10 @@ private: } tf::StampedTransform tmp; - tfListener_.lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp, tmp); - tf::Transform t = tmp.inverse()*st*tmp; + tfListener_.lookupTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp, tmp); + tf::Quaternion q; + q.setRPY(0.0,0.0,tf::getYaw(tmp.getRotation())); + tf::Transform t = tf::Transform(q)*st*tmp.inverse(); // base_frame orientation st.setRotation(t.getRotation()); st.child_frame_id_ = baseFrameId_; }