diff --git a/rtabmap_examples/launch/test_sensor_data.launch b/rtabmap_examples/launch/test_sensor_data.launch
index ee4f111b7..a02b5793a 100644
--- a/rtabmap_examples/launch/test_sensor_data.launch
+++ b/rtabmap_examples/launch/test_sensor_data.launch
@@ -37,7 +37,7 @@
 
 <launch>
 
-  <arg name="pub_sensor_data_image"   default="true"/>
+  <arg name="features_only"           default="false"/>
   <arg name="compression_format"      default=".jpg"/> <!-- ".jpg" or ".png" -->
   <arg name="parallel_compression"    default="true"/>
 
@@ -98,9 +98,8 @@
         <param name="keep_color"      type="bool"   value="true"/>
         <param name="wait_imu_to_init"  type="bool"   value="$(arg wait_imu_to_init)"/>
         <remap from="imu"             to="$(arg imu_topic)"/>
-        <param name="pub_sensor_data_image"                     value="$(arg pub_sensor_data_image)"/>
-        <param name="pub_sensor_data_image_compression_format"  value="$(arg compression_format)"/>
-        <param name="pub_sensor_data_parallel_compression"      value="$(arg parallel_compression)"/>
+        <param name="sensor_data_compression_format"   value="$(arg compression_format)"/>
+        <param name="sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
       </node>
       <!-- RGB-D Odometry -->
       <node unless="$(arg stereo)" pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_odom/rgbd_odometry manager $(arg odom_args)">
@@ -110,9 +109,8 @@
         <param name="keep_color"      type="bool"   value="true"/>
         <param name="wait_imu_to_init"  type="bool"   value="$(arg wait_imu_to_init)"/>
         <remap from="imu"             to="$(arg imu_topic)"/>
-        <param name="pub_sensor_data_image"                     value="$(arg pub_sensor_data_image)"/>
-        <param name="pub_sensor_data_image_compression_format"  value="$(arg compression_format)"/>
-        <param name="pub_sensor_data_parallel_compression"      value="$(arg parallel_compression)"/>
+        <param name="sensor_data_compression_format"  value="$(arg compression_format)"/>
+        <param name="sensor_data_parallel_compression"      value="$(arg parallel_compression)"/>
       </node>
 
       <!-- RTAB-Map -->
@@ -120,7 +118,8 @@
         <param name="frame_id"        type="string" value="$(arg frame_id)"/>
         <param name="subscribe_sensor_data"  type="bool"   value="true"/>
         <param name="approx_sync"     type="bool"   value="false"/>
-        <remap from="sensor_data"     to="odom_sensor_data"/>
+        <remap     if="$(arg features_only)" from="sensor_data"  to="odom_sensor_data/features"/>
+        <remap unless="$(arg features_only)" from="sensor_data"  to="odom_sensor_data/raw"/>
       </node>
     </group>
 
@@ -152,9 +151,8 @@
         <param name="keep_color"      type="bool"   value="true"/>
         <param name="wait_imu_to_init"  type="bool"   value="$(arg wait_imu_to_init)"/>
         <remap from="imu"             to="$(arg imu_topic)"/>
-        <param name="pub_sensor_data_image"                     value="$(arg pub_sensor_data_image)"/>
-        <param name="pub_sensor_data_image_compression_format"  value="$(arg compression_format)"/>
-        <param name="pub_sensor_data_parallel_compression"      value="$(arg parallel_compression)"/>
+        <param name="sensor_data_compression_format"   value="$(arg compression_format)"/>
+        <param name="sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
       </node>
       <!-- RGB-D Odometry -->
       <node unless="$(arg stereo)" pkg="rtabmap_odom" type="rgbd_odometry" name="rgbd_odometry" args="$(arg odom_args)"  output="screen">
@@ -164,9 +162,8 @@
         <param name="keep_color"      type="bool"   value="true"/>
         <param name="wait_imu_to_init"  type="bool"   value="$(arg wait_imu_to_init)"/>
         <remap from="imu"             to="$(arg imu_topic)"/>
