Increased required rtabmap version to 0.20. Added ScanDescriptor and GlobalDescriptor msgs. rtabmap: added subscribe_scan_descriptor argument (updated common subscribers). RGBDImage.msg: added local keypoints, local points, local descriptors and global descriptor members. Info.msg: added wmState member.

CMakeLists.txt

@@ -31,7 +31,7 @@ find_package(find_object_2d)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.19.5 REQUIRED)
+find_package(RTABMap 0.20.0 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 
@@ -46,11 +46,7 @@ IF(WIN32)
 add_compile_options(-bigobj)
 ENDIF(WIN32)
 
-IF(WIN32)
 option(RTABMAP_SYNC_MULTI_RGBD "Build with multi RGBD camera synchronization support"  OFF)
-ELSE()
-option(RTABMAP_SYNC_MULTI_RGBD "Build with multi RGBD camera synchronization support"  ON)
-ENDIF()
 option(RTABMAP_SYNC_USER_DATA "Build with input user data support"  OFF)
 MESSAGE(STATUS "RTABMAP_SYNC_MULTI_RGBD = ${RTABMAP_SYNC_MULTI_RGBD}")
 MESSAGE(STATUS "RTABMAP_SYNC_USER_DATA  = ${RTABMAP_SYNC_USER_DATA}")
@@ -96,6 +92,8 @@ add_message_files(
    FILES
    Info.msg
    KeyPoint.msg
+   GlobalDescriptor.msg
+   ScanDescriptor.msg
    MapData.msg
    MapGraph.msg
    NodeData.msg

include/rtabmap_ros/CommonDataSubscriber.h

@@ -48,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_ros/RGBDImage.h>
 #include <rtabmap_ros/UserData.h>
 #include <rtabmap_ros/OdomInfo.h>
+#include <rtabmap_ros/ScanDescriptor.h>
 #include <rtabmap_ros/CommonDataSubscriberDefines.h>
 
 #include <boost/thread.hpp>
@@ -83,9 +84,13 @@ protected:
 				const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
 				const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
 				const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
-				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::LaserScan& scanMsg,
-				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const sensor_msgs::PointCloud2& scan3dMsg,
-				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) = 0;
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+				const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::GlobalDescriptor>(),
+				const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_ros::KeyPoint> >(),
+				const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_ros::Point3f> >(),
+				const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>()) = 0;
 	virtual void commonStereoCallback(
 				const nav_msgs::OdometryConstPtr & odomMsg,
 				const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -93,15 +98,20 @@ protected:
 				const cv_bridge::CvImageConstPtr& rightImageMsg,
 				const sensor_msgs::CameraInfo& leftCamInfoMsg,
 				const sensor_msgs::CameraInfo& rightCamInfoMsg,
-				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::LaserScan& scanMsg,
-				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const sensor_msgs::PointCloud2& scan3dMsg,
-				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) = 0;
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+				const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::GlobalDescriptor>(),
+				const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_ros::KeyPoint> >(),
+				const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_ros::Point3f> >(),
+				const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>()) = 0;
 	virtual void commonLaserScanCallback(
 				const nav_msgs::OdometryConstPtr & odomMsg,
 				const rtabmap_ros::UserDataConstPtr & userDataMsg,
-				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::LaserScan& scanMsg,
-				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const sensor_msgs::PointCloud2& scan3dMsg,
-				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) = 0;
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+				const rtabmap_ros::GlobalDescriptor & globalDescriptor = rtabmap_ros::GlobalDescriptor()) = 0;
 	virtual void commonOdomCallback(
 				const nav_msgs::OdometryConstPtr & odomMsg,
 				const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -114,9 +124,13 @@ protected:
 				const cv_bridge::CvImageConstPtr & depthMsg,
 				const sensor_msgs::CameraInfo & rgbCameraInfoMsg,
 				const sensor_msgs::CameraInfo & depthCameraInfoMsg,
-				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::LaserScan& scanMsg,
-				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const sensor_msgs::PointCloud2& scan3dMsg,
-				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+				const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::GlobalDescriptor>(),
+				const std::vector<rtabmap_ros::KeyPoint> & localKeyPoints = std::vector<rtabmap_ros::KeyPoint>(),
+				const std::vector<rtabmap_ros::Point3f> & localPoints3d = std::vector<rtabmap_ros::Point3f>(),
+				const cv::Mat & localDescriptors = cv::Mat());
 
 private:
 	void warningLoop();
@@ -128,6 +142,7 @@ private:
 			bool subscribeUserData,
 			bool subscribeScan2d,
 			bool subscribeScan3d,
+			bool subscribeScanDesc,
 			bool subscribeOdomInfo,
 			int queueSize,
 			bool approxSync);
@@ -145,6 +160,7 @@ private:
 			bool subscribeUserData,
 			bool subscribeScan2d,
 			bool subscribeScan3d,
+			bool subscribeScanDesc,
 			bool subscribeOdomInfo,
 			int queueSize,
 			bool approxSync);
@@ -155,6 +171,7 @@ private:
 			bool subscribeUserData,
 			bool subscribeScan2d,
 			bool subscribeScan3d,
+			bool subscribeScanDesc,
 			bool subscribeOdomInfo,
 			int queueSize,
 			bool approxSync);
@@ -166,6 +183,7 @@ private:
 			bool subscribeUserData,
 			bool subscribeScan2d,
 			bool subscribeScan3d,
+			bool subscribeScanDesc,
 			bool subscribeOdomInfo,
 			int queueSize,
 			bool approxSync);
@@ -176,6 +194,7 @@ private:
 			bool subscribeUserData,
 			bool subscribeScan2d,
 			bool subscribeScan3d,
+			bool subscribeScanDesc,
 			bool subscribeOdomInfo,
 			int queueSize,
 			bool approxSync);
@@ -186,6 +205,7 @@ private:
 			bool subscribeUserData,
 			bool subscribeScan2d,
 			bool subscribeScan3d,
+			bool subscribeScanDesc,
 			bool subscribeOdomInfo,
 			int queueSize,
 			bool approxSync); 
