diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp
index 13b9881b..56622f92 100644
--- a/src/MsgConversion.cpp
+++ b/src/MsgConversion.cpp
@@ -1142,26 +1142,27 @@ bool convertRGBDMsgs(
 						depthHeight,
 						depthMsgs[i]->image.rows).c_str());
 
-		rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, cameraInfoMsgs[i].header.frame_id, cameraInfoMsgs[i].header.stamp, listener, waitForTransform);
+		// use depth's stamp so that geometry is sync to odom, use rgb frame as we assume depth is registered (normally depth msg should have same frame than rgb)
+		rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, imageMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp, listener, waitForTransform);
 		if(localTransform.isNull())
 		{
-			ROS_ERROR("TF of received depth image %d at time %fs is not set!", i, cameraInfoMsgs[i].header.stamp.toSec());
+			ROS_ERROR("TF of received depth image %d at time %fs is not set!", i, depthMsgs[i]->header.stamp.toSec());
 			return false;
 		}
 		// sync with odometry stamp
-		if(!odomFrameId.empty() && odomStamp != cameraInfoMsgs[i].header.stamp)
+		if(!odomFrameId.empty() && odomStamp != depthMsgs[i]->header.stamp)
 		{
 			rtabmap::Transform sensorT = getTransform(
 					frameId,
 					odomFrameId,
 					odomStamp,
-					cameraInfoMsgs[i].header.stamp,
+					depthMsgs[i]->header.stamp,
 					listener,
 					waitForTransform);
 			if(sensorT.isNull())
 			{
 				ROS_WARN("Could not get odometry value for depth image stamp (%fs). Latest odometry "
-						"stamp is %fs. The depth image pose will not be synchronized with odometry.", cameraInfoMsgs[i].header.stamp.toSec(), odomStamp.toSec());
+						"stamp is %fs. The depth image pose will not be synchronized with odometry.", depthMsgs[i]->header.stamp.toSec(), odomStamp.toSec());
 			}
 			else
 			{