-        <param name="pub_sensor_data_image"                     value="$(arg pub_sensor_data_image)"/>
-        <param name="pub_sensor_data_image_compression_format"  value="$(arg compression_format)"/>
-        <param name="pub_sensor_data_parallel_compression"      value="$(arg parallel_compression)"/>
+        <param name="sensor_data_compression_format"   value="$(arg compression_format)"/>
+        <param name="sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
       </node>
 
       <!-- RTAB-Map -->
@@ -174,7 +171,8 @@
         <param name="frame_id"        type="string" value="$(arg frame_id)"/>
         <param name="subscribe_sensor_data"  type="bool"   value="true"/>
         <param name="approx_sync"     type="bool"   value="false"/>
-        <remap from="sensor_data"     to="odom_sensor_data"/>
+        <remap     if="$(arg features_only)" from="sensor_data"  to="odom_sensor_data/features"/>
+        <remap unless="$(arg features_only)" from="sensor_data"  to="odom_sensor_data/raw"/>
       </node>
     </group>
 
@@ -184,7 +182,8 @@
       <param name="subscribe_sensor_data"  type="bool"   value="true"/>
       <param name="subscribe_odom_info"  type="bool"   value="true"/>
       <param name="approx_sync"     type="bool"   value="false"/>
-      <remap from="sensor_data"     to="odom_sensor_data"/>
+      <remap     if="$(arg features_only)" from="sensor_data"  to="odom_sensor_data/features"/>
+      <remap unless="$(arg features_only)" from="sensor_data"  to="odom_sensor_data/raw"/>
     </node>
 
   </group>
diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h
index e41ad85ed..a5684b996 100644
--- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h
+++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h
@@ -124,6 +124,7 @@ class OdometryROS : public nodelet::Nodelet, public rtabmap_sync::SyncDiagnostic
 	ros::Publisher odomLastFrame_;
 	ros::Publisher odomRgbdImagePub_;
 	ros::Publisher odomSensorDataPub_;
+	ros::Publisher odomSensorDataFeaturesPub_;
 	ros::Publisher odomSensorDataCompressedPub_;
 	ros::ServiceServer resetSrv_;
 	ros::ServiceServer resetToPoseSrv_;
@@ -149,7 +150,6 @@ class OdometryROS : public nodelet::Nodelet, public rtabmap_sync::SyncDiagnostic
 	double expectedUpdateRate_;
 	double maxUpdateRate_;
 	double minUpdateRate_;
-	bool pubSensorDataImage_;
 	std::string compressionImgFormat_;
 	bool compressionParallelized_;
 	int odomStrategy_;
diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp
index c630c6fab..c7d33ad2c 100644
--- a/rtabmap_odom/src/OdometryROS.cpp
+++ b/rtabmap_odom/src/OdometryROS.cpp
@@ -83,7 +83,6 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
 	expectedUpdateRate_(0.0),
 	maxUpdateRate_(0.0),
 	minUpdateRate_(0.0),
-	pubSensorDataImage_(true),
 	compressionImgFormat_(".jpg"),
 	compressionParallelized_(true),
 	odomStrategy_(Parameters::defaultOdomStrategy()),
@@ -110,8 +109,9 @@ void OdometryROS::onInit()
 	odomLocalScanMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_scan_map", 1);
 	odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
 	odomRgbdImagePub_ = nh.advertise<rtabmap_msgs::RGBDImage>("odom_rgbd_image", 1);
-	odomSensorDataPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data", 1);
-	odomSensorDataCompressedPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data_compressed", 1);
+	odomSensorDataPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/raw", 1);
+	odomSensorDataFeaturesPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/features", 1);
+	odomSensorDataCompressedPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/compressed", 1);
 
 	Transform initialPose = Transform::getIdentity();
 	std::string initialPoseStr;
@@ -150,9 +150,8 @@ void OdometryROS::onInit()
 	pnh.param("max_update_rate", maxUpdateRate_, maxUpdateRate_);
 	pnh.param("min_update_rate", minUpdateRate_, minUpdateRate_);
 
