diff --git a/rtabmap_util/src/nodelets/imu_to_tf.cpp b/rtabmap_util/src/nodelets/imu_to_tf.cpp
index 5de875e8..dc892f5f 100644
--- a/rtabmap_util/src/nodelets/imu_to_tf.cpp
+++ b/rtabmap_util/src/nodelets/imu_to_tf.cpp
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_conversions/MsgConversion.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <tf2/LinearMath/Transform.h>
+#include <tf2/utils.hpp>
 
 namespace rtabmap_util
 {
@@ -81,10 +82,12 @@ void ImuToTF::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
 				return;
 			}
 
-			geometry_msgs::msg::TransformStamped tmp = tfBuffer_->lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp);
+			geometry_msgs::msg::TransformStamped tmp = tfBuffer_->lookupTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp);
 			tf2::Transform tmp_t;
 			tf2::fromMsg(tmp.transform, tmp_t);
-			tf2::Transform t = tmp_t.inverse()*st*tmp_t;
+			tf2::Quaternion q;
+			q.setRPY(0.0,0.0,tf2::getYaw(tmp_t.getRotation()));
+			tf2::Transform t = tf2::Transform(q)*st*tmp_t.inverse(); // base_frame orientation
 			st.setRotation(t.getRotation());
 			childFrameId = baseFrameId_;
 		}