@@ -193,7 +213,8 @@ private:
 	void setupScanCallbacks(
 			ros::NodeHandle & nh,
 			ros::NodeHandle & pnh,
-			bool scan2dTopic,
+			bool subscribeScan2d,
+			bool subscribeScanDesc,
 			bool subscribeOdom,
 			bool subscribeUserData,
 			bool subscribeOdomInfo,
@@ -222,6 +243,7 @@ private:
 	bool subscribedToRGBD_;
 	bool subscribedToScan2d_;
 	bool subscribedToScan3d_;
+	bool subscribedToScanDescriptor_;
 	bool subscribedToOdomInfo_;
 	std::string name_;
 
@@ -244,44 +266,54 @@ private:
 	message_filters::Subscriber<rtabmap_ros::UserData> userDataSub_;
 	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
 	message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
+	message_filters::Subscriber<rtabmap_ros::ScanDescriptor> scanDescSub_;
 	message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_;
 
 	ros::Subscriber scan2dSubOnly_;
 	ros::Subscriber scan3dSubOnly_;
+	ros::Subscriber scanDescSubOnly_;
 	ros::Subscriber odomSubOnly_;
 
 	// RGB + Depth
 	DATA_SYNCS3(depth, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
 	DATA_SYNCS4(depthScan2d, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
 	DATA_SYNCS4(depthScan3d, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
+	DATA_SYNCS4(depthScanDesc, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS4(depthInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
 	DATA_SYNCS5(depthScan2dInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS5(depthScan3dInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS5(depthScanDescInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// RGB + Depth + Odom
 	DATA_SYNCS4(depthOdom, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
 	DATA_SYNCS5(depthOdomScan2d, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
 	DATA_SYNCS5(depthOdomScan3d, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
+	DATA_SYNCS5(depthOdomScanDesc, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS5(depthOdomInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
 	DATA_SYNCS6(depthOdomScan2dInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS6(depthOdomScan3dInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS6(depthOdomScanDescInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// RGB + Depth + User Data
 	DATA_SYNCS4(depthData, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
 	DATA_SYNCS5(depthDataScan2d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
 	DATA_SYNCS5(depthDataScan3d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
+	DATA_SYNCS5(depthDataScanDesc, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS5(depthDataInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
 	DATA_SYNCS6(depthDataScan2dInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS6(depthDataScan3dInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS6(depthDataScanDescInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// RGB + Depth + Odom + User Data
 	DATA_SYNCS5(depthOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
 	DATA_SYNCS6(depthOdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
 	DATA_SYNCS6(depthOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
+	DATA_SYNCS6(depthOdomDataScanDesc, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS6(depthOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
 	DATA_SYNCS7(depthOdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS7(depthOdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS7(depthOdomDataScanDescInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 #endif
 
 	// Stereo
@@ -296,68 +328,84 @@ private:
 	DATA_SYNCS2(rgb, sensor_msgs::Image, sensor_msgs::CameraInfo);
 	DATA_SYNCS3(rgbScan2d, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
 	DATA_SYNCS3(rgbScan3d, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
+	DATA_SYNCS3(rgbScanDesc, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS3(rgbInfo, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
 	DATA_SYNCS4(rgbScan2dInfo, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS4(rgbScan3dInfo, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS4(rgbScanDescInfo, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// RGB-only + Odom
 	DATA_SYNCS3(rgbOdom, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::CameraInfo);
 	DATA_SYNCS4(rgbOdomScan2d, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
 	DATA_SYNCS4(rgbOdomScan3d, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
+	DATA_SYNCS4(rgbOdomScanDesc, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS4(rgbOdomInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
 	DATA_SYNCS5(rgbOdomScan2dInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS5(rgbOdomScan3dInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS5(rgbOdomScanDescInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// RGB-only + User Data
 	DATA_SYNCS3(rgbData, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo);
 	DATA_SYNCS4(rgbDataScan2d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
 	DATA_SYNCS4(rgbDataScan3d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
+	DATA_SYNCS4(rgbDataScanDesc, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS4(rgbDataInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
 	DATA_SYNCS5(rgbDataScan2dInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS5(rgbDataScan3dInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS5(rgbDataScanDescInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// RGB-only + Odom + User Data
 	DATA_SYNCS4(rgbOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo);
 	DATA_SYNCS5(rgbOdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
 	DATA_SYNCS5(rgbOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
+	DATA_SYNCS5(rgbOdomDataScanDesc, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS5(rgbOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
 	DATA_SYNCS6(rgbOdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS6(rgbOdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS6(rgbOdomDataScanDescInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 #endif
 
 	// 1 RGBD
 	void rgbdCallback(const rtabmap_ros::RGBDImageConstPtr&);
 	DATA_SYNCS2(rgbdScan2d, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
-	DATA_SYNCS2(rgbdScan3d, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS2(rgbdScan3d, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2)
+	DATA_SYNCS2(rgbdScanDesc, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS2(rgbdInfo, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 	DATA_SYNCS3(rgbdScan2dInfo, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS3(rgbdScan3dInfo, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS3(rgbdScanDescInfo, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// 1 RGBD + Odom
 	DATA_SYNCS2(rgbdOdom, nav_msgs::Odometry, rtabmap_ros::RGBDImage);
 	DATA_SYNCS3(rgbdOdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
 	DATA_SYNCS3(rgbdOdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS3(rgbdOdomScanDesc, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS3(rgbdOdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 	DATA_SYNCS4(rgbdOdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS4(rgbdOdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS4(rgbdOdomScanDescInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// 1 RGBD + User Data
 	DATA_SYNCS2(rgbdData, rtabmap_ros::UserData, rtabmap_ros::RGBDImage);
 	DATA_SYNCS3(rgbdDataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
 	DATA_SYNCS3(rgbdDataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS3(rgbdDataScanDesc, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS3(rgbdDataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 	DATA_SYNCS4(rgbdDataScan2dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS4(rgbdDataScan3dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS4(rgbdDataScanDescInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// 1 RGBD + Odom + User Data
 	DATA_SYNCS3(rgbdOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage);
 	DATA_SYNCS4(rgbdOdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
 	DATA_SYNCS4(rgbdOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS4(rgbdOdomDataScanDesc, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS4(rgbdOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 	DATA_SYNCS5(rgbdOdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS5(rgbdOdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS5(rgbdOdomDataScanDescInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 #endif
 
 #ifdef RTABMAP_SYNC_MULTI_RGBD
@@ -365,129 +413,161 @@ private:
 	DATA_SYNCS2(rgbd2, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
 	DATA_SYNCS3(rgbd2Scan2d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
 	DATA_SYNCS3(rgbd2Scan3d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS3(rgbd2ScanDesc, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS3(rgbd2Info, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 	DATA_SYNCS4(rgbd2Scan2dInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS4(rgbd2Scan3dInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS4(rgbd2ScanDescInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// 2 RGBD + Odom
 	DATA_SYNCS3(rgbd2Odom, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
 	DATA_SYNCS4(rgbd2OdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
 	DATA_SYNCS4(rgbd2OdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS4(rgbd2OdomScanDesc, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS4(rgbd2OdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 	DATA_SYNCS5(rgbd2OdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS5(rgbd2OdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS5(rgbd2OdomScanDescInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// 2 RGBD + User Data
 	DATA_SYNCS3(rgbd2Data, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
 	DATA_SYNCS4(rgbd2DataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
 	DATA_SYNCS4(rgbd2DataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS4(rgbd2DataScanDesc, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS4(rgbd2DataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 	DATA_SYNCS5(rgbd2DataScan2dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS5(rgbd2DataScan3dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS5(rgbd2DataScanDescInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// 2 RGBD + Odom + User Data
 	DATA_SYNCS4(rgbd2OdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
 	DATA_SYNCS5(rgbd2OdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
 	DATA_SYNCS5(rgbd2OdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS5(rgbd2OdomDataScanDesc, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS5(rgbd2OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 	DATA_SYNCS6(rgbd2OdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS6(rgbd2OdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS6(rgbd2OdomDataScanDescInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 #endif
 
 	// 3 RGBD
 	DATA_SYNCS3(rgbd3, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
 	DATA_SYNCS4(rgbd3Scan2d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
 	DATA_SYNCS4(rgbd3Scan3d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS4(rgbd3ScanDesc, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS4(rgbd3Info, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 	DATA_SYNCS5(rgbd3Scan2dInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS5(rgbd3Scan3dInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS5(rgbd3ScanDescInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// 3 RGBD + Odom
 	DATA_SYNCS4(rgbd3Odom, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
 	DATA_SYNCS5(rgbd3OdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
 	DATA_SYNCS5(rgbd3OdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS5(rgbd3OdomScanDesc, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS5(rgbd3OdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 	DATA_SYNCS6(rgbd3OdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS6(rgbd3OdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS6(rgbd3OdomScanDescInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// 3 RGBD + User Data
 	DATA_SYNCS4(rgbd3Data, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
 	DATA_SYNCS5(rgbd3DataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
 	DATA_SYNCS5(rgbd3DataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS5(rgbd3DataScanDesc, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS5(rgbd3DataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 	DATA_SYNCS6(rgbd3DataScan2dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS6(rgbd3DataScan3dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS6(rgbd3DataScanDescInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// 3 RGBD + Odom + User Data
 	DATA_SYNCS5(rgbd3OdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
 	DATA_SYNCS6(rgbd3OdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
 	DATA_SYNCS6(rgbd3OdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS6(rgbd3OdomDataScanDesc, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS6(rgbd3OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 	DATA_SYNCS7(rgbd3OdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS7(rgbd3OdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS7(rgbd3OdomDataScanDescInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 #endif
 
 	// 4 RGBD
 	DATA_SYNCS4(rgbd4, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
 	DATA_SYNCS5(rgbd4Scan2d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
 	DATA_SYNCS5(rgbd4Scan3d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS5(rgbd4ScanDesc, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS5(rgbd4Info, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 	DATA_SYNCS6(rgbd4Scan2dInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS6(rgbd4Scan3dInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS6(rgbd4ScanDescInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// 4 RGBD + Odom
 	DATA_SYNCS5(rgbd4Odom, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
 	DATA_SYNCS6(rgbd4OdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
 	DATA_SYNCS6(rgbd4OdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS6(rgbd4OdomScanDesc, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS6(rgbd4OdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 	DATA_SYNCS7(rgbd4OdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS7(rgbd4OdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS7(rgbd4OdomScanDescInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// 4 RGBD + User Data
 	DATA_SYNCS5(rgbd4Data, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
 	DATA_SYNCS6(rgbd4DataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
 	DATA_SYNCS6(rgbd4DataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS6(rgbd4DataScanDesc, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS6(rgbd4DataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 	DATA_SYNCS7(rgbd4DataScan2dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS7(rgbd4DataScan3dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS7(rgbd4DataScanDescInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// 4 RGBD + Odom + User Data
 	DATA_SYNCS6(rgbd4OdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
 	DATA_SYNCS7(rgbd4OdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
 	DATA_SYNCS7(rgbd4OdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS7(rgbd4OdomDataScanDesc, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS7(rgbd4OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 	DATA_SYNCS8(rgbd4OdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS8(rgbd4OdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS8(rgbd4OdomDataScanDescInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 #endif
 #endif //RTABMAP_SYNC_MULTI_RGBD
 
 	// Scan
 	void scan2dCallback(const sensor_msgs::LaserScanConstPtr&);
 	void scan3dCallback(const sensor_msgs::PointCloud2ConstPtr&);
+	void scanDescCallback(const rtabmap_ros::ScanDescriptorConstPtr&);
 	DATA_SYNCS2(scan2dInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS2(scan3dInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS2(scanDescInfo, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// Scan + Odom
 	DATA_SYNCS2(odomScan2d, nav_msgs::Odometry, sensor_msgs::LaserScan);
 	DATA_SYNCS2(odomScan3d, nav_msgs::Odometry, sensor_msgs::PointCloud2);
+	DATA_SYNCS2(odomScanDesc, nav_msgs::Odometry, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS3(odomScan2dInfo, nav_msgs::Odometry, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS3(odomScan3dInfo, nav_msgs::Odometry, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS3(odomScanDescInfo, nav_msgs::Odometry, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// Scan + User Data
 	DATA_SYNCS2(dataScan2d, rtabmap_ros::UserData, sensor_msgs::LaserScan);
 	DATA_SYNCS2(dataScan3d, rtabmap_ros::UserData, sensor_msgs::PointCloud2);
+	DATA_SYNCS2(dataScanDesc, rtabmap_ros::UserData, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS3(dataScan2dInfo, rtabmap_ros::UserData, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS3(dataScan3dInfo, rtabmap_ros::UserData, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS3(dataScanDescInfo, rtabmap_ros::UserData, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 
 	// Scan + Odom + User Data
 	DATA_SYNCS3(odomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::LaserScan);
 	DATA_SYNCS3(odomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::PointCloud2);
+	DATA_SYNCS3(odomDataScanDesc, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::ScanDescriptor);
 	DATA_SYNCS4(odomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
 	DATA_SYNCS4(odomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
+	DATA_SYNCS4(odomDataScanDescInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::ScanDescriptor, rtabmap_ros::OdomInfo);
 #endif
 
 	// Odom

include/rtabmap_ros/CoreWrapper.h

@@ -103,18 +103,26 @@ private:
 				const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
 				const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
 				const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
-				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::LaserScan& scanMsg,
-				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const sensor_msgs::PointCloud2& scan3dMsg,
-				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+				const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::GlobalDescriptor>(),
+				const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_ros::KeyPoint> >(),
+				const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_ros::Point3f> >(),
+				const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>());
 	void commonDepthCallbackImpl(
 				const std::string & odomFrameId,
 				const rtabmap_ros::UserDataConstPtr & userDataMsg,
 				const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
 				const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
 				const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
-				const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+				const sensor_msgs::LaserScan& scan2dMsg,
-				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const sensor_msgs::PointCloud2& scan3dMsg,
-				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+				const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
+				const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints,
+				const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d,
+				const std::vector<cv::Mat> & localDescriptors);
 	virtual void commonStereoCallback(
 				const nav_msgs::OdometryConstPtr & odomMsg,
 				const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -122,15 +130,20 @@ private:
 				const cv_bridge::CvImageConstPtr& rightImageMsg,
 				const sensor_msgs::CameraInfo& leftCamInfoMsg,
 				const sensor_msgs::CameraInfo& rightCamInfoMsg,
-				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::LaserScan& scanMsg,
-				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const sensor_msgs::PointCloud2& scan3dMsg,
-				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+				const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::GlobalDescriptor>(),
+				const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_ros::KeyPoint> >(),
+				const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_ros::Point3f> >(),
+				const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>());
 	virtual void commonLaserScanCallback(
 				const nav_msgs::OdometryConstPtr & odomMsg,
 				const rtabmap_ros::UserDataConstPtr & userDataMsg,
-				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::LaserScan& scanMsg,
-				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const sensor_msgs::PointCloud2& scan3dMsg,
-				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+				const rtabmap_ros::GlobalDescriptor & globalDescriptor = rtabmap_ros::GlobalDescriptor());
 	virtual void commonOdomCallback(
 			const nav_msgs::OdometryConstPtr & odomMsg,
 			const rtabmap_ros::UserDataConstPtr & userDataMsg,

include/rtabmap_ros/GuiWrapper.h

@@ -73,9 +73,13 @@ private:
 			const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
 			const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
 			const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
-			const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+			const sensor_msgs::LaserScan& scan2dMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+			const sensor_msgs::PointCloud2& scan3dMsg,
-			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+			const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::GlobalDescriptor>(),
+			const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_ros::KeyPoint> >(),
+			const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_ros::Point3f> >(),
+			const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>());
 	virtual void commonStereoCallback(
 			const nav_msgs::OdometryConstPtr & odomMsg,
 			const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -83,15 +87,20 @@ private:
 			const cv_bridge::CvImageConstPtr& rightImageMsg,
 			const sensor_msgs::CameraInfo& leftCamInfoMsg,
 			const sensor_msgs::CameraInfo& rightCamInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+			const sensor_msgs::LaserScan& scan2dMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+			const sensor_msgs::PointCloud2& scan3dMsg,
-			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+			const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::GlobalDescriptor>(),
+			const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_ros::KeyPoint> >(),
+			const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_ros::Point3f> >(),
+			const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>());
 	virtual void commonLaserScanCallback(
 			const nav_msgs::OdometryConstPtr & odomMsg,
 			const rtabmap_ros::UserDataConstPtr & userDataMsg,
-			const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+			const sensor_msgs::LaserScan& scan2dMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+			const sensor_msgs::PointCloud2& scan3dMsg,
-			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+			const rtabmap_ros::GlobalDescriptor & globalDescriptor = rtabmap_ros::GlobalDescriptor());
 
 	virtual void commonOdomCallback(
 			const nav_msgs::OdometryConstPtr & odomMsg,

include/rtabmap_ros/MsgConversion.h

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/Image.h>
+#include <sensor_msgs/PointCloud2.h>
 
 #include <opencv2/opencv.hpp>
 #include <opencv2/features2d/features2d.hpp>
@@ -91,6 +92,12 @@ void keypointToROS(const cv::KeyPoint & kpt, rtabmap_ros::KeyPoint & msg);
 std::vector<cv::KeyPoint> keypointsFromROS(const std::vector<rtabmap_ros::KeyPoint> & msg);
 void keypointsToROS(const std::vector<cv::KeyPoint> & kpts, std::vector<rtabmap_ros::KeyPoint> & msg);
 
+rtabmap::GlobalDescriptor globalDescriptorFromROS(const rtabmap_ros::GlobalDescriptor & msg);
+void globalDescriptorToROS(const rtabmap::GlobalDescriptor & desc, rtabmap_ros::GlobalDescriptor & msg);
+
+std::vector<rtabmap::GlobalDescriptor> globalDescriptorsFromROS(const std::vector<rtabmap_ros::GlobalDescriptor> & msg);
+void globalDescriptorsToROS(const std::vector<rtabmap::GlobalDescriptor> & desc, std::vector<rtabmap_ros::GlobalDescriptor> & msg);
+
 cv::Point2f point2fFromROS(const rtabmap_ros::Point2f & msg);
 void point2fToROS(const cv::Point2f & kpt, rtabmap_ros::Point2f & msg);
 
@@ -98,10 +105,10 @@ std::vector<cv::Point2f> points2fFromROS(const std::vector<rtabmap_ros::Point2f>
 void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ros::Point2f> & msg);
 
 cv::Point3f point3fFromROS(const rtabmap_ros::Point3f & msg);
-void point3fToROS(const cv::Point3f & kpt, rtabmap_ros::Point3f & msg);
+void point3fToROS(const cv::Point3f & pt, rtabmap_ros::Point3f & msg);
 
 std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg);
-void points3fToROS(const std::vector<cv::Point3f> & kpts, std::vector<rtabmap_ros::Point3f> & msg);
+void points3fToROS(const std::vector<cv::Point3f> & pts, std::vector<rtabmap_ros::Point3f> & msg);
 
 rtabmap::CameraModel cameraModelFromROS(
 		const sensor_msgs::CameraInfo & camInfo,
@@ -218,7 +225,7 @@ bool convertStereoMsg(
 		double waitForTransform);
 
 bool convertScanMsg(
-		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const sensor_msgs::LaserScan & scan2dMsg,
 		const std::string & frameId,
 		const std::string & odomFrameId,
 		const ros::Time & odomStamp,
@@ -228,7 +235,7 @@ bool convertScanMsg(
 		bool outputInFrameId = false);
 
 bool convertScan3dMsg(
-		const sensor_msgs::PointCloud2ConstPtr & scan3dMsg,
+		const sensor_msgs::PointCloud2 & scan3dMsg,
 		const std::string & frameId,
 		const std::string & odomFrameId,
 		const ros::Time & odomStamp,

launch/rtabmap.launch

@@ -84,6 +84,8 @@
   <arg name="scan_topic"              default="/scan"/>
   <arg name="subscribe_scan_cloud"    default="false"/>
   <arg name="scan_cloud_topic"        default="/scan_cloud"/>
+  <arg name="subscribe_scan_descriptor" default="false"/>
+  <arg name="scan_descriptor_topic"   default="/scan_descriptor"/>
   <arg name="scan_cloud_max_points"   default="0"/>
   <arg name="scan_cloud_filtered"     default="false"/> <!-- use filtered cloud from icp_odometry for mapping -->
    
@@ -283,6 +285,7 @@
       <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
       <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
       <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
+      <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
       <param name="subscribe_user_data"  type="bool"   value="$(arg subscribe_user_data)"/>
       <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
       <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="true"/>
@@ -319,6 +322,7 @@
       <remap if="$(eval scan_cloud_assembling)" from="scan_cloud" to="assembled_cloud"/>
       <remap if="$(eval scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="odom_filtered_input_scan"/>
       <remap if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
+      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
       <remap from="user_data"              to="$(arg user_data_topic)"/>
       <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
       <remap from="gps/fix"                to="$(arg gps_topic)"/>
@@ -340,6 +344,7 @@
       <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
       <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
       <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
+      <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
       <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
       <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="true"/>
       <param name="frame_id"             type="string" value="$(arg frame_id)"/>
@@ -362,6 +367,7 @@
       <remap from="scan"                   to="$(arg scan_topic)"/>
       <remap     if="$(arg scan_cloud_filtered)" from="scan_cloud" to="odom_filtered_input_scan"/>
       <remap unless="$(arg scan_cloud_filtered)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
+      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
       <remap from="odom"                   to="$(arg odom_topic)"/>
     </node>
   

msg/GlobalDescriptor.msg

@@ -0,0 +1,8 @@
+
+Header header
+
+# compressed global descriptor
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+int32 type
+uint8[] info
+uint8[] data

msg/Info.msg

@@ -14,6 +14,9 @@ geometry_msgs/Transform loopClosureTransform
 ####
 # For statistics...
 ####
+# State (node IDs) of the current Working Memory (including STM)
+int32[] wmState
+
 # std::map<int, float> posterior;
 int32[] posteriorKeys
 float32[] posteriorValues

msg/NodeData.msg

@@ -33,12 +33,13 @@ float32 baseline
 # local transform (/base_link -> /camera_link)
 geometry_msgs/Transform[] localTransform
 
-# compressed 2D laser scan in /base_link frame
+# compressed 2D or 3D laser scan
 # use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
 uint8[] laserScan
 int32 laserScanMaxPts
 float32 laserScanMaxRange
 int32 laserScanFormat
+# local transform (/base_link -> /base_laser)
 geometry_msgs/Transform laserScanLocalTransform
 
 # compressed user data
@@ -54,11 +55,12 @@ float32 grid_cell_size
 Point3f grid_view_point
 
 # std::multimap<wordId, cv::Keypoint>
-# std::multimap<wordId, pcl::PointXYZ>
+# std::multimap<wordId, cv::Point3f>
 int32[] wordIds
 KeyPoint[] wordKpts
-sensor_msgs/PointCloud2 wordPts
+Point3f[] wordPts
-
 # compressed descriptors
 # use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
-uint8[] descriptors
+uint8[] wordDescriptors
+
+GlobalDescriptor[] globalDescriptors

msg/RGBDImage.msg

@@ -1,13 +1,25 @@
 
 Header header
 
-sensor_msgs/CameraInfo rgbCameraInfo
+# For stereo, rgb corresponds to left camera, and depth the right camera.
-sensor_msgs/CameraInfo depthCameraInfo
+
+# camera info
+sensor_msgs/CameraInfo rgb_camera_info
+sensor_msgs/CameraInfo depth_camera_info
 
 # Raw
 sensor_msgs/Image rgb
 sensor_msgs/Image depth
 
 # Compressed
-sensor_msgs/CompressedImage rgbCompressed
+sensor_msgs/CompressedImage rgb_compressed
-sensor_msgs/CompressedImage depthCompressed
+sensor_msgs/CompressedImage depth_compressed
+
+# Local features
+KeyPoint[] key_points
+Point3f[] points
+# compressed descriptors
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] descriptors
+
+GlobalDescriptor global_descriptor

msg/ScanDescriptor.msg

@@ -0,0 +1,8 @@
+
+Header header
+
+# scan or scan_cloud is set
+sensor_msgs/LaserScan scan
+sensor_msgs/PointCloud2 scan_cloud
+
+GlobalDescriptor global_descriptor

src/CommonDataSubscriber.cpp

@@ -41,40 +41,49 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		subscribedToRGBD_(false),
 		subscribedToScan2d_(false),
 		subscribedToScan3d_(false),
+		subscribedToScanDescriptor_(false),
 		subscribedToOdomInfo_(false),
 
 		// RGB + Depth
 		SYNC_INIT(depth),
 		SYNC_INIT(depthScan2d),
 		SYNC_INIT(depthScan3d),
+		SYNC_INIT(depthScanDesc),
 		SYNC_INIT(depthInfo),
 		SYNC_INIT(depthScan2dInfo),
 		SYNC_INIT(depthScan3dInfo),
+		SYNC_INIT(depthScanDescInfo),
 
 		// RGB + Depth + Odom
 		SYNC_INIT(depthOdom),
 		SYNC_INIT(depthOdomScan2d),
 		SYNC_INIT(depthOdomScan3d),
+		SYNC_INIT(depthOdomScanDesc),
 		SYNC_INIT(depthOdomInfo),
 		SYNC_INIT(depthOdomScan2dInfo),
 		SYNC_INIT(depthOdomScan3dInfo),
+		SYNC_INIT(depthOdomScanDescInfo),
 
 #ifdef RTABMAP_SYNC_USER_DATA
 		// RGB + Depth + User Data
 		SYNC_INIT(depthData),
 		SYNC_INIT(depthDataScan2d),
 		SYNC_INIT(depthDataScan3d),
+		SYNC_INIT(depthDataScanDesc),
 		SYNC_INIT(depthDataInfo),
 		SYNC_INIT(depthDataScan2dInfo),
 		SYNC_INIT(depthDataScan3dInfo),
+		SYNC_INIT(depthDataScanDescInfo),
 
 		// RGB + Depth + Odom + User Data
 		SYNC_INIT(depthOdomData),
 		SYNC_INIT(depthOdomDataScan2d),
 		SYNC_INIT(depthOdomDataScan3d),
+		SYNC_INIT(depthOdomDataScanDesc),
 		SYNC_INIT(depthOdomDataInfo),
 		SYNC_INIT(depthOdomDataScan2dInfo),
 		SYNC_INIT(depthOdomDataScan3dInfo),
+		SYNC_INIT(depthOdomDataScanDescInfo),
 #endif
 
 		// Stereo
@@ -89,66 +98,82 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgb),
 		SYNC_INIT(rgbScan2d),
 		SYNC_INIT(rgbScan3d),
+		SYNC_INIT(rgbScanDesc),
 		SYNC_INIT(rgbInfo),
 		SYNC_INIT(rgbScan2dInfo),
 		SYNC_INIT(rgbScan3dInfo),
+		SYNC_INIT(rgbScanDescInfo),
 
 		// RGB-only + Odom
 		SYNC_INIT(rgbOdom),
 		SYNC_INIT(rgbOdomScan2d),
 		SYNC_INIT(rgbOdomScan3d),
+		SYNC_INIT(rgbOdomScanDesc),
 		SYNC_INIT(rgbOdomInfo),
 		SYNC_INIT(rgbOdomScan2dInfo),
 		SYNC_INIT(rgbOdomScan3dInfo),
+		SYNC_INIT(rgbOdomScanDescInfo),
 
 #ifdef RTABMAP_SYNC_USER_DATA
 		// RGB-only + User Data
 		SYNC_INIT(rgbData),
 		SYNC_INIT(rgbDataScan2d),
 		SYNC_INIT(rgbDataScan3d),
+		SYNC_INIT(rgbDataScanDesc),
 		SYNC_INIT(rgbDataInfo),
 		SYNC_INIT(rgbDataScan2dInfo),
 		SYNC_INIT(rgbDataScan3dInfo),
+		SYNC_INIT(rgbDataScanDescInfo),
 
 		// RGB-only + Odom + User Data
 		SYNC_INIT(rgbOdomData),
 		SYNC_INIT(rgbOdomDataScan2d),
 		SYNC_INIT(rgbOdomDataScan3d),
+		SYNC_INIT(rgbOdomDataScanDesc),
 		SYNC_INIT(rgbOdomDataInfo),
 		SYNC_INIT(rgbOdomDataScan2dInfo),
 		SYNC_INIT(rgbOdomDataScan3dInfo),
+		SYNC_INIT(rgbOdomDataScanDescInfo),
 #endif
 		// 1 RGBD
 		SYNC_INIT(rgbdScan2d),
 		SYNC_INIT(rgbdScan3d),
+		SYNC_INIT(rgbdScanDesc),
 		SYNC_INIT(rgbdInfo),
 		SYNC_INIT(rgbdScan2dInfo),
 		SYNC_INIT(rgbdScan3dInfo),
+		SYNC_INIT(rgbdScanDescInfo),
 
 		// 1 RGBD + Odom
 		SYNC_INIT(rgbdOdom),
 		SYNC_INIT(rgbdOdomScan2d),
 		SYNC_INIT(rgbdOdomScan3d),
+		SYNC_INIT(rgbdOdomScanDesc),
 		SYNC_INIT(rgbdOdomInfo),
 		SYNC_INIT(rgbdOdomScan2dInfo),
 		SYNC_INIT(rgbdOdomScan3dInfo),
+		SYNC_INIT(rgbdOdomScanDescInfo),
 
 #ifdef RTABMAP_SYNC_USER_DATA
 		// 1 RGBD + User Data
 		SYNC_INIT(rgbdData),
 		SYNC_INIT(rgbdDataScan2d),
 		SYNC_INIT(rgbdDataScan3d),
+		SYNC_INIT(rgbdDataScanDesc),
 		SYNC_INIT(rgbdDataInfo),
 		SYNC_INIT(rgbdDataScan2dInfo),
 		SYNC_INIT(rgbdDataScan3dInfo),
+		SYNC_INIT(rgbdDataScanDescInfo),
 
 		// 1 RGBD + Odom + User Data
 		SYNC_INIT(rgbdOdomData),
 		SYNC_INIT(rgbdOdomDataScan2d),
 		SYNC_INIT(rgbdOdomDataScan3d),
+		SYNC_INIT(rgbdOdomDataScanDesc),
 		SYNC_INIT(rgbdOdomDataInfo),
 		SYNC_INIT(rgbdOdomDataScan2dInfo),
 		SYNC_INIT(rgbdOdomDataScan3dInfo),
+		SYNC_INIT(rgbdOdomDataScanDescInfo),
 #endif
 
 #ifdef RTABMAP_SYNC_MULTI_RGBD
@@ -156,127 +181,158 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbd2),
 		SYNC_INIT(rgbd2Scan2d),
 		SYNC_INIT(rgbd2Scan3d),
+		SYNC_INIT(rgbd2ScanDesc),
 		SYNC_INIT(rgbd2Info),
 		SYNC_INIT(rgbd2Scan2dInfo),
 		SYNC_INIT(rgbd2Scan3dInfo),
+		SYNC_INIT(rgbd2ScanDescInfo),
 
 		// 2 RGBD + Odom
 		SYNC_INIT(rgbd2Odom),
 		SYNC_INIT(rgbd2OdomScan2d),
 		SYNC_INIT(rgbd2OdomScan3d),
+		SYNC_INIT(rgbd2OdomScanDesc),
 		SYNC_INIT(rgbd2OdomInfo),
 		SYNC_INIT(rgbd2OdomScan2dInfo),
 		SYNC_INIT(rgbd2OdomScan3dInfo),
+		SYNC_INIT(rgbd2OdomScanDescInfo),
 
 #ifdef RTABMAP_SYNC_USER_DATA
 		// 2 RGBD + User Data
 		SYNC_INIT(rgbd2Data),
 		SYNC_INIT(rgbd2DataScan2d),
 		SYNC_INIT(rgbd2DataScan3d),
+		SYNC_INIT(rgbd2DataScanDesc),
 		SYNC_INIT(rgbd2DataInfo),
 		SYNC_INIT(rgbd2DataScan2dInfo),
 		SYNC_INIT(rgbd2DataScan3dInfo),
+		SYNC_INIT(rgbd2DataScanDescInfo),
 
 		// 2 RGBD + Odom + User Data
 		SYNC_INIT(rgbd2OdomData),
 		SYNC_INIT(rgbd2OdomDataScan2d),
 		SYNC_INIT(rgbd2OdomDataScan3d),
+		SYNC_INIT(rgbd2OdomDataScanDesc),
 		SYNC_INIT(rgbd2OdomDataInfo),
 		SYNC_INIT(rgbd2OdomDataScan2dInfo),
 		SYNC_INIT(rgbd2OdomDataScan3dInfo),
+		SYNC_INIT(rgbd2OdomDataScanDescInfo),
 #endif
 
 		// 3 RGBD
 		SYNC_INIT(rgbd3),
 		SYNC_INIT(rgbd3Scan2d),
 		SYNC_INIT(rgbd3Scan3d),
+		SYNC_INIT(rgbd3ScanDesc),
 		SYNC_INIT(rgbd3Info),
 		SYNC_INIT(rgbd3Scan2dInfo),
 		SYNC_INIT(rgbd3Scan3dInfo),
+		SYNC_INIT(rgbd3ScanDescInfo),
 
 		// 3 RGBD + Odom
 		SYNC_INIT(rgbd3Odom),
 		SYNC_INIT(rgbd3OdomScan2d),
 		SYNC_INIT(rgbd3OdomScan3d),
+		SYNC_INIT(rgbd3OdomScanDesc),
 		SYNC_INIT(rgbd3OdomInfo),
 		SYNC_INIT(rgbd3OdomScan2dInfo),
 		SYNC_INIT(rgbd3OdomScan3dInfo),
+		SYNC_INIT(rgbd3OdomScanDescInfo),
 
 #ifdef RTABMAP_SYNC_USER_DATA
 		// 3 RGBD + User Data
 		SYNC_INIT(rgbd3Data),
 		SYNC_INIT(rgbd3DataScan2d),
 		SYNC_INIT(rgbd3DataScan3d),
+		SYNC_INIT(rgbd3DataScanDesc),
 		SYNC_INIT(rgbd3DataInfo),
 		SYNC_INIT(rgbd3DataScan2dInfo),
 		SYNC_INIT(rgbd3DataScan3dInfo),
+		SYNC_INIT(rgbd3DataScanDescInfo),
 
 		// 3 RGBD + Odom + User Data
 		SYNC_INIT(rgbd3OdomData),
 		SYNC_INIT(rgbd3OdomDataScan2d),
 		SYNC_INIT(rgbd3OdomDataScan3d),
+		SYNC_INIT(rgbd3OdomDataScanDesc),
 		SYNC_INIT(rgbd3OdomDataInfo),
 		SYNC_INIT(rgbd3OdomDataScan2dInfo),
 		SYNC_INIT(rgbd3OdomDataScan3dInfo),
+		SYNC_INIT(rgbd3OdomDataScanDescInfo),
 #endif
 
 		// 4 RGBD
 		SYNC_INIT(rgbd4),
 		SYNC_INIT(rgbd4Scan2d),
 		SYNC_INIT(rgbd4Scan3d),
+		SYNC_INIT(rgbd4ScanDesc),
 		SYNC_INIT(rgbd4Info),
 		SYNC_INIT(rgbd4Scan2dInfo),
 		SYNC_INIT(rgbd4Scan3dInfo),
+		SYNC_INIT(rgbd4ScanDescInfo),
 
 		// 4 RGBD + Odom
 		SYNC_INIT(rgbd4Odom),
 		SYNC_INIT(rgbd4OdomScan2d),
 		SYNC_INIT(rgbd4OdomScan3d),
+		SYNC_INIT(rgbd4OdomScanDesc),
 		SYNC_INIT(rgbd4OdomInfo),
 		SYNC_INIT(rgbd4OdomScan2dInfo),
 		SYNC_INIT(rgbd4OdomScan3dInfo),
+		SYNC_INIT(rgbd4OdomScanDescInfo),
 
 #ifdef RTABMAP_SYNC_USER_DATA
 		// 4 RGBD + User Data
 		SYNC_INIT(rgbd4Data),
 		SYNC_INIT(rgbd4DataScan2d),
 		SYNC_INIT(rgbd4DataScan3d),
+		SYNC_INIT(rgbd4DataScanDesc),
 		SYNC_INIT(rgbd4DataInfo),
 		SYNC_INIT(rgbd4DataScan2dInfo),
 		SYNC_INIT(rgbd4DataScan3dInfo),
+		SYNC_INIT(rgbd4DataScanDescInfo),
 
 		// 4 RGBD + Odom + User Data
 		SYNC_INIT(rgbd4OdomData),
 		SYNC_INIT(rgbd4OdomDataScan2d),
 		SYNC_INIT(rgbd4OdomDataScan3d),
+		SYNC_INIT(rgbd4OdomDataScanDesc),
 		SYNC_INIT(rgbd4OdomDataInfo),
 		SYNC_INIT(rgbd4OdomDataScan2dInfo),
 		SYNC_INIT(rgbd4OdomDataScan3dInfo),
+		SYNC_INIT(rgbd4OdomDataScanDescInfo),
 #endif
 #endif // RTABMAP_SYNC_MULTI_RGBD
 
 		// Scan
 		SYNC_INIT(scan2dInfo),
 		SYNC_INIT(scan3dInfo),
+		SYNC_INIT(scanDescInfo),
 
 		// Scan + Odom
 		SYNC_INIT(odomScan2d),
 		SYNC_INIT(odomScan3d),
+		SYNC_INIT(odomScanDesc),
 		SYNC_INIT(odomScan2dInfo),
 		SYNC_INIT(odomScan3dInfo),
+		SYNC_INIT(odomScanDescInfo),
 
 #ifdef RTABMAP_SYNC_USER_DATA
 		// Scan + User Data
 		SYNC_INIT(dataScan2d),
 		SYNC_INIT(dataScan3d),
+		SYNC_INIT(dataScanDesc),
 		SYNC_INIT(dataScan2dInfo),
 		SYNC_INIT(dataScan3dInfo),
+		SYNC_INIT(dataScanDescInfo),
 
 		// Scan + Odom + User Data
 		SYNC_INIT(odomDataScan2d),
 		SYNC_INIT(odomDataScan3d),
+		SYNC_INIT(odomDataScanDesc),
 		SYNC_INIT(odomDataScan2dInfo),
 		SYNC_INIT(odomDataScan3dInfo),
+		SYNC_INIT(odomDataScanDescInfo),
 #endif
 
 		// Odom
@@ -298,6 +354,7 @@ void CommonDataSubscriber::setupCallbacks(
 {
 	bool subscribeScan2d = false;
 	bool subscribeScan3d = false;
+	bool subscribeScanDesc = false;
 	bool subscribeOdomInfo = false;
 	bool subscribeUserData = false;
 	bool subscribeOdom = true;
@@ -313,6 +370,7 @@ void CommonDataSubscriber::setupCallbacks(
 	pnh.param("subscribe_rgb",       subscribedToRGB_, subscribedToRGB_);
 	pnh.param("subscribe_scan",      subscribeScan2d, subscribeScan2d);
 	pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d);
+	pnh.param("subscribe_scan_descriptor", subscribeScanDesc, subscribeScanDesc);
 	pnh.param("subscribe_stereo",    subscribedToStereo_, subscribedToStereo_);
 	pnh.param("subscribe_rgbd",      subscribedToRGBD_, subscribedToRGBD_);
 	pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
@@ -359,7 +417,17 @@ void CommonDataSubscriber::setupCallbacks(
 		ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
 		subscribeScan3d = false;
 	}
-	if(subscribeScan2d || subscribeScan3d)
+	if(subscribeScan2d && subscribeScanDesc)
+	{
+		ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan is set to false.");
+		subscribeScan2d = false;
+	}
+	if(subscribeScan3d && subscribeScanDesc)
+	{
+		ROS_WARN("rtabmap: Parameters subscribe_scan_cloud and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
+		subscribeScan3d = false;
+	}
+	if(subscribeScan2d || subscribeScan3d || subscribeScanDesc)
 	{
 		if(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_ && !subscribedToRGB_)
 		{
@@ -404,6 +472,7 @@ void CommonDataSubscriber::setupCallbacks(
 	ROS_INFO("%s: subscribe_user_data = %s", name.c_str(), subscribeUserData?"true":"false");
 	ROS_INFO("%s: subscribe_scan = %s", name.c_str(), subscribeScan2d?"true":"false");
 	ROS_INFO("%s: subscribe_scan_cloud = %s", name.c_str(), subscribeScan3d?"true":"false");
+	ROS_INFO("%s: subscribe_scan_descriptor = %s", name.c_str(), subscribeScanDesc?"true":"false");
 	ROS_INFO("%s: queue_size    = %d", name.c_str(), queueSize_);
 	ROS_INFO("%s: approx_sync   = %s", name.c_str(), approxSync_?"true":"false");
 
@@ -417,6 +486,7 @@ void CommonDataSubscriber::setupCallbacks(
 				subscribeUserData,
 				subscribeScan2d,
 				subscribeScan3d,
+				subscribeScanDesc,
 				subscribeOdomInfo,
 				queueSize_,
 				approxSync_);
@@ -440,6 +510,7 @@ void CommonDataSubscriber::setupCallbacks(
 				subscribeUserData,
 				subscribeScan2d,
 				subscribeScan3d,
+				subscribeScanDesc,
 				subscribeOdomInfo,
 				queueSize_,
 				approxSync_);
@@ -456,6 +527,7 @@ void CommonDataSubscriber::setupCallbacks(
 					subscribeUserData,
 					subscribeScan2d,
 					subscribeScan3d,
+					subscribeScanDesc,
 					subscribeOdomInfo,
 					queueSize_,
 					approxSync_);
@@ -469,6 +541,7 @@ void CommonDataSubscriber::setupCallbacks(
 					subscribeUserData,
 					subscribeScan2d,
 					subscribeScan3d,
+					subscribeScanDesc,
 					subscribeOdomInfo,
 					queueSize_,
 					approxSync_);
@@ -482,6 +555,7 @@ void CommonDataSubscriber::setupCallbacks(
 					subscribeUserData,
 					subscribeScan2d,
 					subscribeScan3d,
+					subscribeScanDesc,
 					subscribeOdomInfo,
 					queueSize_,
 					approxSync_);
@@ -501,17 +575,19 @@ void CommonDataSubscriber::setupCallbacks(
 					subscribeUserData,
 					subscribeScan2d,
 					subscribeScan3d,
+					subscribeScanDesc,
 					subscribeOdomInfo,
 					queueSize_,
 					approxSync_);
 		}
 	}
-	else if(subscribeScan2d || subscribeScan3d)
+	else if(subscribeScan2d || subscribeScan3d || subscribeScanDesc)
 	{
 		setupScanCallbacks(
 					nh,
 					pnh,
 					subscribeScan2d,
+					subscribeScanDesc,
 					subscribedToOdom_,
 					subscribeUserData,
 					subscribeOdomInfo,
@@ -529,7 +605,7 @@ void CommonDataSubscriber::setupCallbacks(
 					approxSync_);
 	}
 
-	if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToRGB_ || subscribedToOdom_)
+	if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_ || subscribedToRGB_ || subscribedToOdom_)
 	{
 		warningThread_ = new boost::thread(boost::bind(&CommonDataSubscriber::warningLoop, this));
 		ROS_INFO("%s", subscribedTopicsMsg_.c_str());
@@ -549,34 +625,42 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(depth);
 	SYNC_DEL(depthScan2d);
 	SYNC_DEL(depthScan3d);
+	SYNC_DEL(depthScanDesc);
 	SYNC_DEL(depthInfo);
 	SYNC_DEL(depthScan2dInfo);
 	SYNC_DEL(depthScan3dInfo);
+	SYNC_DEL(depthScanDescInfo);
 
 	// RGB + Depth + Odom
 	SYNC_DEL(depthOdom);
 	SYNC_DEL(depthOdomScan2d);
 	SYNC_DEL(depthOdomScan3d);
+	SYNC_DEL(depthOdomScanDesc);
 	SYNC_DEL(depthOdomInfo);
 	SYNC_DEL(depthOdomScan2dInfo);
 	SYNC_DEL(depthOdomScan3dInfo);
+	SYNC_DEL(depthOdomScanDescInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// RGB + Depth + User Data
 	SYNC_DEL(depthData);
 	SYNC_DEL(depthDataScan2d);
 	SYNC_DEL(depthDataScan3d);
+	SYNC_DEL(depthDataScanDesc);
 	SYNC_DEL(depthDataInfo);
 	SYNC_DEL(depthDataScan2dInfo);
 	SYNC_DEL(depthDataScan3dInfo);
+	SYNC_DEL(depthDataScanDescInfo);
 
 	// RGB + Depth + Odom + User Data
 	SYNC_DEL(depthOdomData);
 	SYNC_DEL(depthOdomDataScan2d);
 	SYNC_DEL(depthOdomDataScan3d);
+	SYNC_DEL(depthOdomDataScanDesc);
 	SYNC_DEL(depthOdomDataInfo);
 	SYNC_DEL(depthOdomDataScan2dInfo);
 	SYNC_DEL(depthOdomDataScan3dInfo);
+	SYNC_DEL(depthOdomDataScanDescInfo);
 #endif
 
 	// Stereo
@@ -591,67 +675,83 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgb);
 	SYNC_DEL(rgbScan2d);
 	SYNC_DEL(rgbScan3d);
+	SYNC_DEL(rgbScanDesc);
 	SYNC_DEL(rgbInfo);
 	SYNC_DEL(rgbScan2dInfo);
 	SYNC_DEL(rgbScan3dInfo);
+	SYNC_DEL(rgbScanDescInfo);
 
 	// RGB-only + Odom
 	SYNC_DEL(rgbOdom);
 	SYNC_DEL(rgbOdomScan2d);
 	SYNC_DEL(rgbOdomScan3d);
+	SYNC_DEL(rgbOdomScanDesc);
 	SYNC_DEL(rgbOdomInfo);
 	SYNC_DEL(rgbOdomScan2dInfo);
 	SYNC_DEL(rgbOdomScan3dInfo);
+	SYNC_DEL(rgbOdomScanDescInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// RGB-only + User Data
 	SYNC_DEL(rgbData);
 	SYNC_DEL(rgbDataScan2d);
 	SYNC_DEL(rgbDataScan3d);
+	SYNC_DEL(rgbDataScanDesc);
 	SYNC_DEL(rgbDataInfo);
 	SYNC_DEL(rgbDataScan2dInfo);
 	SYNC_DEL(rgbDataScan3dInfo);
+	SYNC_DEL(rgbDataScanDescInfo);
 
 	// RGB-only + Odom + User Data
 	SYNC_DEL(rgbOdomData);
 	SYNC_DEL(rgbOdomDataScan2d);
 	SYNC_DEL(rgbOdomDataScan3d);
+	SYNC_DEL(rgbOdomDataScanDesc);
 	SYNC_DEL(rgbOdomDataInfo);
 	SYNC_DEL(rgbOdomDataScan2dInfo);
 	SYNC_DEL(rgbOdomDataScan3dInfo);
+	SYNC_DEL(rgbOdomDataScanDescInfo);
 #endif
 
 	// 1 RGBD
 	SYNC_DEL(rgbdScan2d);
 	SYNC_DEL(rgbdScan3d);
+	SYNC_DEL(rgbdScanDesc);
 	SYNC_DEL(rgbdInfo);
 	SYNC_DEL(rgbdScan2dInfo);
 	SYNC_DEL(rgbdScan3dInfo);
+	SYNC_DEL(rgbdScanDescInfo);
 
 	// 1 RGBD + Odom
 	SYNC_DEL(rgbdOdom);
 	SYNC_DEL(rgbdOdomScan2d);
 	SYNC_DEL(rgbdOdomScan3d);
+	SYNC_DEL(rgbdOdomScanDesc);
 	SYNC_DEL(rgbdOdomInfo);
 	SYNC_DEL(rgbdOdomScan2dInfo);
 	SYNC_DEL(rgbdOdomScan3dInfo);
+	SYNC_DEL(rgbdOdomScanDescInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// 1 RGBD + User Data
 	SYNC_DEL(rgbdData);
 	SYNC_DEL(rgbdDataScan2d);
 	SYNC_DEL(rgbdDataScan3d);
+	SYNC_DEL(rgbdDataScanDesc);
 	SYNC_DEL(rgbdDataInfo);
 	SYNC_DEL(rgbdDataScan2dInfo);
 	SYNC_DEL(rgbdDataScan3dInfo);
+	SYNC_DEL(rgbdDataScanDescInfo);
 
 	// 1 RGBD + Odom + User Data
 	SYNC_DEL(rgbdOdomData);
 	SYNC_DEL(rgbdOdomDataScan2d);
 	SYNC_DEL(rgbdOdomDataScan3d);
+	SYNC_DEL(rgbdOdomDataScanDesc);
 	SYNC_DEL(rgbdOdomDataInfo);
 	SYNC_DEL(rgbdOdomDataScan2dInfo);
 	SYNC_DEL(rgbdOdomDataScan3dInfo);
+	SYNC_DEL(rgbdOdomDataScanDescInfo);
 #endif
 
 #ifdef RTABMAP_SYNC_MULTI_RGBD
@@ -659,102 +759,126 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbd2);
 	SYNC_DEL(rgbd2Scan2d);
 	SYNC_DEL(rgbd2Scan3d);
+	SYNC_DEL(rgbd2ScanDesc);
 	SYNC_DEL(rgbd2Info);
 	SYNC_DEL(rgbd2Scan2dInfo);
 	SYNC_DEL(rgbd2Scan3dInfo);
+	SYNC_DEL(rgbd2ScanDescInfo);
 
 	// 2 RGBD + Odom
 	SYNC_DEL(rgbd2Odom);
 	SYNC_DEL(rgbd2OdomScan2d);
 	SYNC_DEL(rgbd2OdomScan3d);
+	SYNC_DEL(rgbd2OdomScanDesc);
 	SYNC_DEL(rgbd2OdomInfo);
 	SYNC_DEL(rgbd2OdomScan2dInfo);
 	SYNC_DEL(rgbd2OdomScan3dInfo);
+	SYNC_DEL(rgbd2OdomScanDescInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// 2 RGBD + User Data
 	SYNC_DEL(rgbd2Data);
 	SYNC_DEL(rgbd2DataScan2d);
 	SYNC_DEL(rgbd2DataScan3d);
+	SYNC_DEL(rgbd2DataScanDesc);
 	SYNC_DEL(rgbd2DataInfo);
 	SYNC_DEL(rgbd2DataScan2dInfo);
 	SYNC_DEL(rgbd2DataScan3dInfo);
+	SYNC_DEL(rgbd2DataScanDescInfo);
 
 	// 2 RGBD + Odom + User Data
 	SYNC_DEL(rgbd2OdomData);
 	SYNC_DEL(rgbd2OdomDataScan2d);
 	SYNC_DEL(rgbd2OdomDataScan3d);
+	SYNC_DEL(rgbd2OdomDataScanDesc);
 	SYNC_DEL(rgbd2OdomDataInfo);
 	SYNC_DEL(rgbd2OdomDataScan2dInfo);
 	SYNC_DEL(rgbd2OdomDataScan3dInfo);
+	SYNC_DEL(rgbd2OdomDataScanDescInfo);
 #endif
 
 	// 3 RGBD
 	SYNC_DEL(rgbd3);
 	SYNC_DEL(rgbd3Scan2d);
 	SYNC_DEL(rgbd3Scan3d);
+	SYNC_DEL(rgbd3ScanDesc);
 	SYNC_DEL(rgbd3Info);
 	SYNC_DEL(rgbd3Scan2dInfo);
 	SYNC_DEL(rgbd3Scan3dInfo);
+	SYNC_DEL(rgbd3ScanDescInfo);
 
 	// 3 RGBD + Odom
 	SYNC_DEL(rgbd3Odom);
 	SYNC_DEL(rgbd3OdomScan2d);
 	SYNC_DEL(rgbd3OdomScan3d);
+	SYNC_DEL(rgbd3OdomScanDesc);
 	SYNC_DEL(rgbd3OdomInfo);
 	SYNC_DEL(rgbd3OdomScan2dInfo);
 	SYNC_DEL(rgbd3OdomScan3dInfo);
+	SYNC_DEL(rgbd3OdomScanDescInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// 3 RGBD + User Data
 	SYNC_DEL(rgbd3Data);
 	SYNC_DEL(rgbd3DataScan2d);
 	SYNC_DEL(rgbd3DataScan3d);
+	SYNC_DEL(rgbd3DataScanDesc);
 	SYNC_DEL(rgbd3DataInfo);
 	SYNC_DEL(rgbd3DataScan2dInfo);
 	SYNC_DEL(rgbd3DataScan3dInfo);
+	SYNC_DEL(rgbd3DataScanDescInfo);
 
 	// 3 RGBD + Odom + User Data
 	SYNC_DEL(rgbd3OdomData);
 	SYNC_DEL(rgbd3OdomDataScan2d);
 	SYNC_DEL(rgbd3OdomDataScan3d);
+	SYNC_DEL(rgbd3OdomDataScanDesc);
 	SYNC_DEL(rgbd3OdomDataInfo);
 	SYNC_DEL(rgbd3OdomDataScan2dInfo);
 	SYNC_DEL(rgbd3OdomDataScan3dInfo);
+	SYNC_DEL(rgbd3OdomDataScanDescInfo);
 #endif
 
 	// 4 RGBD
 	SYNC_DEL(rgbd4);
 	SYNC_DEL(rgbd4Scan2d);
 	SYNC_DEL(rgbd4Scan3d);
+	SYNC_DEL(rgbd4ScanDesc);
 	SYNC_DEL(rgbd4Info);
 	SYNC_DEL(rgbd4Scan2dInfo);
 	SYNC_DEL(rgbd4Scan3dInfo);
+	SYNC_DEL(rgbd4ScanDescInfo);
 
 	// 4 RGBD + Odom
 	SYNC_DEL(rgbd4Odom);
 	SYNC_DEL(rgbd4OdomScan2d);
 	SYNC_DEL(rgbd4OdomScan3d);
+	SYNC_DEL(rgbd4OdomScanDesc);
 	SYNC_DEL(rgbd4OdomInfo);
 	SYNC_DEL(rgbd4OdomScan2dInfo);
 	SYNC_DEL(rgbd4OdomScan3dInfo);
+	SYNC_DEL(rgbd4OdomScanDescInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// 4 RGBD + User Data
 	SYNC_DEL(rgbd4Data);
 	SYNC_DEL(rgbd4DataScan2d);
 	SYNC_DEL(rgbd4DataScan3d);
+	SYNC_DEL(rgbd4DataScanDesc);
 	SYNC_DEL(rgbd4DataInfo);
 	SYNC_DEL(rgbd4DataScan2dInfo);
 	SYNC_DEL(rgbd4DataScan3dInfo);
+	SYNC_DEL(rgbd4DataScanDescInfo);
 
 	// 4 RGBD + Odom + User Data
 	SYNC_DEL(rgbd4OdomData);
 	SYNC_DEL(rgbd4OdomDataScan2d);
 	SYNC_DEL(rgbd4OdomDataScan3d);
+	SYNC_DEL(rgbd4OdomDataScanDesc);
 	SYNC_DEL(rgbd4OdomDataInfo);
 	SYNC_DEL(rgbd4OdomDataScan2dInfo);
 	SYNC_DEL(rgbd4OdomDataScan3dInfo);
+	SYNC_DEL(rgbd4OdomDataScanDescInfo);
 #endif
 #endif //RTABMAP_SYNC_MULTI_RGBD
 
@@ -765,21 +889,27 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	// Scan + Odom
 	SYNC_DEL(odomScan2d);
 	SYNC_DEL(odomScan3d);
+	SYNC_DEL(odomScanDesc);
 	SYNC_DEL(odomScan2dInfo);
 	SYNC_DEL(odomScan3dInfo);
+	SYNC_DEL(odomScanDescInfo);
 
 #ifdef RTABMAP_SYNC_USER_DATA
 	// Scan + User Data
 	SYNC_DEL(dataScan2d);
 	SYNC_DEL(dataScan3d);
+	SYNC_DEL(dataScanDesc);
 	SYNC_DEL(dataScan2dInfo);
 	SYNC_DEL(dataScan3dInfo);
+	SYNC_DEL(dataScanDescInfo);
 
 	// Scan + Odom + User Data
 	SYNC_DEL(odomDataScan2d);
 	SYNC_DEL(odomDataScan3d);
+	SYNC_DEL(odomDataScanDesc);
 	SYNC_DEL(odomDataScan2dInfo);
 	SYNC_DEL(odomDataScan3dInfo);
+	SYNC_DEL(odomDataScanDescInfo);
 #endif
 
 	// Odom
@@ -844,12 +974,23 @@ void CommonDataSubscriber::commonSingleDepthCallback(
 		const cv_bridge::CvImageConstPtr & depthMsg,
 		const sensor_msgs::CameraInfo & rgbCameraInfoMsg,
 		const sensor_msgs::CameraInfo & depthCameraInfoMsg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const sensor_msgs::LaserScan& scanMsg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+		const sensor_msgs::PointCloud2& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+		const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
+		const std::vector<rtabmap_ros::KeyPoint> & localKeyPoints,
+		const std::vector<rtabmap_ros::Point3f> & localPoints3d,
+		const cv::Mat & localDescriptors)
 {
 	callbackCalled();
 
+	std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPointsMsgs;
+	localKeyPointsMsgs.push_back(localKeyPoints);
+	std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3dMsgs;
+	localPoints3dMsgs.push_back(localPoints3d);
+	std::vector<cv::Mat> localDescriptorsMsgs;
+	localDescriptorsMsgs.push_back(localDescriptors);
+
 	if(depthMsg.get() == 0 ||
 	   depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
 	   depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
@@ -867,11 +1008,36 @@ void CommonDataSubscriber::commonSingleDepthCallback(
 			depthMsgs.push_back(depthMsg);
 		}
 		cameraInfoMsgs.push_back(rgbCameraInfoMsg);
-		commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+		commonDepthCallback(
+				odomMsg,
+				userDataMsg,
+				imageMsgs,
+				depthMsgs,
+				cameraInfoMsgs,
+				scanMsg,
+				scan3dMsg,
+				odomInfoMsg,
+				globalDescriptorMsgs,
+				localKeyPointsMsgs,
+				localPoints3dMsgs,
+				localDescriptorsMsgs);
 	}
 	else // assuming stereo
 	{
-		commonStereoCallback(odomMsg, userDataMsg, imageMsg, depthMsg, rgbCameraInfoMsg, depthCameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+		commonStereoCallback(
+				odomMsg,
+				userDataMsg,
+				imageMsg,
+				depthMsg,
+				rgbCameraInfoMsg,
+				depthCameraInfoMsg,
+				scanMsg,
+				scan3dMsg,
+				odomInfoMsg,
+				globalDescriptorMsgs,
+				localKeyPointsMsgs,
+				localPoints3dMsgs,
+				localDescriptorsMsgs);
 	}
 }
 

src/CoreWrapper.cpp

@@ -1052,24 +1052,28 @@ void CoreWrapper::commonDepthCallback(
 		const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
 		const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
 		const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
-		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const sensor_msgs::LaserScan& scan2dMsg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+		const sensor_msgs::PointCloud2& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+		const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
+		const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints,
+		const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d,
+		const std::vector<cv::Mat> & localDescriptors)
 {
 	std::string odomFrameId = odomFrameId_;
 	if(odomMsg.get())
 	{
 		odomFrameId = odomMsg->header.frame_id;
-		if(scan2dMsg.get())
+		if(!scan2dMsg.ranges.empty())
 		{
-			if(!odomUpdate(odomMsg, scan2dMsg->header.stamp))
+			if(!odomUpdate(odomMsg, scan2dMsg.header.stamp))
 			{
 				return;
 			}
 		}
-		else if(scan3dMsg.get())
+		else if(!scan3dMsg.data.empty())
 		{
-			if(!odomUpdate(odomMsg, scan3dMsg->header.stamp))
+			if(!odomUpdate(odomMsg, scan3dMsg.header.stamp))
 			{
 				return;
 			}
@@ -1079,16 +1083,16 @@ void CoreWrapper::commonDepthCallback(
 			return;
 		}
 	}
-	else if(scan2dMsg.get())
+	else if(!scan2dMsg.ranges.empty())
 	{
-		if(!odomTFUpdate(scan2dMsg->header.stamp))
+		if(!odomTFUpdate(scan2dMsg.header.stamp))
 		{
 			return;
 		}
 	}
-	else if(scan3dMsg.get())
+	else if(!scan3dMsg.data.empty())
 	{
-		if(!odomTFUpdate(scan3dMsg->header.stamp))
+		if(!odomTFUpdate(scan3dMsg.header.stamp))
 		{
 			return;
 		}
@@ -1105,7 +1109,11 @@ void CoreWrapper::commonDepthCallback(
 			cameraInfoMsgs,
 			scan2dMsg,
 			scan3dMsg,
-			odomInfoMsg);
+			odomInfoMsg,
+			globalDescriptorMsgs,
+			localKeyPoints,
+			localPoints3d,
+			localDescriptors);
 }
 
 void CoreWrapper::commonDepthCallbackImpl(
@@ -1114,9 +1122,13 @@ void CoreWrapper::commonDepthCallbackImpl(
 		const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
 		const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
 		const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
-		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const sensor_msgs::LaserScan& scan2dMsg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+		const sensor_msgs::PointCloud2& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+		const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
+		const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints,
+		const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d,
+		const std::vector<cv::Mat> & localDescriptors)
 {
 	cv::Mat rgb;
 	cv::Mat depth;
@@ -1155,7 +1167,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 
 	LaserScan scan;
 	bool genMaxScanPts = 0;
-	if(scan2dMsg.get() == 0 && scan3dMsg.get() == 0 && !depth.empty() && genScan_)
+	if(!scan2dMsg.ranges.empty() && !scan3dMsg.data.empty() && !depth.empty() && genScan_)
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud2d(new pcl::PointCloud<pcl::PointXYZ>);
 		*scanCloud2d = util3d::laserScanFromDepthImages(
@@ -1166,7 +1178,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 		genMaxScanPts += depth.cols;
 		scan = LaserScan(rtabmap::util3d::laserScan2dFromPointCloud(*scanCloud2d), 0, genScanMaxDepth_, LaserScan::kXY);
 	}
-	else if(scan2dMsg.get() != 0)
+	else if(!scan2dMsg.ranges.empty())
 	{
 		if(!rtabmap_ros::convertScanMsg(
 				scan2dMsg,
@@ -1183,7 +1195,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 			return;
 		}
 	}
-	else if(scan3dMsg.get() != 0)
+	else if(!scan3dMsg.data.empty())
 	{
 		if(!rtabmap_ros::convertScan3dMsg(
 				scan3dMsg,
@@ -1233,6 +1245,11 @@ void CoreWrapper::commonDepthCallbackImpl(
 		odomInfo = odomInfoFromROS(*odomInfoMsg);
 	}
 
+	if(!globalDescriptorMsgs.empty())
+	{
+		data.setGlobalDescriptors(rtabmap_ros::globalDescriptorsFromROS(globalDescriptorMsgs));
+	}
+
 	process(lastPoseStamp_,
 			data,
 			lastPose_,
@@ -1249,24 +1266,28 @@ void CoreWrapper::commonStereoCallback(
 		const cv_bridge::CvImageConstPtr& rightImageMsg,
 		const sensor_msgs::CameraInfo& leftCamInfoMsg,
 		const sensor_msgs::CameraInfo& rightCamInfoMsg,
-		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const sensor_msgs::LaserScan& scan2dMsg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+		const sensor_msgs::PointCloud2& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+		const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
+		const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints,
+		const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d,
+		const std::vector<cv::Mat> & localDescriptors)
 {
 	std::string odomFrameId = odomFrameId_;
 	if(odomMsg.get())
 	{
 		odomFrameId = odomMsg->header.frame_id;
-		if(scan2dMsg.get())
+		if(!scan2dMsg.ranges.empty())
 		{
-			if(!odomUpdate(odomMsg, scan2dMsg->header.stamp))
+			if(!odomUpdate(odomMsg, scan2dMsg.header.stamp))
 			{
 				return;
 			}
 		}
-		else if(scan3dMsg.get())
+		else if(!scan3dMsg.data.empty())
 		{
-			if(!odomUpdate(odomMsg, scan3dMsg->header.stamp))
+			if(!odomUpdate(odomMsg, scan3dMsg.header.stamp))
 			{
 				return;
 			}
@@ -1276,16 +1297,16 @@ void CoreWrapper::commonStereoCallback(
 			return;
 		}
 	}
-	else if(scan2dMsg.get())
+	else if(!scan2dMsg.ranges.empty())
 	{
-		if(!odomTFUpdate(scan2dMsg->header.stamp))
+		if(!odomTFUpdate(scan2dMsg.header.stamp))
 		{
 			return;
 		}
 	}
-	else if(scan3dMsg.get())
+	else if(!scan3dMsg.data.empty())
 	{
-		if(!odomTFUpdate(scan3dMsg->header.stamp))
+		if(!odomTFUpdate(scan3dMsg.header.stamp))
 		{
 			return;
 		}
@@ -1359,12 +1380,17 @@ void CoreWrapper::commonStereoCallback(
 		depthImages[0] = imgDepth;
 		cameraInfos[0] = leftCamInfoMsg;
 
-		commonDepthCallbackImpl(odomFrameId, rtabmap_ros::UserDataConstPtr(), rgbImages, depthImages, cameraInfos, scan2dMsg, scan3dMsg, odomInfoMsg);
+		commonDepthCallbackImpl(odomFrameId,
+				rtabmap_ros::UserDataConstPtr(),
+				rgbImages, depthImages, cameraInfos,
+				scan2dMsg, scan3dMsg,
+				odomInfoMsg,
+				globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 		return;
 	}
 
 	LaserScan scan;
-	if(scan2dMsg.get() != 0)
+	if(!scan2dMsg.ranges.empty())
 	{
 		if(!rtabmap_ros::convertScanMsg(
 				scan2dMsg,
@@ -1381,7 +1407,7 @@ void CoreWrapper::commonStereoCallback(
 			return;
 		}
 	}
-	else if(scan3dMsg.get() != 0)
+	else if(!scan3dMsg.data.empty())
 	{
 		if(!rtabmap_ros::convertScan3dMsg(
 				scan3dMsg,
@@ -1431,6 +1457,11 @@ void CoreWrapper::commonStereoCallback(
 		odomInfo = odomInfoFromROS(*odomInfoMsg);
 	}
 
+	if(!globalDescriptorMsgs.empty())
+	{
+		data.setGlobalDescriptors(rtabmap_ros::globalDescriptorsFromROS(globalDescriptorMsgs));
+	}
+
 	process(lastPoseStamp_,
 			data,
 			lastPose_,
@@ -1444,25 +1475,25 @@ void CoreWrapper::commonStereoCallback(
 void CoreWrapper::commonLaserScanCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
-		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const sensor_msgs::LaserScan& scan2dMsg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+		const sensor_msgs::PointCloud2& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+		const rtabmap_ros::GlobalDescriptor & globalDescriptor)
 {
-	UASSERT(scan2dMsg.get() || scan3dMsg.get());
 	std::string odomFrameId = odomFrameId_;
 	if(odomMsg.get())
 	{
 		odomFrameId = odomMsg->header.frame_id;
-		if(scan2dMsg.get())
+		if(!scan2dMsg.ranges.empty())
 		{
-			if(!odomUpdate(odomMsg, scan2dMsg->header.stamp))
+			if(!odomUpdate(odomMsg, scan2dMsg.header.stamp))
 			{
 				return;
 			}
 		}
-		else if(scan3dMsg.get())
+		else if(!scan3dMsg.data.empty())
 		{
-			if(!odomUpdate(odomMsg, scan3dMsg->header.stamp))
+			if(!odomUpdate(odomMsg, scan3dMsg.header.stamp))
 			{
 				return;
 			}
@@ -1472,16 +1503,16 @@ void CoreWrapper::commonLaserScanCallback(
 			return;
 		}
 	}
-	else if(scan2dMsg.get())
+	else if(!scan2dMsg.ranges.empty())
 	{
-		if(!odomTFUpdate(scan2dMsg->header.stamp))
+		if(!odomTFUpdate(scan2dMsg.header.stamp))
 		{
 			return;
 		}
 	}
-	else if(scan3dMsg.get())
+	else if(!scan3dMsg.data.empty())
 	{
-		if(!odomTFUpdate(scan3dMsg->header.stamp))
+		if(!odomTFUpdate(scan3dMsg.header.stamp))
 		{
 			return;
 		}
@@ -1492,7 +1523,7 @@ void CoreWrapper::commonLaserScanCallback(
 	}
 
 	LaserScan scan;
-	if(scan2dMsg.get() != 0)
+	if(!scan2dMsg.ranges.empty())
 	{
 		if(!rtabmap_ros::convertScanMsg(
 				scan2dMsg,
@@ -1509,7 +1540,7 @@ void CoreWrapper::commonLaserScanCallback(
 			return;
 		}
 	}
-	else if(scan3dMsg.get() != 0)
+	else if(!scan3dMsg.data.empty())
 	{
 		if(!rtabmap_ros::convertScan3dMsg(
 				scan3dMsg,
@@ -1560,7 +1591,7 @@ void CoreWrapper::commonLaserScanCallback(
 			rgb,
 			depth,
 			model,
-			lastPoseIntermediate_?-1:scan2dMsg.get() != 0?scan2dMsg->header.seq:scan3dMsg->header.seq,
+			lastPoseIntermediate_?-1:!scan2dMsg.ranges.empty()?scan2dMsg.header.seq:scan3dMsg.header.seq,
 			rtabmap_ros::timestampFromROS(lastPoseStamp_),
 			userData);
 
@@ -1570,6 +1601,11 @@ void CoreWrapper::commonLaserScanCallback(
 		odomInfo = odomInfoFromROS(*odomInfoMsg);
 	}
 
+	if(!globalDescriptor.data.empty())
+	{
+		data.addGlobalDescriptor(rtabmap_ros::globalDescriptorFromROS(globalDescriptor));
+	}
+
 	process(lastPoseStamp_,
 			data,
 			lastPose_,

src/GuiWrapper.cpp

@@ -425,9 +425,13 @@ void GuiWrapper::commonDepthCallback(
 		const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
 		const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
 		const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
-		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const sensor_msgs::LaserScan& scan2dMsg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+		const sensor_msgs::PointCloud2& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+		const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
+		const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints,
+		const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d,
+		const std::vector<cv::Mat> & localDescriptors)
 {
 	UASSERT(imageMsgs.size() == 0 || (imageMsgs.size() == cameraInfoMsgs.size()));
 
@@ -438,13 +442,13 @@ void GuiWrapper::commonDepthCallback(
 	}
 	else
 	{
-		if(scan2dMsg.get())
+		if(!scan2dMsg.ranges.empty())
 		{
-			odomHeader = scan2dMsg->header;
+			odomHeader = scan2dMsg.header;
 		}
-		else if(scan3dMsg.get())
+		else if(!scan3dMsg.data.empty())
 		{
-			odomHeader = scan3dMsg->header;
+			odomHeader = scan3dMsg.header;
 		}
 		else if(cameraInfoMsgs.size())
 		{
@@ -529,7 +533,7 @@ void GuiWrapper::commonDepthCallback(
 			}
 		}
 
-		if(scan2dMsg.get() != 0)
+		if(!scan2dMsg.ranges.empty())
 		{
 			if(!rtabmap_ros::convertScanMsg(
 					scan2dMsg,
@@ -544,7 +548,7 @@ void GuiWrapper::commonDepthCallback(
 				return;
 			}
 		}
-		else if(scan3dMsg.get() != 0)
+		else if(!scan3dMsg.data.empty())
 		{
 			if(!rtabmap_ros::convertScan3dMsg(
 					scan3dMsg,
@@ -599,9 +603,13 @@ void GuiWrapper::commonStereoCallback(
 		const cv_bridge::CvImageConstPtr& rightImageMsg,
 		const sensor_msgs::CameraInfo& leftCamInfoMsg,
 		const sensor_msgs::CameraInfo& rightCamInfoMsg,
-		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const sensor_msgs::LaserScan& scan2dMsg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+		const sensor_msgs::PointCloud2& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+		const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
+		const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints,
+		const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d,
+		const std::vector<cv::Mat> & localDescriptors)
 {
 	std_msgs::Header odomHeader;
 	if(odomMsg.get())
@@ -610,13 +618,13 @@ void GuiWrapper::commonStereoCallback(
 	}
 	else
 	{
-		if(scan2dMsg.get())
+		if(!scan2dMsg.ranges.empty())
 		{
-			odomHeader = scan2dMsg->header;
+			odomHeader = scan2dMsg.header;
 		}
-		else if(scan3dMsg.get())
+		else if(!scan3dMsg.data.empty())
 		{
-			odomHeader = scan3dMsg->header;
+			odomHeader = scan3dMsg.header;
 		}
 		else
 		{
@@ -691,7 +699,7 @@ void GuiWrapper::commonStereoCallback(
 			return;
 		}
 
-		if(scan2dMsg.get() != 0)
+		if(!scan2dMsg.ranges.empty())
 		{
 			if(!rtabmap_ros::convertScanMsg(
 					scan2dMsg,
@@ -706,7 +714,7 @@ void GuiWrapper::commonStereoCallback(
 				return;
 			}
 		}
-		else if(scan3dMsg.get() != 0)
+		else if(!scan3dMsg.data.empty())
 		{
 			if(!rtabmap_ros::convertScan3dMsg(
 					scan3dMsg,
@@ -757,12 +765,11 @@ void GuiWrapper::commonStereoCallback(
 void GuiWrapper::commonLaserScanCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
-		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const sensor_msgs::LaserScan& scan2dMsg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+		const sensor_msgs::PointCloud2& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
+		const rtabmap_ros::GlobalDescriptor & globalDescriptor)
 {
-	UASSERT(scan2dMsg.get() || scan3dMsg.get());
-
 	std_msgs::Header odomHeader;
 	if(odomMsg.get())
 	{
@@ -770,13 +777,13 @@ void GuiWrapper::commonLaserScanCallback(
 	}
 	else
 	{
-		if(scan2dMsg.get())
+		if(!scan2dMsg.ranges.empty())
 		{
-			odomHeader = scan2dMsg->header;
+			odomHeader = scan2dMsg.header;
 		}
-		else if(scan3dMsg.get())
+		else if(!scan3dMsg.data.empty())
 		{
-			odomHeader = scan3dMsg->header;
+			odomHeader = scan3dMsg.header;
 		}
 		else
 		{
@@ -831,7 +838,7 @@ void GuiWrapper::commonLaserScanCallback(
 	{
 		lastOdomInfoUpdateTime_ = UTimer::now();
 
-		if(scan2dMsg.get() != 0)
+		if(!scan2dMsg.ranges.empty())
 		{
 			if(!rtabmap_ros::convertScanMsg(
 					scan2dMsg,
@@ -846,7 +853,7 @@ void GuiWrapper::commonLaserScanCallback(
 				return;
 			}
 		}
-		else if(scan3dMsg.get() != 0)
+		else if(!scan3dMsg.data.empty())
 		{
 			if(!rtabmap_ros::convertScan3dMsg(
 					scan3dMsg,
@@ -871,13 +878,13 @@ void GuiWrapper::commonLaserScanCallback(
 	else if(odomInfoMsg.get())
 	{
 		//just get scan local transform to adjust camera frame
-		if(scan2dMsg.get() != 0)
+		if(!scan2dMsg.ranges.empty())
 		{
-			fakeCameraLocalTransform = getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
+			fakeCameraLocalTransform = getTransform(frameId_, scan2dMsg.header.frame_id, scan2dMsg.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
 		}
-		else if(scan3dMsg.get() != 0)
+		else if(!scan3dMsg.data.empty())
 		{
-			fakeCameraLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
+			fakeCameraLocalTransform = getTransform(frameId_, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
 		}
 
 		info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg).copyWithoutData();

src/MsgConversion.cpp

@@ -133,12 +133,12 @@ void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb,
 	{
 		rgb = cv_bridge::toCvCopy(image.rgb);
 	}
-	else if(!image.rgbCompressed.data.empty())
+	else if(!image.rgb_compressed.data.empty())
 	{
 #ifdef CV_BRIDGE_HYDRO
 		ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
 #else
-		rgb = cv_bridge::toCvCopy(image.rgbCompressed);
+		rgb = cv_bridge::toCvCopy(image.rgb_compressed);
 #endif
 	}
 	else
@@ -151,11 +151,11 @@ void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb,
 	{
 		depth = cv_bridge::toCvCopy(image.depth);
 	}
-	else if(!image.depthCompressed.data.empty())
+	else if(!image.depth_compressed.data.empty())
 	{
 		cv_bridge::CvImagePtr ptr = boost::make_shared<cv_bridge::CvImage>();
-		ptr->header = image.depthCompressed.header;
+		ptr->header = image.depth_compressed.header;
-		ptr->image = rtabmap::uncompressImage(image.depthCompressed.data);
+		ptr->image = rtabmap::uncompressImage(image.depth_compressed.data);
 		ROS_ASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1);
 		ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
 		depth = ptr;
@@ -173,12 +173,12 @@ void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageC
 	{
 		rgb = cv_bridge::toCvShare(image->rgb, image);
 	}
-	else if(!image->rgbCompressed.data.empty())
+	else if(!image->rgb_compressed.data.empty())
 	{
 #ifdef CV_BRIDGE_HYDRO
 		ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
 #else
-		rgb = cv_bridge::toCvCopy(image->rgbCompressed);
+		rgb = cv_bridge::toCvCopy(image->rgb_compressed);
 #endif
 	}
 	else
@@ -191,21 +191,21 @@ void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageC
 	{
 		depth = cv_bridge::toCvShare(image->depth, image);
 	}
-	else if(!image->depthCompressed.data.empty())
+	else if(!image->depth_compressed.data.empty())
 	{
-		if(image->depthCompressed.format.compare("jpg")==0)
+		if(image->depth_compressed.format.compare("jpg")==0)
 		{
 #ifdef CV_BRIDGE_HYDRO
 			ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
 #else
-			depth = cv_bridge::toCvCopy(image->depthCompressed);
+			depth = cv_bridge::toCvCopy(image->depth_compressed);
 #endif
 		}
 		else
 		{
 			cv_bridge::CvImagePtr ptr = boost::make_shared<cv_bridge::CvImage>();
-			ptr->header = image->depthCompressed.header;
+			ptr->header = image->depth_compressed.header;
-			ptr->image = rtabmap::uncompressImage(image->depthCompressed.data);
+			ptr->image = rtabmap::uncompressImage(image->depth_compressed.data);
 			ROS_ASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1);
 			ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
 			depth = ptr;
@@ -225,7 +225,7 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & imag
 	cv_bridge::CvImageConstPtr depthMsg;
 	toCvShare(image, imageMsg, depthMsg);
 
-	rtabmap::StereoCameraModel stereoModel = stereoCameraModelFromROS(image->rgbCameraInfo, image->depthCameraInfo, rtabmap::Transform::getIdentity());
+	rtabmap::StereoCameraModel stereoModel = stereoCameraModelFromROS(image->rgb_camera_info, image->depth_camera_info, rtabmap::Transform::getIdentity());
 
 	if(stereoModel.isValidForProjection())
 	{
@@ -342,7 +342,7 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & imag
 		data = rtabmap::SensorData(
 				ptrImage->image,
 				ptrDepth->image,
-				rtabmap_ros::cameraModelFromROS(image->rgbCameraInfo),
+				rtabmap_ros::cameraModelFromROS(image->rgb_camera_info),
 				0,
 				rtabmap_ros::timestampFromROS(image->header.stamp));
 	}
@@ -387,6 +387,9 @@ void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat)
 
 	stat.setLoopClosureTransform(rtabmap_ros::transformFromGeometryMsg(info.loopClosureTransform));
 
+	//wmState
+	stat.setWmState(info.wmState);
+
 	//Posterior, likelihood, childCount
 	std::map<int, float> mapIntFloat;
 	for(unsigned int i=0; i<info.posteriorKeys.size() && i<info.posteriorValues.size(); ++i)
@@ -444,6 +447,9 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info)
 	// Detailed info
 	if(stats.extended())
 	{
+		//wmState
+		info.wmState = stats.wmState();
+
 		//Posterior, likelihood, childCount
 		info.posteriorKeys = uKeys(stats.posterior());
 		info.posteriorValues = uValues(stats.posterior());
@@ -517,6 +523,45 @@ void keypointsToROS(const std::vector<cv::KeyPoint> & kpts, std::vector<rtabmap_
 	}
 }
 
+rtabmap::GlobalDescriptor globalDescriptorFromROS(const rtabmap_ros::GlobalDescriptor & msg)
+{
+	return rtabmap::GlobalDescriptor(msg.type, rtabmap::uncompressData(msg.data), rtabmap::uncompressData(msg.info));
+}
+
+void globalDescriptorToROS(const rtabmap::GlobalDescriptor & desc, rtabmap_ros::GlobalDescriptor & msg)
+{
+	msg.type = desc.type();
+	msg.info = rtabmap::compressData(desc.info());
+	msg.data = rtabmap::compressData(desc.data());
+}
+
+std::vector<rtabmap::GlobalDescriptor> globalDescriptorsFromROS(const std::vector<rtabmap_ros::GlobalDescriptor> & msg)
+{
+	if(!msg.empty())
+	{
+		std::vector<rtabmap::GlobalDescriptor> v(msg.size());
+		for(unsigned int i=0; i<msg.size(); ++i)
+		{
+			v[i] = globalDescriptorFromROS(msg[i]);
+		}
+		return v;
+	}
+	return std::vector<rtabmap::GlobalDescriptor>();
+}
+
+void globalDescriptorsToROS(const std::vector<rtabmap::GlobalDescriptor> & desc, std::vector<rtabmap_ros::GlobalDescriptor> & msg)
+{
+	msg.clear();
+	if(!desc.empty())
+	{
+		msg.resize(desc.size());
+		for(unsigned int i=0; i<msg.size(); ++i)
+		{
+			globalDescriptorToROS(desc[i], msg[i]);
+		}
+	}
+}
+
 cv::Point2f point2fFromROS(const rtabmap_ros::Point2f & msg)
 {
 	return cv::Point2f(msg.x, msg.y);
@@ -552,11 +597,11 @@ cv::Point3f point3fFromROS(const rtabmap_ros::Point3f & msg)
 	return cv::Point3f(msg.x, msg.y, msg.z);
 }
 
-void point3fToROS(const cv::Point3f & kpt, rtabmap_ros::Point3f & msg)
+void point3fToROS(const cv::Point3f & pt, rtabmap_ros::Point3f & msg)
 {
-	msg.x = kpt.x;
+	msg.x = pt.x;
-	msg.y = kpt.y;
+	msg.y = pt.y;
-	msg.z = kpt.z;
+	msg.z = pt.z;
 }
 
 std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg)
@@ -569,12 +614,12 @@ std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_ros::Point3f>
 	return v;
 }
 
-void points3fToROS(const std::vector<cv::Point3f> & kpts, std::vector<rtabmap_ros::Point3f> & msg)
+void points3fToROS(const std::vector<cv::Point3f> & pts, std::vector<rtabmap_ros::Point3f> & msg)
 {
-	msg.resize(kpts.size());
+	msg.resize(pts.size());
 	for(unsigned int i=0; i<msg.size(); ++i)
 	{
-		point3fToROS(kpts[i], msg[i]);
+		point3fToROS(pts[i], msg[i]);
 	}
 }
 
@@ -833,23 +878,16 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 	std::multimap<int, cv::KeyPoint> words;
 	std::multimap<int, cv::Point3f> words3D;
 	std::multimap<int, cv::Mat> wordsDescriptors;
-	pcl::PointCloud<pcl::PointXYZ> cloud;
+	cv::Mat descriptors = rtabmap::uncompressData(msg.wordDescriptors);
-	cv::Mat descriptors;
-	if(msg.wordPts.data.size() &&
-	   msg.wordPts.height*msg.wordPts.width == msg.wordIds.size())
-	{
-		pcl::fromROSMsg(msg.wordPts, cloud);
-		descriptors = rtabmap::uncompressData(msg.descriptors);
-	}
 
 	for(unsigned int i=0; i<msg.wordIds.size() && i<msg.wordKpts.size(); ++i)
 	{
 		cv::KeyPoint pt = keypointFromROS(msg.wordKpts.at(i));
 		int wordId = msg.wordIds.at(i);
 		words.insert(std::make_pair(wordId, pt));
-		if(i< cloud.size())
+		if(i< msg.wordPts.size())
 		{
-			words3D.insert(std::make_pair(wordId, cv::Point3f(cloud[i].x, cloud[i].y, cloud[i].z)));
+			words3D.insert(std::make_pair(wordId, point3fFromROS(msg.wordPts[i])));
 		}
 		if(i < descriptors.rows)
 		{
@@ -951,6 +989,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 	s.setWords(words);
 	s.setWords3(words3D);
 	s.setWordsDescriptors(wordsDescriptors);
+	s.sensorData().setGlobalDescriptors(rtabmap_ros::globalDescriptorsFromROS(msg.globalDescriptors));
 	s.sensorData().setOccupancyGrid(
 			compressedMatFromBytes(msg.grid_ground),
 			compressedMatFromBytes(msg.grid_obstacles),
@@ -1036,18 +1075,14 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 
 	if(signature.getWords3().size() && signature.getWords3().size() == signature.getWords().size())
 	{
-		pcl::PointCloud<pcl::PointXYZ> cloud;
+		msg.wordPts.resize(signature.getWords3().size());
-		cloud.resize(signature.getWords3().size());
+		int i=0;
-		index = 0;
 		for(std::multimap<int, cv::Point3f>::const_iterator jter=signature.getWords3().begin();
 			jter!=signature.getWords3().end();
 			++jter)
 		{
-			cloud[index].x = jter->second.x;
+			point3fToROS(jter->second, msg.wordPts[i++]);
-			cloud[index].y = jter->second.y;
-			cloud[index++].z = jter->second.z;
 		}
-		pcl::toROSMsg(cloud, msg.wordPts);
 	}
 	else if(signature.getWords3().size())
 	{
@@ -1082,7 +1117,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 
 		if(valid)
 		{
-			msg.descriptors = rtabmap::compressData(descriptors);
+			msg.wordDescriptors = rtabmap::compressData(descriptors);
 		}
 	}
 	else if(signature.getWordsDescriptors().size())
@@ -1091,6 +1126,8 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 				(int)signature.getWords().size(),
 				(int)signature.getWordsDescriptors().size());
 	}
+
+	rtabmap_ros::globalDescriptorsToROS(signature.sensorData().globalDescriptors(), msg.globalDescriptors);
 }
 
 rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg)
@@ -1760,7 +1797,7 @@ bool convertStereoMsg(
 }
 
 bool convertScanMsg(
-		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const sensor_msgs::LaserScan & scan2dMsg,
 		const std::string & frameId,
 		const std::string & odomFrameId,
 		const ros::Time & odomStamp,
@@ -1772,8 +1809,8 @@ bool convertScanMsg(
 	// make sure the frame of the laser is updated too
 	rtabmap::Transform tmpT = getTransform(
 			odomFrameId.empty()?frameId:odomFrameId,
-			scan2dMsg->header.frame_id,
+			scan2dMsg.header.frame_id,
-			scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment),
+			scan2dMsg.header.stamp + ros::Duration().fromSec(scan2dMsg.ranges.size()*scan2dMsg.time_increment),
 			listener,
 			waitForTransform);
 	if(tmpT.isNull())
@@ -1783,8 +1820,8 @@ bool convertScanMsg(
 
 	rtabmap::Transform scanLocalTransform = getTransform(
 			frameId,
-			scan2dMsg->header.frame_id,
+			scan2dMsg.header.frame_id,
-			scan2dMsg->header.stamp,
+			scan2dMsg.header.stamp,
 			listener,
 			waitForTransform);
 	if(scanLocalTransform.isNull())
@@ -1795,13 +1832,13 @@ bool convertScanMsg(
 	//transform in frameId_ frame
 	sensor_msgs::PointCloud2 scanOut;
 	laser_geometry::LaserProjection projection;
-	projection.transformLaserScanToPointCloud(odomFrameId.empty()?frameId:odomFrameId, *scan2dMsg, scanOut, listener);
+	projection.transformLaserScanToPointCloud(odomFrameId.empty()?frameId:odomFrameId, scan2dMsg, scanOut, listener);
 
 	//transform back in laser frame
 	rtabmap::Transform laserToOdom = getTransform(
-			scan2dMsg->header.frame_id,
+			scan2dMsg.header.frame_id,
 			odomFrameId.empty()?frameId:odomFrameId,
-			scan2dMsg->header.stamp,
+			scan2dMsg.header.stamp,
 			listener,
 			waitForTransform);
 	if(laserToOdom.isNull())
@@ -1810,19 +1847,19 @@ bool convertScanMsg(
 	}
 
 	// sync with odometry stamp
-	if(!odomFrameId.empty() && odomStamp != scan2dMsg->header.stamp)
+	if(!odomFrameId.empty() && odomStamp != scan2dMsg.header.stamp)
 	{
 		rtabmap::Transform sensorT = getTransform(
 				frameId,
 				odomFrameId,
 				odomStamp,
-				scan2dMsg->header.stamp,
+				scan2dMsg.header.stamp,
 				listener,
 				waitForTransform);
 		if(sensorT.isNull())
 		{
 			ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry "
-					"stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan2dMsg->header.stamp.toSec(), odomStamp.toSec());
+					"stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan2dMsg.header.stamp.toSec(), odomStamp.toSec());
 		}
 		else
 		{
@@ -1886,18 +1923,18 @@ bool convertScanMsg(
 	scan = rtabmap::LaserScan(
 			data,
 			format,
-			scan2dMsg->range_min,
+			scan2dMsg.range_min,
-			scan2dMsg->range_max,
+			scan2dMsg.range_max,
-			scan2dMsg->angle_min,
+			scan2dMsg.angle_min,
-			scan2dMsg->angle_max,
+			scan2dMsg.angle_max,
-			scan2dMsg->angle_increment,
+			scan2dMsg.angle_increment,
 			outputInFrameId?rtabmap::Transform::getIdentity():scanLocalTransform);
 
 	return true;
 }
 
 bool convertScan3dMsg(
-		const sensor_msgs::PointCloud2ConstPtr & scan3dMsg,
+		const sensor_msgs::PointCloud2 & scan3dMsg,
 		const std::string & frameId,
 		const std::string & odomFrameId,
 		const ros::Time & odomStamp,
@@ -1910,19 +1947,19 @@ bool convertScan3dMsg(
 	bool containNormals = false;
 	bool containColors = false;
 	bool containIntensity = false;
-	for(unsigned int i=0; i<scan3dMsg->fields.size(); ++i)
+	for(unsigned int i=0; i<scan3dMsg.fields.size(); ++i)
 	{
-		if(scan3dMsg->fields[i].name.compare("normal_x") == 0)
+		if(scan3dMsg.fields[i].name.compare("normal_x") == 0)
 		{
 			containNormals = true;
 		}
-		if(scan3dMsg->fields[i].name.compare("rgb") == 0 || scan3dMsg->fields[i].name.compare("rgba") == 0)
+		if(scan3dMsg.fields[i].name.compare("rgb") == 0 || scan3dMsg.fields[i].name.compare("rgba") == 0)
 		{
 			containColors = true;
 		}
-		if(scan3dMsg->fields[i].name.compare("intensity") == 0)
+		if(scan3dMsg.fields[i].name.compare("intensity") == 0)
 		{
-			if(scan3dMsg->fields[i].datatype == sensor_msgs::PointField::FLOAT32)
+			if(scan3dMsg.fields[i].datatype == sensor_msgs::PointField::FLOAT32)
 			{
 				containIntensity = true;
 			}
@@ -1933,34 +1970,34 @@ bool convertScan3dMsg(
 				{
 					ROS_WARN("The input scan cloud has an \"intensity\" field "
 							"but the datatype (%d) is not supported. Intensity will be ignored. "
-							"This message is only shown once.", scan3dMsg->fields[i].datatype);
+							"This message is only shown once.", scan3dMsg.fields[i].datatype);
 					warningShown = true;
 				}
 			}
 		}
 	}
 
-	rtabmap::Transform scanLocalTransform = getTransform(frameId, scan3dMsg->header.frame_id, scan3dMsg->header.stamp, listener, waitForTransform);
+	rtabmap::Transform scanLocalTransform = getTransform(frameId, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, listener, waitForTransform);
 	if(scanLocalTransform.isNull())
 	{
-		ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec());
+		ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg.header.stamp.toSec());
 		return false;
 	}
 
 	// sync with odometry stamp
-	if(!odomFrameId.empty() && odomStamp != scan3dMsg->header.stamp)
+	if(!odomFrameId.empty() && odomStamp != scan3dMsg.header.stamp)
 	{
 		rtabmap::Transform sensorT = getTransform(
 				frameId,
 				odomFrameId,
 				odomStamp,
-				scan3dMsg->header.stamp,
+				scan3dMsg.header.stamp,
 				listener,
 				waitForTransform);
 		if(sensorT.isNull())
 		{
 			ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry "
-					"stamp is %fs. The 3d laser scan pose will not be synchronized with odometry.", scan3dMsg->header.stamp.toSec(), odomStamp.toSec());
+					"stamp is %fs. The 3d laser scan pose will not be synchronized with odometry.", scan3dMsg.header.stamp.toSec(), odomStamp.toSec());
 		}
 		else
 		{
@@ -1973,7 +2010,7 @@ bool convertScan3dMsg(
 		if(containColors)
 		{
 			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-			pcl::fromROSMsg(*scan3dMsg, *pclScan);
+			pcl::fromROSMsg(scan3dMsg, *pclScan);
 			if(!pclScan->is_dense)
 			{
 				pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
@@ -1983,7 +2020,7 @@ bool convertScan3dMsg(
 		else if(containIntensity)
 		{
 			pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
-			pcl::fromROSMsg(*scan3dMsg, *pclScan);
+			pcl::fromROSMsg(scan3dMsg, *pclScan);
 			if(!pclScan->is_dense)
 			{
 				pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
@@ -1993,7 +2030,7 @@ bool convertScan3dMsg(
 		else
 		{
 			pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
-			pcl::fromROSMsg(*scan3dMsg, *pclScan);
+			pcl::fromROSMsg(scan3dMsg, *pclScan);
 			if(!pclScan->is_dense)
 			{
 				pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
@@ -2006,7 +2043,7 @@ bool convertScan3dMsg(
 		if(containColors)
 		{
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZRGB>);
-			pcl::fromROSMsg(*scan3dMsg, *pclScan);
+			pcl::fromROSMsg(scan3dMsg, *pclScan);
 			if(!pclScan->is_dense)
 			{
 				pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
@@ -2016,7 +2053,7 @@ bool convertScan3dMsg(
 		else if(containIntensity)
 		{
 			pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
-			pcl::fromROSMsg(*scan3dMsg, *pclScan);
+			pcl::fromROSMsg(scan3dMsg, *pclScan);
 			if(!pclScan->is_dense)
 			{
 				pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
@@ -2026,7 +2063,7 @@ bool convertScan3dMsg(
 		else
 		{
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
-			pcl::fromROSMsg(*scan3dMsg, *pclScan);
+			pcl::fromROSMsg(scan3dMsg, *pclScan);
 			if(!pclScan->is_dense)
 			{
 				pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);

src/impl/CommonDataSubscriberDepth.cpp

@@ -37,8 +37,8 @@ void CommonDataSubscriber::depthCallback(
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
@@ -50,9 +50,9 @@ void CommonDataSubscriber::depthScan2dCallback(
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthScan3dCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
@@ -62,9 +62,25 @@ void CommonDataSubscriber::depthScan3dCallback(
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthScanDescCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 void CommonDataSubscriber::depthInfoCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
@@ -74,8 +90,8 @@ void CommonDataSubscriber::depthInfoCallback(
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthScan2dInfoCallback(
@@ -87,8 +103,8 @@ void CommonDataSubscriber::depthScan2dInfoCallback(
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthScan3dInfoCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
@@ -99,8 +115,24 @@ void CommonDataSubscriber::depthScan3dInfoCallback(
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthScanDescInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 
 // RGB + Depth + Odom
@@ -111,8 +143,8 @@ void CommonDataSubscriber::depthOdomCallback(
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
@@ -124,9 +156,9 @@ void CommonDataSubscriber::depthOdomScan2dCallback(
 		const sensor_msgs::LaserScanConstPtr& scanMsg)
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomScan3dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -136,9 +168,25 @@ void CommonDataSubscriber::depthOdomScan3dCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 void CommonDataSubscriber::depthOdomInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -148,8 +196,8 @@ void CommonDataSubscriber::depthOdomInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomScan2dInfoCallback(
@@ -161,8 +209,8 @@ void CommonDataSubscriber::depthOdomScan2dInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -173,8 +221,24 @@ void CommonDataSubscriber::depthOdomScan3dInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 
 #ifdef RTABMAP_SYNC_USER_DATA
@@ -186,8 +250,8 @@ void CommonDataSubscriber::depthDataCallback(
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
 {
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
@@ -199,9 +263,9 @@ void CommonDataSubscriber::depthDataScan2dCallback(
 		const sensor_msgs::LaserScanConstPtr& scanMsg)
 {
 	nav_msgs::OdometryConstPtr odomMsg; // null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthDataScan3dCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -211,9 +275,25 @@ void CommonDataSubscriber::depthDataScan3dCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
 {
 	nav_msgs::OdometryConstPtr odomMsg; // null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataScanDescCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 void CommonDataSubscriber::depthDataInfoCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -223,8 +303,8 @@ void CommonDataSubscriber::depthDataInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	nav_msgs::OdometryConstPtr odomMsg; // null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthDataScan2dInfoCallback(
@@ -236,8 +316,8 @@ void CommonDataSubscriber::depthDataScan2dInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	nav_msgs::OdometryConstPtr odomMsg; // null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthDataScan3dInfoCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -248,8 +328,24 @@ void CommonDataSubscriber::depthDataScan3dInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	nav_msgs::OdometryConstPtr odomMsg; // null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataScanDescInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 
 // RGB + Depth + Odom + User Data
@@ -260,8 +356,8 @@ void CommonDataSubscriber::depthOdomDataCallback(
 		const sensor_msgs::ImageConstPtr& depthMsg,
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
 {
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
@@ -273,9 +369,9 @@ void CommonDataSubscriber::depthOdomDataScan2dCallback(
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
 		const sensor_msgs::LaserScanConstPtr& scanMsg)
 {
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomDataScan3dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -285,9 +381,25 @@ void CommonDataSubscriber::depthOdomDataScan3dCallback(
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
 {
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomDataScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg)
+{
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 void CommonDataSubscriber::depthOdomDataInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -297,8 +409,8 @@ void CommonDataSubscriber::depthOdomDataInfoCallback(
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
@@ -310,8 +422,8 @@ void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
 		const sensor_msgs::LaserScanConstPtr& scanMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -322,8 +434,24 @@ void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomDataScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 #endif
 
@@ -334,6 +462,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
 		bool subscribeUserData,
 		bool subscribeScan2d,
 		bool subscribeScan3d,
+		bool subscribeScanDesc,
 		bool subscribeOdomInfo,
 		int queueSize,
 		bool approxSync)
@@ -361,7 +490,22 @@ void CommonDataSubscriber::setupDepthCallbacks(
 		odomSub_.subscribe(nh, "odom", 1);
 		userDataSub_.subscribe(nh, "user_data", 1);
 
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL7(depthOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL6(depthOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
@@ -408,7 +552,22 @@ void CommonDataSubscriber::setupDepthCallbacks(
 	{
 		odomSub_.subscribe(nh, "odom", 1);
 
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL6(depthOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(depthOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
@@ -454,7 +613,23 @@ void CommonDataSubscriber::setupDepthCallbacks(
 	{
 		userDataSub_.subscribe(nh, "user_data", 1);
 
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL6(depthDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(depthDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
@@ -499,7 +674,22 @@ void CommonDataSubscriber::setupDepthCallbacks(
 #endif
 	else
 	{
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(depthScanDescInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(depthScanDesc, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);

src/impl/CommonDataSubscriberRGB.cpp

@@ -36,8 +36,8 @@ void CommonDataSubscriber::rgbCallback(
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
@@ -49,10 +49,10 @@ void CommonDataSubscriber::rgbScan2dCallback(
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbScan3dCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
@@ -61,10 +61,26 @@ void CommonDataSubscriber::rgbScan3dCallback(
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbScanDescCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 void CommonDataSubscriber::rgbInfoCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
@@ -73,8 +89,8 @@ void CommonDataSubscriber::rgbInfoCallback(
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
@@ -86,9 +102,9 @@ void CommonDataSubscriber::rgbScan2dInfoCallback(
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbScan3dInfoCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
@@ -98,9 +114,25 @@ void CommonDataSubscriber::rgbScan3dInfoCallback(
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbScanDescInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 
 // RGB + Odom
@@ -110,8 +142,8 @@ void CommonDataSubscriber::rgbOdomCallback(
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
@@ -123,10 +155,10 @@ void CommonDataSubscriber::rgbOdomScan2dCallback(
 		const sensor_msgs::LaserScanConstPtr& scanMsg)
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbOdomScan3dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -135,10 +167,26 @@ void CommonDataSubscriber::rgbOdomScan3dCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 void CommonDataSubscriber::rgbOdomInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -147,8 +195,8 @@ void CommonDataSubscriber::rgbOdomInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
@@ -160,9 +208,9 @@ void CommonDataSubscriber::rgbOdomScan2dInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -172,9 +220,25 @@ void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 
 #ifdef RTABMAP_SYNC_USER_DATA
@@ -185,8 +249,8 @@ void CommonDataSubscriber::rgbDataCallback(
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
 {
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
@@ -198,10 +262,10 @@ void CommonDataSubscriber::rgbDataScan2dCallback(
 		const sensor_msgs::LaserScanConstPtr& scanMsg)
 {
 	nav_msgs::OdometryConstPtr odomMsg; // null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbDataScan3dCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -210,10 +274,26 @@ void CommonDataSubscriber::rgbDataScan3dCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
 {
 	nav_msgs::OdometryConstPtr odomMsg; // null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScanDescCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 void CommonDataSubscriber::rgbDataInfoCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -222,8 +302,8 @@ void CommonDataSubscriber::rgbDataInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	nav_msgs::OdometryConstPtr odomMsg; // null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
@@ -235,9 +315,9 @@ void CommonDataSubscriber::rgbDataScan2dInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	nav_msgs::OdometryConstPtr odomMsg; // null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbDataScan3dInfoCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -247,9 +327,25 @@ void CommonDataSubscriber::rgbDataScan3dInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	nav_msgs::OdometryConstPtr odomMsg; // null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScanDescInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 
 // RGB + Depth + Odom + User Data
@@ -259,8 +355,8 @@ void CommonDataSubscriber::rgbOdomDataCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
 {
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
@@ -272,10 +368,10 @@ void CommonDataSubscriber::rgbOdomDataScan2dCallback(
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
 		const sensor_msgs::LaserScanConstPtr& scanMsg)
 {
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbOdomDataScan3dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -284,10 +380,26 @@ void CommonDataSubscriber::rgbOdomDataScan3dCallback(
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
 {
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg)
+{
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 void CommonDataSubscriber::rgbOdomDataInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -296,8 +408,8 @@ void CommonDataSubscriber::rgbOdomDataInfoCallback(
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
@@ -309,9 +421,9 @@ void CommonDataSubscriber::rgbOdomDataScan2dInfoCallback(
 		const sensor_msgs::LaserScanConstPtr& scanMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -321,9 +433,25 @@ void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 #endif
 
@@ -334,6 +462,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
 		bool subscribeUserData,
 		bool subscribeScan2d,
 		bool subscribeScan3d,
+		bool subscribeScanDesc,
 		bool subscribeOdomInfo,
 		int queueSize,
 		bool approxSync)
@@ -355,7 +484,22 @@ void CommonDataSubscriber::setupRGBCallbacks(
 		odomSub_.subscribe(nh, "odom", 1);
 		userDataSub_.subscribe(nh, "user_data", 1);
 
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL6(rgbOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(rgbOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
@@ -402,7 +546,22 @@ void CommonDataSubscriber::setupRGBCallbacks(
 	{
 		odomSub_.subscribe(nh, "odom", 1);
 
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
@@ -448,7 +607,23 @@ void CommonDataSubscriber::setupRGBCallbacks(
 	{
 		userDataSub_.subscribe(nh, "user_data", 1);
 
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
@@ -493,7 +668,22 @@ void CommonDataSubscriber::setupRGBCallbacks(
 #endif
 	else
 	{
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL4(rgbScanDescInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(rgbScanDesc, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);

src/impl/CommonDataSubscriberRGBD.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap_ros/CommonDataSubscriber.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
 #include <rtabmap_ros/MsgConversion.h>
 #include <cv_bridge/cv_bridge.h>
 
@@ -41,10 +42,21 @@ void CommonDataSubscriber::rgbdCallback(
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdScan2dCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -55,9 +67,20 @@ void CommonDataSubscriber::rgbdScan2dCallback(
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			*scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdScan3dCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -68,9 +91,43 @@ void CommonDataSubscriber::rgbdScan3dCallback(
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, *scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
+}
+void CommonDataSubscriber::rgbdScanDescCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdInfoCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -81,9 +138,20 @@ void CommonDataSubscriber::rgbdInfoCallback(
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdScan2dInfoCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -95,8 +163,19 @@ void CommonDataSubscriber::rgbdScan2dInfoCallback(
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			*scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdScan3dInfoCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -108,8 +187,42 @@ void CommonDataSubscriber::rgbdScan3dInfoCallback(
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, *scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
+}
+void CommonDataSubscriber::rgbdScanDescInfoCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 
 // 1 RGBD camera + Odom
@@ -121,10 +234,21 @@ void CommonDataSubscriber::rgbdOdomCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdOdomScan2dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -135,9 +259,20 @@ void CommonDataSubscriber::rgbdOdomScan2dCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			*scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdOdomScan3dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -148,9 +283,47 @@ void CommonDataSubscriber::rgbdOdomScan3dCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, *scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
+}
+void CommonDataSubscriber::rgbdOdomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdOdomInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -161,9 +334,20 @@ void CommonDataSubscriber::rgbdOdomInfoCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdOdomScan2dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -175,8 +359,19 @@ void CommonDataSubscriber::rgbdOdomScan2dInfoCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			*scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdOdomScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -184,12 +379,61 @@ void CommonDataSubscriber::rgbdOdomScan3dInfoCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
+
+	std::vector<double> current_feature_vector;
+	    std::vector<int> lengths_feature_vector;
+
+	rtabmap_ros::ScanDescriptor scanDescriptor;
+	scanDescriptor.header = scan3dMsg->header;
+	scanDescriptor.scan_cloud = *scan3dMsg;
+	scanDescriptor.global_descriptor.type=0;
+	scanDescriptor.global_descriptor.info=rtabmap::compressData(cv::Mat(1, lengths_feature_vector.size(), CV_32FC1, (void*)lengths_feature_vector.data()));
+	scanDescriptor.global_descriptor.data=rtabmap::compressData(cv::Mat(1, current_feature_vector.size(), CV_64FC1, (void*)current_feature_vector.data()));
+
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, *scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
+}
+void CommonDataSubscriber::rgbdOdomScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 
 #ifdef RTABMAP_SYNC_USER_DATA
@@ -202,10 +446,21 @@ void CommonDataSubscriber::rgbdDataCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdDataScan2dCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -216,9 +471,20 @@ void CommonDataSubscriber::rgbdDataScan2dCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			*scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdDataScan3dCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -229,9 +495,47 @@ void CommonDataSubscriber::rgbdDataScan3dCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, *scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
+}
+void CommonDataSubscriber::rgbdDataScanDescCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdDataInfoCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -242,9 +546,20 @@ void CommonDataSubscriber::rgbdDataInfoCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, *scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdDataScan2dInfoCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -256,8 +571,19 @@ void CommonDataSubscriber::rgbdDataScan2dInfoCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			*scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdDataScan3dInfoCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -269,8 +595,47 @@ void CommonDataSubscriber::rgbdDataScan3dInfoCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, *scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
+}
+void CommonDataSubscriber::rgbdDataScanDescInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 
 // 1 RGBD camera + Odom + User Data
@@ -282,10 +647,21 @@ void CommonDataSubscriber::rgbdOdomDataCallback(
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -296,9 +672,20 @@ void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			*scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -309,9 +696,47 @@ void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg ,rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, *scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
+}
+void CommonDataSubscriber::rgbdOdomDataScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg ,rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdOdomDataInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -322,9 +747,20 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback(
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdOdomDataScan2dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -336,8 +772,19 @@ void CommonDataSubscriber::rgbdOdomDataScan2dInfoCallback(
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			*scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdOdomDataScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -349,8 +796,45 @@ void CommonDataSubscriber::rgbdOdomDataScan3dInfoCallback(
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, *scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
+}
+void CommonDataSubscriber::rgbdOdomDataScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 #endif
 
@@ -361,6 +845,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 		bool subscribeUserData,
 		bool subscribeScan2d,
 		bool subscribeScan3d,
+		bool subscribeScanDesc,
 		bool subscribeOdomInfo,
 		int queueSize,
 		bool approxSync)
@@ -378,7 +863,22 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 		{
 			odomSub_.subscribe(nh, "odom", 1);
 			userDataSub_.subscribe(nh, "user_data", 1);
-			if(subscribeScan2d)
+			if(subscribeScanDesc)
+			{
+				subscribedToScanDescriptor_ = true;
+				scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL5(rgbdOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL4(rgbdOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
+				}
+			}
+			else if(subscribeScan2d)
 			{
 				subscribedToScan2d_ = true;
 				scanSub_.subscribe(nh, "scan", 1);
@@ -424,7 +924,22 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 		if(subscribeOdom)
 		{
 			odomSub_.subscribe(nh, "odom", 1);
-			if(subscribeScan2d)
+			if(subscribeScanDesc)
+			{
+				subscribedToScanDescriptor_ = true;
+				scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL4(rgbdOdomScanDescInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL3(rgbdOdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
+				}
+			}
+			else if(subscribeScan2d)
 			{
 				subscribedToScan2d_ = true;
 				scanSub_.subscribe(nh, "scan", 1);
@@ -469,7 +984,22 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 		else if(subscribeUserData)
 		{
 			userDataSub_.subscribe(nh, "user_data", 1);
-			if(subscribeScan2d)
+			if(subscribeScanDesc)
+			{
+				subscribedToScanDescriptor_ = true;
+				scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL4(rgbdDataScanDescInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL3(rgbdDataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
+				}
+			}
+			else if(subscribeScan2d)
 			{
 				subscribedToScan2d_ = true;
 				scanSub_.subscribe(nh, "scan", 1);
@@ -513,7 +1043,22 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 #endif
 		else
 		{
-			if(subscribeScan2d)
+			if(subscribeScanDesc)
+			{
+				subscribedToScanDescriptor_ = true;
+				scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL3(rgbdScanDescInfo, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(rgbdScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_);
+				}
+			}
+			else if(subscribeScan2d)
 			{
 				subscribedToScan2d_ = true;
 				scanSub_.subscribe(nh, "scan", 1);

src/impl/CommonDataSubscriberRGBD2.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap_ros/CommonDataSubscriber.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
 #include <rtabmap_ros/MsgConversion.h>
 #include <cv_bridge/cv_bridge.h>
 
@@ -39,8 +40,22 @@ namespace rtabmap_ros {
 		rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
 		rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
 		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
-		cameraInfoMsgs.push_back(image1Msg->rgbCameraInfo); \
+		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
-		cameraInfoMsgs.push_back(image2Msg->rgbCameraInfo);
+		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
+		std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3d; \
+		std::vector<cv::Mat> localDescriptors; \
+		if(!image1Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
+		if(!image2Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
+		localKeyPoints.push_back(image1Msg->key_points); \
+		localKeyPoints.push_back(image2Msg->key_points); \
+		localPoints3d.push_back(image1Msg->points); \
+		localPoints3d.push_back(image2Msg->points); \
+		localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors));
 
 // 2 RGBD
 void CommonDataSubscriber::rgbd2Callback(
@@ -51,10 +66,10 @@ void CommonDataSubscriber::rgbd2Callback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2Scan2dCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -65,9 +80,9 @@ void CommonDataSubscriber::rgbd2Scan2dCallback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2Scan3dCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -78,9 +93,25 @@ void CommonDataSubscriber::rgbd2Scan3dCallback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2ScanDescCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2InfoCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -91,9 +122,9 @@ void CommonDataSubscriber::rgbd2InfoCallback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2Scan2dInfoCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -105,8 +136,8 @@ void CommonDataSubscriber::rgbd2Scan2dInfoCallback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2Scan3dInfoCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -118,8 +149,24 @@ void CommonDataSubscriber::rgbd2Scan3dInfoCallback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2ScanDescInfoCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 
 // 2 RGBD + Odom
@@ -131,10 +178,10 @@ void CommonDataSubscriber::rgbd2OdomCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2OdomScan2dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -145,9 +192,9 @@ void CommonDataSubscriber::rgbd2OdomScan2dCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2OdomScan3dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -158,9 +205,25 @@ void CommonDataSubscriber::rgbd2OdomScan3dCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2OdomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2OdomInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -171,9 +234,9 @@ void CommonDataSubscriber::rgbd2OdomInfoCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2OdomScan2dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -185,8 +248,8 @@ void CommonDataSubscriber::rgbd2OdomScan2dInfoCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2OdomScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -198,8 +261,24 @@ void CommonDataSubscriber::rgbd2OdomScan3dInfoCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2OdomScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 
 #ifdef RTABMAP_SYNC_USER_DATA
@@ -212,10 +291,10 @@ void CommonDataSubscriber::rgbd2DataCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2DataScan2dCallback(
 		const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -226,9 +305,9 @@ void CommonDataSubscriber::rgbd2DataScan2dCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2DataScan3dCallback(
 		const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -239,9 +318,25 @@ void CommonDataSubscriber::rgbd2DataScan3dCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2DataScanDescCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2DataInfoCallback(
 		const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -252,9 +347,9 @@ void CommonDataSubscriber::rgbd2DataInfoCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2DataScan2dInfoCallback(
 		const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -266,8 +361,8 @@ void CommonDataSubscriber::rgbd2DataScan2dInfoCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2DataScan3dInfoCallback(
 		const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -279,8 +374,24 @@ void CommonDataSubscriber::rgbd2DataScan3dInfoCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2DataScanDescInfoCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 
 // 2 RGBD + Odom + User Data
@@ -292,10 +403,10 @@ void CommonDataSubscriber::rgbd2OdomDataCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2OdomDataScan2dCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
@@ -306,9 +417,9 @@ void CommonDataSubscriber::rgbd2OdomDataScan2dCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2OdomDataScan3dCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
@@ -319,9 +430,25 @@ void CommonDataSubscriber::rgbd2OdomDataScan3dCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2OdomDataScanDescCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
@@ -332,9 +459,9 @@ void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2OdomDataScan2dInfoCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
@@ -346,8 +473,8 @@ void CommonDataSubscriber::rgbd2OdomDataScan2dInfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2OdomDataScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
@@ -359,8 +486,23 @@ void CommonDataSubscriber::rgbd2OdomDataScan3dInfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2OdomDataScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 #endif
 
@@ -371,6 +513,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 		bool subscribeUserData,
 		bool subscribeScan2d,
 		bool subscribeScan3d,
+		bool subscribeScanDesc,
 		bool subscribeOdomInfo,
 		int queueSize,
 		bool approxSync)
@@ -388,7 +531,22 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 	{
 		odomSub_.subscribe(nh, "odom", 1);
 		userDataSub_.subscribe(nh, "user_data", 1);
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL6(rgbd2OdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(rgbd2OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
@@ -434,7 +592,22 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 	if(subscribeOdom)
 	{
 		odomSub_.subscribe(nh, "odom", 1);
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbd2OdomScanDescInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbd2OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
@@ -479,7 +652,22 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 	else if(subscribeUserData)
 	{
 		userDataSub_.subscribe(nh, "user_data", 1);
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbd2DataScanDescInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbd2DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
@@ -523,7 +711,22 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 #endif
 	else
 	{
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL4(rgbd2ScanDescInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(rgbd2ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);

src/impl/CommonDataSubscriberRGBD3.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap_ros/CommonDataSubscriber.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
 #include <rtabmap_ros/MsgConversion.h>
 #include <cv_bridge/cv_bridge.h>
 
@@ -40,9 +41,28 @@ namespace rtabmap_ros {
 		rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
 		rtabmap_ros::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
 		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
-		cameraInfoMsgs.push_back(image1Msg->rgbCameraInfo); \
+		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
-		cameraInfoMsgs.push_back(image2Msg->rgbCameraInfo); \
+		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
-		cameraInfoMsgs.push_back(image3Msg->rgbCameraInfo);
+		cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
+		std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3d; \
+		std::vector<cv::Mat> localDescriptors; \
+		if(!image1Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
+		if(!image2Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
+		if(!image3Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image3Msg->global_descriptor); \
+		localKeyPoints.push_back(image1Msg->key_points); \
+		localKeyPoints.push_back(image2Msg->key_points); \
+		localKeyPoints.push_back(image3Msg->key_points); \
+		localPoints3d.push_back(image1Msg->points); \
+		localPoints3d.push_back(image2Msg->points); \
+		localPoints3d.push_back(image3Msg->points); \
+		localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image3Msg->descriptors));
 
 // 3 RGBD
 void CommonDataSubscriber::rgbd3Callback(
@@ -54,10 +74,13 @@ void CommonDataSubscriber::rgbd3Callback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3Scan2dCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -69,9 +92,12 @@ void CommonDataSubscriber::rgbd3Scan2dCallback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, *scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3Scan3dCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -83,9 +109,32 @@ void CommonDataSubscriber::rgbd3Scan3dCallback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3ScanDescCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
+			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3InfoCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -97,9 +146,12 @@ void CommonDataSubscriber::rgbd3InfoCallback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3Scan2dInfoCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -112,8 +164,11 @@ void CommonDataSubscriber::rgbd3Scan2dInfoCallback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, *scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3Scan3dInfoCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -126,8 +181,31 @@ void CommonDataSubscriber::rgbd3Scan3dInfoCallback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3ScanDescInfoCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
+			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 
 // 2 RGBD + Odom
@@ -140,10 +218,13 @@ void CommonDataSubscriber::rgbd3OdomCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3OdomScan2dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -155,9 +236,12 @@ void CommonDataSubscriber::rgbd3OdomScan2dCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, *scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3OdomScan3dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -169,9 +253,32 @@ void CommonDataSubscriber::rgbd3OdomScan3dCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3OdomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
+			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3OdomInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -183,9 +290,12 @@ void CommonDataSubscriber::rgbd3OdomInfoCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3OdomScan2dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -198,8 +308,11 @@ void CommonDataSubscriber::rgbd3OdomScan2dInfoCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, *scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3OdomScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -212,8 +325,32 @@ void CommonDataSubscriber::rgbd3OdomScan3dInfoCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3OdomScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
+			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 
 #ifdef RTABMAP_SYNC_USER_DATA
@@ -227,10 +364,13 @@ void CommonDataSubscriber::rgbd3DataCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3DataScan2dCallback(
 		const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -242,9 +382,12 @@ void CommonDataSubscriber::rgbd3DataScan2dCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, *scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3DataScan3dCallback(
 		const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -256,9 +399,32 @@ void CommonDataSubscriber::rgbd3DataScan3dCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3DataScanDescCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
+			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3DataInfoCallback(
 		const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -270,9 +436,12 @@ void CommonDataSubscriber::rgbd3DataInfoCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3DataScan2dInfoCallback(
 		const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -285,8 +454,11 @@ void CommonDataSubscriber::rgbd3DataScan2dInfoCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, *scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3DataScan3dInfoCallback(
 		const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -299,8 +471,31 @@ void CommonDataSubscriber::rgbd3DataScan3dInfoCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3DataScanDescInfoCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
+			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 
 // 2 RGBD + Odom + User Data
@@ -313,10 +508,13 @@ void CommonDataSubscriber::rgbd3OdomDataCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3OdomDataScan2dCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
@@ -328,9 +526,12 @@ void CommonDataSubscriber::rgbd3OdomDataScan2dCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, *scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3OdomDataScan3dCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
@@ -342,9 +543,32 @@ void CommonDataSubscriber::rgbd3OdomDataScan3dCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3OdomDataScanDescCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
+			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3OdomDataInfoCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
@@ -356,9 +580,12 @@ void CommonDataSubscriber::rgbd3OdomDataInfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3OdomDataScan2dInfoCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
@@ -371,8 +598,11 @@ void CommonDataSubscriber::rgbd3OdomDataScan2dInfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, *scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3OdomDataScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
@@ -385,8 +615,31 @@ void CommonDataSubscriber::rgbd3OdomDataScan3dInfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3OdomDataScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
+			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 #endif
 
@@ -397,6 +650,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 		bool subscribeUserData,
 		bool subscribeScan2d,
 		bool subscribeScan3d,
+		bool subscribeScanDescriptor,
 		bool subscribeOdomInfo,
 		int queueSize,
 		bool approxSync)
@@ -414,7 +668,22 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 	{
 		odomSub_.subscribe(nh, "odom", 1);
 		userDataSub_.subscribe(nh, "user_data", 1);
-		if(subscribeScan2d)
+		if(subscribeScanDescriptor)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL7(rgbd3OdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL6(rgbd3OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
@@ -460,7 +729,22 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 	if(subscribeOdom)
 	{
 		odomSub_.subscribe(nh, "odom", 1);
-		if(subscribeScan2d)
+		if(subscribeScanDescriptor)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL6(rgbd3OdomScanDescInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(rgbd3OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
@@ -505,7 +789,22 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 	else if(subscribeUserData)
 	{
 		userDataSub_.subscribe(nh, "user_data", 1);
-		if(subscribeScan2d)
+		if(subscribeScanDescriptor)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL6(rgbd3DataScanDescInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(rgbd3DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
@@ -549,7 +848,22 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 #endif
 	else
 	{
-		if(subscribeScan2d)
+		if(subscribeScanDescriptor)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbd3ScanDescInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbd3ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);

src/impl/CommonDataSubscriberRGBD4.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap_ros/CommonDataSubscriber.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
 #include <rtabmap_ros/MsgConversion.h>
 #include <cv_bridge/cv_bridge.h>
 
@@ -41,10 +42,34 @@ namespace rtabmap_ros {
 		rtabmap_ros::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
 		rtabmap_ros::toCvShare(image4Msg, imageMsgs[3], depthMsgs[3]); \
 		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
-		cameraInfoMsgs.push_back(image1Msg->rgbCameraInfo); \
+		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
-		cameraInfoMsgs.push_back(image2Msg->rgbCameraInfo); \
+		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
-		cameraInfoMsgs.push_back(image3Msg->rgbCameraInfo); \
+		cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
-		cameraInfoMsgs.push_back(image4Msg->rgbCameraInfo);
+		cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
+		std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3d; \
+		std::vector<cv::Mat> localDescriptors; \
+		if(!image1Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
+		if(!image2Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
+		if(!image3Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image3Msg->global_descriptor); \
+		if(!image4Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image4Msg->global_descriptor); \
+		localKeyPoints.push_back(image1Msg->key_points); \
+		localKeyPoints.push_back(image2Msg->key_points); \
+		localKeyPoints.push_back(image3Msg->key_points); \
+		localKeyPoints.push_back(image4Msg->key_points); \
+		localPoints3d.push_back(image1Msg->points); \
+		localPoints3d.push_back(image2Msg->points); \
+		localPoints3d.push_back(image3Msg->points); \
+		localPoints3d.push_back(image4Msg->points); \
+		localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image3Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image4Msg->descriptors));
 
 // 4 RGBD
 void CommonDataSubscriber::rgbd4Callback(
@@ -57,10 +82,10 @@ void CommonDataSubscriber::rgbd4Callback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4Scan2dCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -73,9 +98,9 @@ void CommonDataSubscriber::rgbd4Scan2dCallback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4Scan3dCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -88,9 +113,27 @@ void CommonDataSubscriber::rgbd4Scan3dCallback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4ScanDescCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4InfoCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -103,9 +146,9 @@ void CommonDataSubscriber::rgbd4InfoCallback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4Scan2dInfoCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -119,8 +162,8 @@ void CommonDataSubscriber::rgbd4Scan2dInfoCallback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4Scan3dInfoCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -134,8 +177,26 @@ void CommonDataSubscriber::rgbd4Scan3dInfoCallback(
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4ScanDescInfoCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 
 // 2 RGBD + Odom
@@ -149,10 +210,10 @@ void CommonDataSubscriber::rgbd4OdomCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4OdomScan2dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -165,9 +226,9 @@ void CommonDataSubscriber::rgbd4OdomScan2dCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4OdomScan3dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -180,9 +241,27 @@ void CommonDataSubscriber::rgbd4OdomScan3dCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4OdomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4OdomInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -195,9 +274,9 @@ void CommonDataSubscriber::rgbd4OdomInfoCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4OdomScan2dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -211,8 +290,8 @@ void CommonDataSubscriber::rgbd4OdomScan2dInfoCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4OdomScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -226,8 +305,26 @@ void CommonDataSubscriber::rgbd4OdomScan3dInfoCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4OdomScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 
 #ifdef RTABMAP_SYNC_USER_DATA
@@ -242,10 +339,10 @@ void CommonDataSubscriber::rgbd4DataCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4DataScan2dCallback(
 		const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -258,9 +355,9 @@ void CommonDataSubscriber::rgbd4DataScan2dCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4DataScan3dCallback(
 		const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -273,9 +370,27 @@ void CommonDataSubscriber::rgbd4DataScan3dCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4DataScanDescCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4DataInfoCallback(
 		const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -288,9 +403,9 @@ void CommonDataSubscriber::rgbd4DataInfoCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4DataScan2dInfoCallback(
 		const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -304,8 +419,8 @@ void CommonDataSubscriber::rgbd4DataScan2dInfoCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4DataScan3dInfoCallback(
 		const rtabmap_ros::UserDataConstPtr& userDataMsg,
@@ -319,8 +434,26 @@ void CommonDataSubscriber::rgbd4DataScan3dInfoCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4DataScanDescInfoCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 
 // 2 RGBD + Odom + User Data
@@ -334,10 +467,10 @@ void CommonDataSubscriber::rgbd4OdomDataCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4OdomDataScan2dCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
@@ -350,9 +483,9 @@ void CommonDataSubscriber::rgbd4OdomDataScan2dCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4OdomDataScan3dCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
@@ -365,9 +498,27 @@ void CommonDataSubscriber::rgbd4OdomDataScan3dCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4OdomDataScanDescCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
@@ -380,9 +531,9 @@ void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4OdomDataScan2dInfoCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
@@ -396,8 +547,8 @@ void CommonDataSubscriber::rgbd4OdomDataScan2dInfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4OdomDataScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr& odomMsg,
@@ -411,8 +562,26 @@ void CommonDataSubscriber::rgbd4OdomDataScan3dInfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4OdomDataScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image3Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image4Msg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 #endif
 
@@ -423,6 +592,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 		bool subscribeUserData,
 		bool subscribeScan2d,
 		bool subscribeScan3d,
+		bool subscribeScanDesc,
 		bool subscribeOdomInfo,
 		int queueSize,
 		bool approxSync)
@@ -440,7 +610,22 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 	{
 		odomSub_.subscribe(nh, "odom", 1);
 		userDataSub_.subscribe(nh, "user_data", 1);
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL8(rgbd4OdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL7(rgbd4OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
@@ -486,7 +671,22 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 	if(subscribeOdom)
 	{
 		odomSub_.subscribe(nh, "odom", 1);
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL7(rgbd4OdomScanDescInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL6(rgbd4OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
@@ -531,7 +731,22 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 	else if(subscribeUserData)
 	{
 		userDataSub_.subscribe(nh, "user_data", 1);
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL7(rgbd4DataScanDescInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL6(rgbd4DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
@@ -575,7 +790,22 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 #endif
 	else
 	{
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL6(rgbd4ScanDescInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(rgbd4ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);

src/impl/CommonDataSubscriberScan.cpp

@@ -35,9 +35,9 @@ void CommonDataSubscriber::scan2dCallback(
 	callbackCalled();
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::scan3dCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
@@ -45,9 +45,18 @@ void CommonDataSubscriber::scan3dCallback(
 	callbackCalled();
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::scanDescCallback(
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg)
+{
+	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 void CommonDataSubscriber::scan2dInfoCallback(
 		const sensor_msgs::LaserScanConstPtr& scanMsg,
@@ -56,8 +65,8 @@ void CommonDataSubscriber::scan2dInfoCallback(
 	callbackCalled();
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::scan3dInfoCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
@@ -66,8 +75,17 @@ void CommonDataSubscriber::scan3dInfoCallback(
 	callbackCalled();
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::scanDescInfoCallback(
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 
 void CommonDataSubscriber::odomScan2dCallback(
@@ -76,9 +94,9 @@ void CommonDataSubscriber::odomScan2dCallback(
 {
 	callbackCalled();
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::odomScan3dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -86,9 +104,18 @@ void CommonDataSubscriber::odomScan3dCallback(
 {
 	callbackCalled();
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg)
+{
+	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 void CommonDataSubscriber::odomScan2dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -97,8 +124,8 @@ void CommonDataSubscriber::odomScan2dInfoCallback(
 {
 	callbackCalled();
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::odomScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -107,8 +134,17 @@ void CommonDataSubscriber::odomScan3dInfoCallback(
 {
 	callbackCalled();
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 
 #ifdef RTABMAP_SYNC_USER_DATA
@@ -118,9 +154,9 @@ void CommonDataSubscriber::dataScan2dCallback(
 {
 	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::dataScan3dCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -128,9 +164,18 @@ void CommonDataSubscriber::dataScan3dCallback(
 {
 	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::dataScanDescCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg)
+{
+	callbackCalled();
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 void CommonDataSubscriber::dataScan2dInfoCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -139,8 +184,8 @@ void CommonDataSubscriber::dataScan2dInfoCallback(
 {
 	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::dataScan3dInfoCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -149,8 +194,17 @@ void CommonDataSubscriber::dataScan3dInfoCallback(
 {
 	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::dataScanDescInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 
 void CommonDataSubscriber::odomDataScan2dCallback(
@@ -159,9 +213,9 @@ void CommonDataSubscriber::odomDataScan2dCallback(
 		const sensor_msgs::LaserScanConstPtr& scanMsg)
 {
 	callbackCalled();
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::odomDataScan3dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -169,9 +223,18 @@ void CommonDataSubscriber::odomDataScan3dCallback(
 		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
 {
 	callbackCalled();
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomDataScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg)
+{
+	callbackCalled();
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 void CommonDataSubscriber::odomDataScan2dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -180,8 +243,8 @@ void CommonDataSubscriber::odomDataScan2dInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	callbackCalled();
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
-	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::odomDataScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -190,8 +253,17 @@ void CommonDataSubscriber::odomDataScan3dInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	callbackCalled();
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomDataScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 #endif
 
@@ -199,6 +271,7 @@ void CommonDataSubscriber::setupScanCallbacks(
 		ros::NodeHandle & nh,
 		ros::NodeHandle & pnh,
 		bool scan2dTopic,
+		bool scanDescTopic,
 		bool subscribeOdom,
 		bool subscribeUserData,
 		bool subscribeOdomInfo,
@@ -209,7 +282,12 @@ void CommonDataSubscriber::setupScanCallbacks(
 
 	if(subscribeOdom || subscribeUserData || subscribeOdomInfo)
 	{
-		if(scan2dTopic)
+		if(scanDescTopic)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", 1);
+		}
+		else if(scan2dTopic)
 		{
 			subscribedToScan2d_ = true;
 			scanSub_.subscribe(nh, "scan", 1);
@@ -226,7 +304,20 @@ void CommonDataSubscriber::setupScanCallbacks(
 			odomSub_.subscribe(nh, "odom", 1);
 			userDataSub_.subscribe(nh, "user_data", 1);
 
-			if(scan2dTopic)
+			if(scanDescTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL4(odomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL3(odomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_);
+				}
+			}
+			else if(scan2dTopic)
 			{
 				if(subscribeOdomInfo)
 				{
@@ -259,7 +350,20 @@ void CommonDataSubscriber::setupScanCallbacks(
 		{
 			odomSub_.subscribe(nh, "odom", 1);
 
-			if(scan2dTopic)
+			if(scanDescTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL3(odomScanDescInfo, approxSync, queueSize, odomSub_, scanDescSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(odomScanDesc, approxSync, queueSize, odomSub_, scanDescSub_);
+				}
+			}
+			else if(scan2dTopic)
 			{
 				if(subscribeOdomInfo)
 				{
@@ -291,7 +395,20 @@ void CommonDataSubscriber::setupScanCallbacks(
 		{
 			userDataSub_.subscribe(nh, "user_data", 1);
 
-			if(scan2dTopic)
+			if(scanDescTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", 1);
+					SYNC_DECL3(dataScanDescInfo, approxSync, queueSize, userDataSub_, scanDescSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(dataScanDesc, approxSync, queueSize, userDataSub_, scanDescSub_);
+				}
+			}
+			else if(scan2dTopic)
 			{
 				if(subscribeOdomInfo)
 				{
@@ -319,31 +436,36 @@ void CommonDataSubscriber::setupScanCallbacks(
 			}
 		}
 #endif
-		else
+		else if(subscribeOdomInfo)
 		{
-			if(scan2dTopic)
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			if(scanDescTopic)
 			{
-				if(subscribeOdomInfo)
+				SYNC_DECL2(scanDescInfo, approxSync, queueSize, scanDescSub_, odomInfoSub_);
-				{
+			}
-					subscribedToOdomInfo_ = true;
+			else if(scan2dTopic)
-					odomInfoSub_.subscribe(nh, "odom_info", 1);
+			{
-					SYNC_DECL2(scan2dInfo, approxSync, queueSize, scanSub_, odomInfoSub_);
+				SYNC_DECL2(scan2dInfo, approxSync, queueSize, scanSub_, odomInfoSub_);
-				}
 			}
 			else
 			{
-				if(subscribeOdomInfo)
+				SYNC_DECL2(scan3dInfo, approxSync, queueSize, scan3dSub_, odomInfoSub_);
-				{
-					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(nh, "odom_info", 1);
-					SYNC_DECL2(scan3dInfo, approxSync, queueSize, scan3dSub_, odomInfoSub_);
-				}
 			}
 		}
 	}
 	else
 	{
-		if(scan2dTopic)
+		if(scanDescTopic)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSubOnly_ = nh.subscribe("scan_descriptor", 1, &CommonDataSubscriber::scanDescCallback, this);
+			subscribedTopicsMsg_ =
+					uFormat("\n%s subscribed to:\n   %s",
+					ros::this_node::getName().c_str(),
+					scanDescSubOnly_.getTopic().c_str());
+		}
+		else if(scan2dTopic)
 		{
 			subscribedToScan2d_ = true;
 			scan2dSubOnly_ = nh.subscribe("scan", 1, &CommonDataSubscriber::scan2dCallback, this);

src/impl/CommonDataSubscriberStereo.cpp

@@ -39,8 +39,8 @@ void CommonDataSubscriber::stereoCallback(
 	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // null
+	sensor_msgs::LaserScan scanMsg; // null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
@@ -54,8 +54,8 @@ void CommonDataSubscriber::stereoInfoCallback(
 	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // null
+	sensor_msgs::LaserScan scan2dMsg; // null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
 	commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 
@@ -69,8 +69,8 @@ void CommonDataSubscriber::stereoOdomCallback(
 {
 	callbackCalled();
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
@@ -84,8 +84,8 @@ void CommonDataSubscriber::stereoOdomInfoCallback(
 {
 	callbackCalled();
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
-	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
-	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
 	commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 

src/nodelets/rgbd_odometry.cpp

@@ -469,7 +469,7 @@ private:
 			std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
 			std::vector<sensor_msgs::CameraInfo> infoMsgs;
 			rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
-			infoMsgs.push_back(image->rgbCameraInfo);
+			infoMsgs.push_back(image->rgb_camera_info);
 
 			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
 		}
@@ -487,8 +487,8 @@ private:
 			std::vector<sensor_msgs::CameraInfo> infoMsgs;
 			rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
 			rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
-			infoMsgs.push_back(image->rgbCameraInfo);
+			infoMsgs.push_back(image->rgb_camera_info);
-			infoMsgs.push_back(image2->rgbCameraInfo);
+			infoMsgs.push_back(image2->rgb_camera_info);
 
 			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
 		}
@@ -508,9 +508,9 @@ private:
 			rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
 			rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
 			rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
-			infoMsgs.push_back(image->rgbCameraInfo);
+			infoMsgs.push_back(image->rgb_camera_info);
-			infoMsgs.push_back(image2->rgbCameraInfo);
+			infoMsgs.push_back(image2->rgb_camera_info);
-			infoMsgs.push_back(image3->rgbCameraInfo);
+			infoMsgs.push_back(image3->rgb_camera_info);
 
 			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
 		}
@@ -532,10 +532,10 @@ private:
 			rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
 			rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
 			rtabmap_ros::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
-			infoMsgs.push_back(image->rgbCameraInfo);
+			infoMsgs.push_back(image->rgb_camera_info);
-			infoMsgs.push_back(image2->rgbCameraInfo);
+			infoMsgs.push_back(image2->rgb_camera_info);
-			infoMsgs.push_back(image3->rgbCameraInfo);
+			infoMsgs.push_back(image3->rgb_camera_info);
-			infoMsgs.push_back(image4->rgbCameraInfo);
+			infoMsgs.push_back(image4->rgb_camera_info);
 
 			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
 		}

src/nodelets/rgbd_relay.cpp

@@ -98,17 +98,17 @@ private:
 
 			rtabmap_ros::RGBDImage output;
 			output.header = input->header;
-			output.rgbCameraInfo = input->rgbCameraInfo;
+			output.rgb_camera_info = input->rgb_camera_info;
-			output.depthCameraInfo = input->depthCameraInfo;
+			output.depth_camera_info = input->depth_camera_info;
 
-			rtabmap::StereoCameraModel stereoModel = stereoCameraModelFromROS(input->rgbCameraInfo, input->depthCameraInfo, rtabmap::Transform::getIdentity());
+			rtabmap::StereoCameraModel stereoModel = stereoCameraModelFromROS(input->rgb_camera_info, input->depth_camera_info, rtabmap::Transform::getIdentity());
 
 			if(compress_)
 			{
-				if(!input->rgbCompressed.data.empty())
+				if(!input->rgb_compressed.data.empty())
 				{
 					// already compressed, just copy pointer
-					output.rgbCompressed = input->rgbCompressed;
+					output.rgb_compressed = input->rgb_compressed;
 				}
 				else if(!input->rgb.data.empty())
 				{
@@ -116,14 +116,14 @@ private:
 					ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
 #else
 					cv_bridge::CvImageConstPtr rgb = cv_bridge::toCvShare(input->rgb, input);
-					rgb->toCompressedImageMsg(output.rgbCompressed, cv_bridge::JPG);
+					rgb->toCompressedImageMsg(output.rgb_compressed, cv_bridge::JPG);
 #endif
 				}
 
-				if(!input->depthCompressed.data.empty())
+				if(!input->depth_compressed.data.empty())
 				{
 					// already compressed, just copy pointer
-					output.depthCompressed = input->depthCompressed;
+					output.depth_compressed = input->depth_compressed;
 				}
 				else if(!input->depth.data.empty())
 				{
@@ -131,14 +131,14 @@ private:
 					{
 						// right stereo image
 						cv_bridge::CvImageConstPtr imageRightPtr = cv_bridge::toCvShare(input->depth, input);
-						imageRightPtr->toCompressedImageMsg(output.depthCompressed, cv_bridge::JPG);
+						imageRightPtr->toCompressedImageMsg(output.depth_compressed, cv_bridge::JPG);
 					}
 					else
 					{
 						// depth image
 						cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(input->depth, input);
-						output.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
+						output.depth_compressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
-						output.depthCompressed.format = "png";
+						output.depth_compressed.format = "png";
 					}
 				}
 			}
@@ -149,12 +149,12 @@ private:
 					// already raw, just copy pointer
 					output.rgb = input->rgb;
 				}
-				if(!input->rgbCompressed.data.empty())
+				if(!input->rgb_compressed.data.empty())
 				{
 #ifdef CV_BRIDGE_HYDRO
 					ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
 #else
-					cv_bridge::toCvCopy(input->rgbCompressed)->toImageMsg(output.rgb);
+					cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(output.rgb);
 #endif
 				}
 
@@ -163,21 +163,21 @@ private:
 					// already raw, just copy pointer
 					output.depth = input->depth;
 				}
-				else if(input->depthCompressed.format.compare("jpg")==0)
+				else if(input->depth_compressed.format.compare("jpg")==0)
 				{
 					// right stereo image
 #ifdef CV_BRIDGE_HYDRO
 					ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
 #else
-					cv_bridge::toCvCopy(input->depthCompressed)->toImageMsg(output.depth);
+					cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(output.depth);
 #endif
 				}
 				else
 				{
 					// dpeth image
 					cv_bridge::CvImagePtr ptr = boost::make_shared<cv_bridge::CvImage>();
-					ptr->header = input->depthCompressed.header;
+					ptr->header = input->depth_compressed.header;
-					ptr->image = rtabmap::uncompressImage(input->depthCompressed.data);
+					ptr->image = rtabmap::uncompressImage(input->depth_compressed.data);
 					ROS_ASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1);
 					ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
 					ptr->toImageMsg(output.depth);

src/nodelets/rgbd_sync.cpp

@@ -193,13 +193,13 @@ private:
 				sensor_msgs::CameraInfo info;
 				rtabmap_ros::cameraModelToROS(model.scaled(1.0f/float(decimation_)), info);
 				info.header = cameraInfo->header;
-				msg.rgbCameraInfo = info;
+				msg.rgb_camera_info = info;
-				msg.depthCameraInfo = info;
+				msg.depth_camera_info = info;
 			}
 			else
 			{
-				msg.rgbCameraInfo = *cameraInfo;
+				msg.rgb_camera_info = *cameraInfo;
-				msg.depthCameraInfo = *cameraInfo;
+				msg.depth_camera_info = *cameraInfo;
 			}
 
 			cv::Mat rgbMat;
@@ -238,19 +238,19 @@ private:
 
 					rtabmap_ros::RGBDImage msgCompressed;
 					msgCompressed.header = msg.header;
-					msgCompressed.rgbCameraInfo = msg.rgbCameraInfo;
+					msgCompressed.rgb_camera_info = msg.rgb_camera_info;
-					msgCompressed.depthCameraInfo = msg.depthCameraInfo;
+					msgCompressed.depth_camera_info = msg.depth_camera_info;
 
 					cv_bridge::CvImage cvImg;
 					cvImg.header = image->header;
 					cvImg.image = rgbMat;
 					cvImg.encoding = image->encoding;
-					cvImg.toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG);
+					cvImg.toCompressedImageMsg(msgCompressed.rgb_compressed, cv_bridge::JPG);
 
-					msgCompressed.depthCompressed.header = imageDepthPtr->header;
+					msgCompressed.depth_compressed.header = imageDepthPtr->header;
-					msgCompressed.depthCompressed.data = rtabmap::compressImage(depthMat, ".png");
+					msgCompressed.depth_compressed.data = rtabmap::compressImage(depthMat, ".png");
 
-					msgCompressed.depthCompressed.format = "png";
+					msgCompressed.depth_compressed.format = "png";
 
 					rgbdImageCompressedPub_.publish(msgCompressed);
 				}

src/nodelets/stereo_odometry.cpp

@@ -294,7 +294,7 @@ private:
 			int quality = -1;
 			if(!imageRectLeft->image.empty() && !imageRectRight->image.empty())
 			{
-				rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(image->rgbCameraInfo, image->depthCameraInfo, localTransform);
+				rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(image->rgb_camera_info, image->depth_camera_info, localTransform);
 				if(stereoModel.baseline() <= 0)
 				{
 					NODELET_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "

src/nodelets/stereo_sync.cpp

@@ -172,8 +172,8 @@ private:
 			rtabmap_ros::RGBDImage msg;
 			msg.header.frame_id = cameraInfoLeft->header.frame_id;
 			msg.header.stamp = imageLeft->header.stamp>imageRight->header.stamp?imageLeft->header.stamp:imageRight->header.stamp;
-			msg.rgbCameraInfo = *cameraInfoLeft;
+			msg.rgb_camera_info = *cameraInfoLeft;
-			msg.depthCameraInfo = *cameraInfoRight;
+			msg.depth_camera_info = *cameraInfoRight;
 
 			if(rgbdImageCompressedPub_.getNumSubscribers())
 			{
@@ -194,10 +194,10 @@ private:
 					rtabmap_ros::RGBDImage msgCompressed = msg;
 
 					cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
-					imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG);
+					imagePtr->toCompressedImageMsg(msgCompressed.rgb_compressed, cv_bridge::JPG);
 
 					cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageRight);
-					imageDepthPtr->toCompressedImageMsg(msgCompressed.depthCompressed, cv_bridge::JPG);
+					imageDepthPtr->toCompressedImageMsg(msgCompressed.depth_compressed, cv_bridge::JPG);
 
 					rgbdImageCompressedPub_.publish(msgCompressed);
 				}