-	pnh.param("pub_sensor_data_image", pubSensorDataImage_, pubSensorDataImage_);
-	pnh.param("pub_sensor_data_image_compression_format", compressionImgFormat_, compressionImgFormat_);
-	pnh.param("pub_sensor_data_parallel_compression", compressionParallelized_, compressionParallelized_);
+	pnh.param("sensor_data_compression_format", compressionImgFormat_, compressionImgFormat_);
+	pnh.param("sensor_data_parallel_compression", compressionParallelized_, compressionParallelized_);
 
 	pnh.param("wait_imu_to_init", waitIMUToinit_, waitIMUToinit_);
 
@@ -188,9 +187,8 @@ void OdometryROS::onInit()
 	NODELET_INFO("Odometry: max_update_rate        = %f Hz", maxUpdateRate_);
 	NODELET_INFO("Odometry: min_update_rate        = %f Hz", minUpdateRate_);
 	NODELET_INFO("Odometry: wait_imu_to_init       = %s", waitIMUToinit_?"true":"false");
-	NODELET_INFO("Odometry: pub_sensor_data_image  = %s", pubSensorDataImage_?"true":"false");
-	NODELET_INFO("Odometry: pub_sensor_data_image_compression_format = %s", compressionImgFormat_.c_str());
-	NODELET_INFO("Odometry: pub_sensor_data_parallel_compression     = %s", compressionParallelized_?"true":"false");
+	NODELET_INFO("Odometry: sensor_data_compression_format   = %s", compressionImgFormat_.c_str());
+	NODELET_INFO("Odometry: sensor_data_parallel_compression = %s", compressionParallelized_?"true":"false");
 
 	configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
 	if(configPath.size() && configPath.at(0) != '/')
@@ -936,12 +934,26 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
 
 	if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
 	{
-		if(odomSensorDataPub_.getNumSubscribers())
+		if(odomSensorDataPub_.getNumSubscribers() || odomSensorDataFeaturesPub_.getNumSubscribers())
 		{
 			rtabmap_msgs::SensorData msg;
-			rtabmap_conversions::sensorDataToROS(data, msg, frameId_, pubSensorDataImage_);
+			rtabmap_conversions::sensorDataToROS(data, msg, frameId_, odomSensorDataPub_.getNumSubscribers());
 			msg.header.stamp = header.stamp; // use corresponding time stamp to image
-			odomSensorDataPub_.publish(msg);
+			if(odomSensorDataPub_.getNumSubscribers())
+			{
+				odomSensorDataPub_.publish(msg);
+			}
+			if(odomSensorDataFeaturesPub_.getNumSubscribers())
+			{
+				// remove data
+				msg.left = sensor_msgs::Image();
+				msg.right = sensor_msgs::Image();
+				msg.laser_scan = sensor_msgs::PointCloud2();
+				msg.grid_ground.clear();
+				msg.grid_obstacles.clear();
+				msg.grid_empty_cells.clear();
+				odomSensorDataFeaturesPub_.publish(msg);
+			}
 		}
 		if(odomSensorDataCompressedPub_.getNumSubscribers())
 		{
@@ -953,11 +965,11 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
 				rtabmap::CompressionThread ctImage(data.imageRaw(), compressionImgFormat_);
 				rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_);
 				rtabmap::CompressionThread ctLaserScan(data.laserScanRaw().data());
-				if(pubSensorDataImage_ && !data.imageRaw().empty())
+				if(!data.imageRaw().empty())
 				{
 					ctImage.start();
 				}
-				if(pubSensorDataImage_ && !data.depthOrRightRaw().empty())
+				if(!data.depthOrRightRaw().empty())
 				{
 					ctDepth.start();
 				}
@@ -975,11 +987,8 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
 			}
 			else
 			{
-				if(pubSensorDataImage_)
-				{
-					compressedImage = compressImage2(data.imageRaw(), compressionImgFormat_);
-					compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_);
-				}
+				compressedImage = compressImage2(data.imageRaw(), compressionImgFormat_);
+				compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_);
 				compressedScan = compressData2(data.laserScanRaw().data());
 			}
 			if(!compressedImage.empty() && !data.stereoCameraModels().empty())