diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp
index 36bfb0629..3b586c387 100644
--- a/rtabmap_conversions/src/MsgConversion.cpp
+++ b/rtabmap_conversions/src/MsgConversion.cpp
@@ -1105,11 +1105,10 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::SensorData & msg)
 
 	// Image data
 	cv::Mat left, right;
-	cv_bridge::CvImageConstPtr leftRawPtr, rightRawPtr;
 	if(!msg.left.data.empty())
 	{
 		boost::shared_ptr<void const> trackedObject;
-		leftRawPtr = cv_bridge::toCvShare(msg.left, trackedObject);
+		cv_bridge::CvImageConstPtr leftRawPtr = cv_bridge::toCvShare(msg.left, trackedObject);
 
 		if(!(leftRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
 			 leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
@@ -1143,7 +1142,7 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::SensorData & msg)
 	if(!msg.right.data.empty())
 	{
 		boost::shared_ptr<void const> trackedObject;
-		rightRawPtr = cv_bridge::toCvShare(msg.right, trackedObject);
+		cv_bridge::CvImageConstPtr rightRawPtr = cv_bridge::toCvShare(msg.right, trackedObject);
 
 		if(!(rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
 			 rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
@@ -1169,7 +1168,7 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::SensorData & msg)
 			}
 			else
 			{
-				right = cv_bridge::cvtColor(leftRawPtr, "mono8")->image;
+				right = cv_bridge::cvtColor(rightRawPtr, "mono8")->image;
 			}
 		}
 	}
diff --git a/rtabmap_msgs/CMakeLists.txt b/rtabmap_msgs/CMakeLists.txt
index 36c936220..c2c84c1a5 100644
--- a/rtabmap_msgs/CMakeLists.txt
+++ b/rtabmap_msgs/CMakeLists.txt
@@ -26,7 +26,8 @@ add_message_files(
    SensorData.msg
    Link.msg
    OdomInfo.msg
-   Landmark.msg
+   LandmarkDetection.msg
+   LandmarkDetections.msg
    Point2f.msg
    Point3f.msg
    Goal.msg
diff --git a/rtabmap_msgs/msg/Landmark.msg b/rtabmap_msgs/msg/Landmark.msg
deleted file mode 100644
index 19cb4f678..000000000
--- a/rtabmap_msgs/msg/Landmark.msg
+++ /dev/null
@@ -1,13 +0,0 @@
-#class rtabmap::Landmark
-#{
-#   int id_;
-#   float size_;
-#	Transform pose_;
-#	cv::Mat covariance_;
-#}
-
-Header header
-int32 id
-float32 size
-geometry_msgs/Transform pose
-float64[36] covariance
\ No newline at end of file
diff --git a/rtabmap_msgs/msg/LandmarkDetection.msg b/rtabmap_msgs/msg/LandmarkDetection.msg
new file mode 100644
index 000000000..993f537dc
--- /dev/null
+++ b/rtabmap_msgs/msg/LandmarkDetection.msg
@@ -0,0 +1,22 @@
+
+# header.stamp: the timestamp of the detection (e.g. image timestamp)
+# header.frame_id: the base frame of pose (e.g., camera optical frame)
+std_msgs/Header header
+
+# Landmark's frame id
+string landmark_frame_id
+
+# Landmark's unique ID: should be >0
+int32 id
+
+# Size in meters of the landmark/tag (optional, set 0 to not use it).
+float32 size
+
+# Pose of the landmark in header.frame_id frame.
+# If covariance is unknown, keep it as null matrix.
+# rtabmap_slam/rtabmap's landmark_angular_variance and
+# landmark_linear_variance parameters can be used
+# for convenience if covariance is null.
+geometry_msgs/PoseWithCovariance pose
+
+
diff --git a/rtabmap_msgs/msg/LandmarkDetections.msg b/rtabmap_msgs/msg/LandmarkDetections.msg
new file mode 100644
index 000000000..c896d3b99
--- /dev/null
+++ b/rtabmap_msgs/msg/LandmarkDetections.msg
@@ -0,0 +1,8 @@
+
+# header.stamp: the timestamp of the detection (e.g. image timestamp)
+# header.frame_id: the base frame of pose (e.g., camera optical frame)
+std_msgs/Header header
+
+LandmarkDetection[] landmarks
+
+
diff --git a/rtabmap_msgs/msg/SensorData.msg b/rtabmap_msgs/msg/SensorData.msg
index 1290fb89b..d7bdb167a 100644
--- a/rtabmap_msgs/msg/SensorData.msg
+++ b/rtabmap_msgs/msg/SensorData.msg
@@ -57,10 +57,10 @@ EnvSensor[] env_sensors
 sensor_msgs/Imu imu
 geometry_msgs/Transform imu_local_transform
 
-Landmark[] landmarks
+LandmarkDetection[] landmarks
 
 # Ground truth
 geometry_msgs/Pose ground_truth_pose
 
 # GPS
-GPS gps
\ No newline at end of file
+GPS gps
diff --git a/rtabmap_sync/src/CommonDataSubscriber.cpp b/rtabmap_sync/src/CommonDataSubscriber.cpp
index b8421ddbc..0960064d5 100644
--- a/rtabmap_sync/src/CommonDataSubscriber.cpp
+++ b/rtabmap_sync/src/CommonDataSubscriber.cpp
@@ -191,6 +191,11 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbdXOdomDataInfo),
 #endif
 
+        // SensorData
+        SYNC_INIT(sensorDataInfo),
+        SYNC_INIT(sensorDataOdom),
+        SYNC_INIT(sensorDataOdomInfo),
+
 #ifdef RTABMAP_SYNC_MULTI_RGBD
 		// 2 RGBD
 		SYNC_INIT(rgbd2),