First version rtabmap_launch working

rtabmap_sync/src/RGBDSyncNode.cpp

@@ -0,0 +1,47 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "rgbd_sync");
+
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		nargv.push_back(argv[i]);
+	}
+
+	nodelet::Loader nodelet;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_sync/rgbd_sync", remap, nargv);
+	ros::spin();
+	return 0;
+}

rtabmap_sync/src/RGBDXSyncNode.cpp

@@ -0,0 +1,47 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "rgbdx_sync");
+
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		nargv.push_back(argv[i]);
+	}
+
+	nodelet::Loader nodelet;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_sync/rgbdx_sync", remap, nargv);
+	ros::spin();
+	return 0;
+}

rtabmap_sync/src/RGBSyncNode.cpp

@@ -0,0 +1,47 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "rgb_sync");
+
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		nargv.push_back(argv[i]);
+	}
+
+	nodelet::Loader nodelet;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_sync/rgb_sync", remap, nargv);
+	ros::spin();
+	return 0;
+}

rtabmap_sync/src/StereoSyncNode.cpp

@@ -0,0 +1,47 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "stereo_sync");
+
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		nargv.push_back(argv[i]);
+	}
+
+	nodelet::Loader nodelet;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_sync/stereo_sync", remap, nargv);
+	ros::spin();
+	return 0;
+}

rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp

@@ -0,0 +1,735 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_sync/CommonDataSubscriber.h>
+
+namespace rtabmap_sync {
+
+// RGB + Depth
+void CommonDataSubscriber::depthCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthScan2dCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthScan3dCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleCameraCallback(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_msgs::ScanDescriptorConstPtr& scanMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleCameraCallback(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,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthScan2dInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthScan3dInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	commonSingleCameraCallback(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_msgs::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
+}
+
+// RGB + Depth + Odom
+void CommonDataSubscriber::depthOdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleCameraCallback(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_msgs::ScanDescriptorConstPtr& scanMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleCameraCallback(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,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomScan2dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomScan3dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	commonSingleCameraCallback(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_msgs::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
+}
+
+#ifdef RTABMAP_SYNC_USER_DATA
+// RGB + Depth + User Data
+void CommonDataSubscriber::depthDataCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataScan2dCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataScan3dCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataScanDescCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
+}
+void CommonDataSubscriber::depthDataInfoCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataScan2dInfoCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataScan3dInfoCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataScanDescInfoCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
+}
+
+// RGB + Depth + Odom + User Data
+void CommonDataSubscriber::depthOdomDataCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleCameraCallback(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_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg)
+{
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleCameraCallback(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,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	commonSingleCameraCallback(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_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
+}
+#endif
+
+void CommonDataSubscriber::setupDepthCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeScanDesc,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup depth callback");
+
+	std::string rgbPrefix = "rgb";
+	std::string depthPrefix = "depth";
+	ros::NodeHandle rgb_nh(nh, rgbPrefix);
+	ros::NodeHandle depth_nh(nh, depthPrefix);
+	ros::NodeHandle rgb_pnh(pnh, rgbPrefix);
+	ros::NodeHandle depth_pnh(pnh, depthPrefix);
+	image_transport::ImageTransport rgb_it(rgb_nh);
+	image_transport::ImageTransport depth_it(depth_nh);
+	image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+	image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+	imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize, hintsRgb);
+	imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize, hintsDepth);
+	cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize);
+
+#ifdef RTABMAP_SYNC_USER_DATA
+	if(subscribeOdom && subscribeUserData)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+		userDataSub_.subscribe(nh, "user_data", queueSize);
+
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL6(CommonDataSubscriber, depthOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL5(CommonDataSubscriber, depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+		}
+	}
+	else 
+#endif
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(CommonDataSubscriber, depthOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(CommonDataSubscriber, depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(CommonDataSubscriber, depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(CommonDataSubscriber, depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+		}
+	}
+#ifdef RTABMAP_SYNC_USER_DATA
+	else if(subscribeUserData)
+	{
+		userDataSub_.subscribe(nh, "user_data", queueSize);
+
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(CommonDataSubscriber, depthDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(CommonDataSubscriber, depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(CommonDataSubscriber, depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(CommonDataSubscriber, depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+		}
+	}
+#endif
+	else
+	{
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(CommonDataSubscriber, depthScanDesc, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(CommonDataSubscriber, depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(CommonDataSubscriber, depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(CommonDataSubscriber, depth, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_);
+		}
+	}
+}
+
+} /* namespace rtabmap_sync */

rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp

@@ -0,0 +1,118 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_sync/CommonDataSubscriber.h>
+
+namespace rtabmap_sync {
+
+void CommonDataSubscriber::odomCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomInfoCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
+}
+#ifdef RTABMAP_SYNC_USER_DATA
+void CommonDataSubscriber::odomDataCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomDataInfoCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
+}
+#endif
+
+void CommonDataSubscriber::setupOdomCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeUserData,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup scan callback");
+
+	if(subscribeUserData || subscribeOdomInfo)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+
+#ifdef RTABMAP_SYNC_USER_DATA
+		if(subscribeUserData)
+		{
+			userDataSub_.subscribe(nh, "user_data", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL2(CommonDataSubscriber, odomData, approxSync, queueSize, odomSub_, userDataSub_);
+			}
+		}
+		else 
+#endif
+		if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_);
+		}
+	}
+	else
+	{
+		odomSubOnly_ = nh.subscribe("odom", queueSize, &CommonDataSubscriber::odomCallback, this);
+		subscribedTopicsMsg_ =
+				uFormat("\n%s subscribed to:\n   %s",
+				ros::this_node::getName().c_str(),
+				odomSubOnly_.getTopic().c_str());
+	}
+}
+
+} /* namespace rtabmap_sync */

rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp

@@ -0,0 +1,729 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_sync/CommonDataSubscriber.h>
+
+namespace rtabmap_sync {
+
+// RGB
+void CommonDataSubscriber::rgbCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbScan2dCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbScan3dCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(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_msgs::ScanDescriptorConstPtr& scanMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
+}
+void CommonDataSubscriber::rgbInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbScan2dInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbScan3dInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(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_msgs::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
+}
+
+// RGB + Odom
+void CommonDataSubscriber::rgbOdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(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_msgs::ScanDescriptorConstPtr& scanMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
+}
+void CommonDataSubscriber::rgbOdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomScan2dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(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_msgs::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
+}
+
+#ifdef RTABMAP_SYNC_USER_DATA
+// RGB + Depth + User Data
+void CommonDataSubscriber::rgbDataCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScan2dCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScan3dCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScanDescCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
+}
+void CommonDataSubscriber::rgbDataInfoCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScan2dInfoCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScan3dInfoCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScanDescInfoCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
+}
+
+// RGB + Depth + Odom + User Data
+void CommonDataSubscriber::rgbOdomDataCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg)
+{
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
+}
+void CommonDataSubscriber::rgbOdomDataInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScan2dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
+}
+#endif
+
+void CommonDataSubscriber::setupRGBCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeScanDesc,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgb-only callback");
+
+	std::string rgbPrefix = "rgb";
+	ros::NodeHandle rgb_nh(nh, rgbPrefix);
+	ros::NodeHandle rgb_pnh(pnh, rgbPrefix);
+	image_transport::ImageTransport rgb_it(rgb_nh);
+	image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+
+	imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize, hintsRgb);
+	cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize);
+
+#ifdef RTABMAP_SYNC_USER_DATA
+	if(subscribeOdom && subscribeUserData)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+		userDataSub_.subscribe(nh, "user_data", queueSize);
+
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(CommonDataSubscriber, rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
+		}
+	}
+	else
+#endif		
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(CommonDataSubscriber, rgbOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(CommonDataSubscriber, rgbOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(CommonDataSubscriber, rgbOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(CommonDataSubscriber, rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_);
+		}
+	}
+#ifdef RTABMAP_SYNC_USER_DATA
+	else if(subscribeUserData)
+	{
+		userDataSub_.subscribe(nh, "user_data", queueSize);
+
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(CommonDataSubscriber, rgbDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(CommonDataSubscriber, rgbDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(CommonDataSubscriber, rgbDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_);
+		}
+	}
+#endif
+	else
+	{
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(CommonDataSubscriber, rgbScanDesc, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(CommonDataSubscriber, rgbScan2d, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(CommonDataSubscriber, rgbScan3d, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL2(CommonDataSubscriber, rgb, approxSync, queueSize, imageSub_, cameraInfoSub_);
+		}
+	}
+}
+
+} /* namespace rtabmap_sync */

rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp

@@ -0,0 +1,766 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_sync/CommonDataSubscriber.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap_conversions/MsgConversion.h>
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_sync {
+
+// 1 RGBD camera
+void CommonDataSubscriber::rgbdCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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_msgs::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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_msgs::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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));
+}
+
+// 1 RGBD camera + Odom
+void CommonDataSubscriber::rgbdOdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_msgs::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);
+	}
+
+	commonSingleCameraCallback(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,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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));
+}
+
+#ifdef RTABMAP_SYNC_USER_DATA
+// 1 RGBD camera + User Data
+void CommonDataSubscriber::rgbdDataCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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_msgs::UserDataConstPtr & userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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_msgs::UserDataConstPtr & userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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_msgs::UserDataConstPtr & userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_msgs::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);
+	}
+
+	commonSingleCameraCallback(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_msgs::UserDataConstPtr & userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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));
+}
+
+// 1 RGBD camera + Odom + User Data
+void CommonDataSubscriber::rgbdOdomDataCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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_msgs::UserDataConstPtr & userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_msgs::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);
+	}
+
+	commonSingleCameraCallback(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,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+
+	std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleCameraCallback(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));
+}
+#endif
+
+void CommonDataSubscriber::setupRGBDCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeScanDesc,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgbd callback");
+
+	if(subscribeOdom ||
+#ifdef RTABMAP_SYNC_USER_DATA
+	   subscribeUserData ||
+#endif
+	   subscribeScan2d ||
+	   subscribeScan3d ||
+	   subscribeScanDesc ||
+	   subscribeOdomInfo)
+	{
+		rgbdSubs_.resize(1);
+		rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_msgs::RGBDImage>;
+		rgbdSubs_[0]->subscribe(nh, "rgbd_image", queueSize);
+
+#ifdef RTABMAP_SYNC_USER_DATA
+		if(subscribeOdom && subscribeUserData)
+		{
+			odomSub_.subscribe(nh, "odom", queueSize);
+			userDataSub_.subscribe(nh, "user_data", queueSize);
+			if(subscribeScanDesc)
+			{
+				subscribedToScanDescriptor_ = true;
+				scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					ROS_WARN("subscribe_odom_info ignored...");
+				}
+				SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
+			}
+			else if(subscribeScan2d)
+			{
+				subscribedToScan2d_ = true;
+				scanSub_.subscribe(nh, "scan", queueSize);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					ROS_WARN("subscribe_odom_info ignored...");
+				}
+				SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
+			}
+			else if(subscribeScan3d)
+			{
+				subscribedToScan3d_ = true;
+				scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					ROS_WARN("subscribe_odom_info ignored...");
+				}
+				SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
+			}
+			else if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
+			}
+		}
+		else
+#endif			
+		if(subscribeOdom)
+		{
+			odomSub_.subscribe(nh, "odom", queueSize);
+			if(subscribeScanDesc)
+			{
+				subscribedToScanDescriptor_ = true;
+				scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					ROS_WARN("subscribe_odom_info ignored...");
+				}
+				SYNC_DECL3(CommonDataSubscriber, rgbdOdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
+			}
+			else if(subscribeScan2d)
+			{
+				subscribedToScan2d_ = true;
+				scanSub_.subscribe(nh, "scan", queueSize);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					ROS_WARN("subscribe_odom_info ignored...");
+				}
+				SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
+			}
+			else if(subscribeScan3d)
+			{
+				subscribedToScan3d_ = true;
+				scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					ROS_WARN("subscribe_odom_info ignored...");
+				}
+				SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
+			}
+			else if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
+			}
+		}
+#ifdef RTABMAP_SYNC_USER_DATA
+		else if(subscribeUserData)
+		{
+			userDataSub_.subscribe(nh, "user_data", queueSize);
+			if(subscribeScanDesc)
+			{
+				subscribedToScanDescriptor_ = true;
+				scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					ROS_WARN("subscribe_odom_info ignored...");
+				}
+				SYNC_DECL3(CommonDataSubscriber, rgbdDataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
+			}
+			else if(subscribeScan2d)
+			{
+				subscribedToScan2d_ = true;
+				scanSub_.subscribe(nh, "scan", queueSize);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					ROS_WARN("subscribe_odom_info ignored...");
+				}
+				SYNC_DECL3(CommonDataSubscriber, rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
+			}
+			else if(subscribeScan3d)
+			{
+				subscribedToScan3d_ = true;
+				scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					ROS_WARN("subscribe_odom_info ignored...");
+				}
+				SYNC_DECL3(CommonDataSubscriber, rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
+			}
+			else if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
+			}
+		}
+#endif
+		else
+		{
+			if(subscribeScanDesc)
+			{
+				subscribedToScanDescriptor_ = true;
+				scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					ROS_WARN("subscribe_odom_info ignored...");
+				}
+				SYNC_DECL2(CommonDataSubscriber, rgbdScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_);
+			}
+			else if(subscribeScan2d)
+			{
+				subscribedToScan2d_ = true;
+				scanSub_.subscribe(nh, "scan", queueSize);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					ROS_WARN("subscribe_odom_info ignored...");
+				}
+				SYNC_DECL2(CommonDataSubscriber, rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
+			}
+			else if(subscribeScan3d)
+			{
+				subscribedToScan3d_ = true;
+				scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					ROS_WARN("subscribe_odom_info ignored...");
+				}
+				SYNC_DECL2(CommonDataSubscriber, rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
+			}
+			else if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+				SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
+			}
+			else
+			{
+				ROS_FATAL("Not supposed to be here!");
+			}
+		}
+	}
+	else
+	{
+		rgbdSub_ = nh.subscribe("rgbd_image", queueSize, &CommonDataSubscriber::rgbdCallback, this);
+
+		subscribedTopicsMsg_ =
+				uFormat("\n%s subscribed to:\n   %s",
+				ros::this_node::getName().c_str(),
+				rgbdSub_.getTopic().c_str());
+	}
+}
+
+} /* namespace rtabmap_sync */

rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp

@@ -0,0 +1,561 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_sync/CommonDataSubscriber.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap_conversions/MsgConversion.h>
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_sync {
+
+#define IMAGE_CONVERSION() \
+		callbackCalled(); \
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2); \
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2); \
+		rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
+		rtabmap_conversions::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear(); \
+		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
+		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
+		std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs; \
+		depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
+		depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
+		std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_msgs::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_msgs::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(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2Scan2dCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2Scan3dCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2ScanDescCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2InfoCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+// 2 RGBD + Odom
+void CommonDataSubscriber::rgbd2OdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2OdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2OdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2OdomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2OdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+#ifdef RTABMAP_SYNC_USER_DATA
+// 2 RGBD + User Data
+void CommonDataSubscriber::rgbd2DataCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2DataScan2dCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2DataScan3dCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2DataScanDescCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2DataInfoCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+// 2 RGBD + Odom + User Data
+void CommonDataSubscriber::rgbd2OdomDataCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2OdomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2OdomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2OdomDataScanDescCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+#endif
+
+void CommonDataSubscriber::setupRGBD2Callbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeScanDesc,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgbd2 callback");
+
+	rgbdSubs_.resize(2);
+	for(int i=0; i<2; ++i)
+	{
+		rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::RGBDImage>;
+		rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize);
+	}
+#ifdef RTABMAP_SYNC_USER_DATA
+	if(subscribeOdom && subscribeUserData)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+		userDataSub_.subscribe(nh, "user_data", queueSize);
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(CommonDataSubscriber, rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+		}
+	}
+	else 
+#endif
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(CommonDataSubscriber, rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+		}
+	}
+#ifdef RTABMAP_SYNC_USER_DATA
+	else if(subscribeUserData)
+	{
+		userDataSub_.subscribe(nh, "user_data", queueSize);
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbd2DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+		}
+	}
+#endif
+	else
+	{
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbd2ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL2(CommonDataSubscriber, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+		}
+	}
+}
+
+} /* namespace rtabmap_sync */

rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp

@@ -0,0 +1,648 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_sync/CommonDataSubscriber.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
+#include <cv_bridge/cv_bridge.h>
+#include "../../../rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h"
+
+namespace rtabmap_sync {
+
+#define IMAGE_CONVERSION() \
+		callbackCalled(); \
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3); \
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(3); \
+		rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
+		rtabmap_conversions::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
+		rtabmap_conversions::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear(); \
+		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
+		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
+		std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs; \
+		depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
+		depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
+		depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
+		std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_msgs::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_msgs::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(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3Scan2dCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3Scan3dCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
+			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3ScanDescCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
+			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3InfoCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+
+// 2 RGBD + Odom
+void CommonDataSubscriber::rgbd3OdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3OdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3OdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
+			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3OdomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
+			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3OdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+
+#ifdef RTABMAP_SYNC_USER_DATA
+// 2 RGBD + User Data
+void CommonDataSubscriber::rgbd3DataCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3DataScan2dCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3DataScan3dCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
+			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3DataScanDescCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
+			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3DataInfoCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+// 2 RGBD + Odom + User Data
+void CommonDataSubscriber::rgbd3OdomDataCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3OdomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3OdomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
+			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3OdomDataScanDescCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
+			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3OdomDataInfoCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+#endif
+
+void CommonDataSubscriber::setupRGBD3Callbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeScanDescriptor,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgbd3 callback");
+
+	rgbdSubs_.resize(3);
+	for(int i=0; i<3; ++i)
+	{
+		rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::RGBDImage>;
+		rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize);
+	}
+#ifdef RTABMAP_SYNC_USER_DATA
+	if(subscribeOdom && subscribeUserData)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+		userDataSub_.subscribe(nh, "user_data", queueSize);
+		if(subscribeScanDescriptor)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL5(CommonDataSubscriber, rgbd3OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
+		}
+	}
+	else
+#endif		
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+		if(subscribeScanDescriptor)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(CommonDataSubscriber, rgbd3Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
+		}
+	}
+#ifdef RTABMAP_SYNC_USER_DATA
+	else if(subscribeUserData)
+	{
+		userDataSub_.subscribe(nh, "user_data", queueSize);
+		if(subscribeScanDescriptor)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd3DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(CommonDataSubscriber, rgbd3Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
+		}
+	}
+#endif
+	else
+	{
+		if(subscribeScanDescriptor)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbd3ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbd3Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbd3Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(CommonDataSubscriber, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
+		}
+	}
+}
+
+} /* namespace rtabmap_sync */

rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp

@@ -0,0 +1,618 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_sync/CommonDataSubscriber.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap_conversions/MsgConversion.h>
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_sync {
+
+#define IMAGE_CONVERSION() \
+		callbackCalled(); \
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4); \
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(4); \
+		rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
+		rtabmap_conversions::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
+		rtabmap_conversions::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
+		rtabmap_conversions::toCvShare(image4Msg, imageMsgs[3], depthMsgs[3]); \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear(); \
+		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
+		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
+		std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs; \
+		depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
+		depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
+		depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
+		depthCameraInfoMsgs.push_back(image4Msg->depth_camera_info); \
+		std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_msgs::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_msgs::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(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4Scan2dCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4Scan3dCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4ScanDescCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4InfoCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+// 2 RGBD + Odom
+void CommonDataSubscriber::rgbd4OdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4OdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4OdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4OdomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4OdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+#ifdef RTABMAP_SYNC_USER_DATA
+// 2 RGBD + User Data
+void CommonDataSubscriber::rgbd4DataCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4DataScan2dCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4DataScan3dCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4DataScanDescCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4DataInfoCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+// 2 RGBD + Odom + User Data
+void CommonDataSubscriber::rgbd4OdomDataCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4OdomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4OdomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4OdomDataScanDescCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+#endif
+
+void CommonDataSubscriber::setupRGBD4Callbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeScanDesc,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgbd4 callback");
+
+	rgbdSubs_.resize(4);
+	for(int i=0; i<4; ++i)
+	{
+		rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::RGBDImage>;
+		rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize);
+	}
+#ifdef RTABMAP_SYNC_USER_DATA
+	if(subscribeOdom && subscribeUserData)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+		userDataSub_.subscribe(nh, "user_data", queueSize);
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL6(CommonDataSubscriber, rgbd4OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
+		}
+	}
+	else
+#endif		
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL5(CommonDataSubscriber, rgbd4Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
+		}
+	}
+#ifdef RTABMAP_SYNC_USER_DATA
+	else if(subscribeUserData)
+	{
+		userDataSub_.subscribe(nh, "user_data", queueSize);
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd4DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL5(CommonDataSubscriber, rgbd4Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
+		}
+	}
+#endif
+	else
+	{
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd4ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd4Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd4Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(CommonDataSubscriber, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
+		}
+	}
+}
+
+} /* namespace rtabmap_sync */

rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp

@@ -0,0 +1,374 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_sync/CommonDataSubscriber.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap_conversions/MsgConversion.h>
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_sync {
+
+#define IMAGE_CONVERSION() \
+		callbackCalled(); \
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5); \
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(5); \
+		rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
+		rtabmap_conversions::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
+		rtabmap_conversions::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
+		rtabmap_conversions::toCvShare(image4Msg, imageMsgs[3], depthMsgs[3]); \
+		rtabmap_conversions::toCvShare(image5Msg, imageMsgs[4], depthMsgs[4]); \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear(); \
+		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
+		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image5Msg->rgb_camera_info); \
+		std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs; \
+		depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
+		depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
+		depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
+		depthCameraInfoMsgs.push_back(image4Msg->depth_camera_info); \
+		depthCameraInfoMsgs.push_back(image5Msg->depth_camera_info); \
+		std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_msgs::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_msgs::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); \
+		if(!image5Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image5Msg->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); \
+		localKeyPoints.push_back(image5Msg->key_points); \
+		localPoints3d.push_back(image1Msg->points); \
+		localPoints3d.push_back(image2Msg->points); \
+		localPoints3d.push_back(image3Msg->points); \
+		localPoints3d.push_back(image4Msg->points); \
+		localPoints3d.push_back(image5Msg->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)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image5Msg->descriptors));
+
+// 5 RGBD
+void CommonDataSubscriber::rgbd5Callback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5Scan2dCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5Scan3dCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5ScanDescCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5InfoCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+// 5 RGBD + Odom
+void CommonDataSubscriber::rgbd5OdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5OdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5OdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5OdomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5OdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+void CommonDataSubscriber::setupRGBD5Callbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeScanDesc,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgbd5 callback");
+
+	rgbdSubs_.resize(5);
+	for(int i=0; i<5; ++i)
+	{
+		rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::RGBDImage>;
+		rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize);
+	}
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL6(CommonDataSubscriber, rgbd5Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
+		}
+	}
+	else
+	{
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd5ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd5Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd5Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL5(CommonDataSubscriber, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
+		}
+	}
+}
+
+} /* namespace rtabmap_sync */

rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp

@@ -0,0 +1,392 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_sync/CommonDataSubscriber.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap_conversions/MsgConversion.h>
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_sync {
+
+#define IMAGE_CONVERSION() \
+		callbackCalled(); \
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(6); \
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(6); \
+		rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
+		rtabmap_conversions::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
+		rtabmap_conversions::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
+		rtabmap_conversions::toCvShare(image4Msg, imageMsgs[3], depthMsgs[3]); \
+		rtabmap_conversions::toCvShare(image5Msg, imageMsgs[4], depthMsgs[4]); \
+		rtabmap_conversions::toCvShare(image6Msg, imageMsgs[5], depthMsgs[5]); \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear(); \
+		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
+		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image5Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image6Msg->rgb_camera_info); \
+		std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs; \
+		depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
+		depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
+		depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
+		depthCameraInfoMsgs.push_back(image4Msg->depth_camera_info); \
+		depthCameraInfoMsgs.push_back(image5Msg->depth_camera_info); \
+		depthCameraInfoMsgs.push_back(image6Msg->depth_camera_info); \
+		std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_msgs::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_msgs::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); \
+		if(!image5Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image5Msg->global_descriptor); \
+		if(!image6Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image6Msg->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); \
+		localKeyPoints.push_back(image5Msg->key_points); \
+		localKeyPoints.push_back(image6Msg->key_points); \
+		localPoints3d.push_back(image1Msg->points); \
+		localPoints3d.push_back(image2Msg->points); \
+		localPoints3d.push_back(image3Msg->points); \
+		localPoints3d.push_back(image4Msg->points); \
+		localPoints3d.push_back(image5Msg->points); \
+		localPoints3d.push_back(image6Msg->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)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image5Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image6Msg->descriptors));
+
+// 6 RGBD
+void CommonDataSubscriber::rgbd6Callback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image6Msg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6Scan2dCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image6Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6Scan3dCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image6Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6ScanDescCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image6Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6InfoCallback(
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image6Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+// 6 RGBD + Odom
+void CommonDataSubscriber::rgbd6OdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image6Msg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6OdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image6Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6OdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image6Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6OdomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image6Msg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6OdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
+		const rtabmap_msgs::RGBDImageConstPtr& image6Msg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+void CommonDataSubscriber::setupRGBD6Callbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeScanDesc,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgbd6 callback");
+
+	rgbdSubs_.resize(6);
+	for(int i=0; i<6; ++i)
+	{
+		rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::RGBDImage>;
+		rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize);
+	}
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL7(CommonDataSubscriber, rgbd6Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
+		}
+	}
+	else
+	{
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd6ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd6Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd6Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL6(CommonDataSubscriber, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
+		}
+	}
+}
+
+} /* namespace rtabmap_sync */

rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp

@@ -0,0 +1,537 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_sync/CommonDataSubscriber.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap_conversions/MsgConversion.h>
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_sync {
+
+#define IMAGE_CONVERSION() \
+		UASSERT(!imagesMsg->rgbd_images.empty()); \
+		callbackCalled(); \
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(imagesMsg->rgbd_images.size()); \
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(imagesMsg->rgbd_images.size()); \
+		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
+		std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs; \
+		std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_msgs::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_msgs::Point3f> > localPoints3d; \
+		std::vector<cv::Mat> localDescriptors; \
+		for(size_t i=0; i<imageMsgs.size(); ++i) \
+		{ \
+			rtabmap_conversions::toCvShare(imagesMsg->rgbd_images[i], imagesMsg, imageMsgs[i], depthMsgs[i]); \
+			cameraInfoMsgs.push_back(imagesMsg->rgbd_images[i].rgb_camera_info); \
+			depthCameraInfoMsgs.push_back(imagesMsg->rgbd_images[i].depth_camera_info); \
+			if(!imagesMsg->rgbd_images[i].global_descriptor.data.empty()) \
+				globalDescriptorMsgs.push_back(imagesMsg->rgbd_images[i].global_descriptor); \
+			localKeyPoints.push_back(imagesMsg->rgbd_images[i].key_points); \
+			localPoints3d.push_back(imagesMsg->rgbd_images[i].points); \
+			localDescriptors.push_back(rtabmap::uncompressData(imagesMsg->rgbd_images[i].descriptors)); \
+		} \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear();
+
+// X RGBD
+void CommonDataSubscriber::rgbdXCallback(
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXScan2dCallback(
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXScan3dCallback(
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXScanDescCallback(
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXInfoCallback(
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+// X RGBD + Odom
+void CommonDataSubscriber::rgbdXOdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+#ifdef RTABMAP_SYNC_USER_DATA
+// X RGBD + User Data
+void CommonDataSubscriber::rgbdXDataCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXDataScan2dCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXDataScan3dCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXDataScanDescCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXDataInfoCallback(
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+// X RGBD + Odom + User Data
+void CommonDataSubscriber::rgbdXOdomDataCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomDataScanDescCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomDataInfoCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_msgs::UserDataConstPtr& userDataMsg,
+		const rtabmap_msgs::RGBDImagesConstPtr& imagesMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+#endif
+
+void CommonDataSubscriber::setupRGBDXCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeScanDesc,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgbdX callback");
+
+	rgbdXSub_.subscribe(nh, "rgbd_images", queueSize);
+#ifdef RTABMAP_SYNC_USER_DATA
+	if(subscribeOdom && subscribeUserData)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+		userDataSub_.subscribe(nh, "user_data", queueSize);
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(CommonDataSubscriber, rgbdXOdomData, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_);
+		}
+	}
+	else 
+#endif
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScanDesc, approxSync, queueSize, odomSub_, rgbdXSub_, scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan2d, approxSync, queueSize, odomSub_, rgbdXSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan3d, approxSync, queueSize, odomSub_, rgbdXSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync, queueSize, odomSub_, rgbdXSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL2(CommonDataSubscriber, rgbdXOdom, approxSync, queueSize, odomSub_, rgbdXSub_);
+		}
+	}
+#ifdef RTABMAP_SYNC_USER_DATA
+	else if(subscribeUserData)
+	{
+		userDataSub_.subscribe(nh, "user_data", queueSize);
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXDataScanDesc, approxSync, queueSize, userDataSub_, rgbdXSub_, scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan2d, approxSync, queueSize, userDataSub_, rgbdXSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan3d, approxSync, queueSize, userDataSub_, rgbdXSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync, queueSize, userDataSub_, rgbdXSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL2(CommonDataSubscriber, rgbdXData, approxSync, queueSize, userDataSub_, rgbdXSub_);
+		}
+	}
+#endif
+	else
+	{
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL2(CommonDataSubscriber, rgbdXScanDesc, approxSync, queueSize, rgbdXSub_, scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL2(CommonDataSubscriber, rgbdXScan2d, approxSync, queueSize, rgbdXSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL2(CommonDataSubscriber, rgbdXScan3d, approxSync, queueSize, rgbdXSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync, queueSize, rgbdXSub_, odomInfoSub_);
+		}
+		else
+		{
+			rgbdXSubOnly_ = nh.subscribe("rgbd_images", queueSize, &CommonDataSubscriber::rgbdXCallback, this);
+
+			subscribedTopicsMsg_ =
+					uFormat("\n%s subscribed to:\n   %s",
+					ros::this_node::getName().c_str(),
+					rgbdXSubOnly_.getTopic().c_str());
+		}
+	}
+}
+
+} /* namespace rtabmap_sync */

rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp

@@ -0,0 +1,489 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_sync/CommonDataSubscriber.h>
+
+namespace rtabmap_sync {
+
+void CommonDataSubscriber::scan2dCallback(
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::scan3dCallback(
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::scanDescCallback(
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
+}
+void CommonDataSubscriber::scan2dInfoCallback(
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::scan3dInfoCallback(
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::scanDescInfoCallback(
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
+}
+
+void CommonDataSubscriber::odomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
+}
+void CommonDataSubscriber::odomScan2dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomScan3dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
+}
+
+#ifdef RTABMAP_SYNC_USER_DATA
+void CommonDataSubscriber::dataScan2dCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	callbackCalled();
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::dataScan3dCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	callbackCalled();
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::dataScanDescCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg)
+{
+	callbackCalled();
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
+}
+void CommonDataSubscriber::dataScan2dInfoCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::dataScan3dInfoCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::dataScanDescInfoCallback(
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
+}
+
+void CommonDataSubscriber::odomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	callbackCalled();
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	callbackCalled();
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomDataScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
+}
+void CommonDataSubscriber::odomDataScan2dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomDataScan3dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomDataScanDescInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_msgs::UserDataConstPtr & userDataMsg,
+		const rtabmap_msgs::ScanDescriptorConstPtr& scanMsg,
+		const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
+}
+#endif
+
+void CommonDataSubscriber::setupScanCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool scan2dTopic,
+		bool scanDescTopic,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup scan callback");
+
+	if(subscribeOdom || subscribeUserData || subscribeOdomInfo)
+	{
+		if(scanDescTopic)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+		}
+		else if(scan2dTopic)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+		}
+		else
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+		}
+
+#ifdef RTABMAP_SYNC_USER_DATA
+		if(subscribeOdom && subscribeUserData)
+		{
+			odomSub_.subscribe(nh, "odom", queueSize);
+			userDataSub_.subscribe(nh, "user_data", queueSize);
+
+			if(scanDescTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_);
+				}
+			}
+			else if(scan2dTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, scanSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, scanSub_);
+				}
+			}
+			else
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL3(CommonDataSubscriber, odomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_);
+				}
+			}
+		}
+		else
+#endif			
+		if(subscribeOdom)
+		{
+			odomSub_.subscribe(nh, "odom", queueSize);
+
+			if(scanDescTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync, queueSize, odomSub_, scanDescSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync, queueSize, odomSub_, scanDescSub_);
+				}
+			}
+			else if(scan2dTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync, queueSize, odomSub_, scanSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync, queueSize, odomSub_, scanSub_);
+				}
+			}
+			else
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync, queueSize, odomSub_, scan3dSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(CommonDataSubscriber, odomScan3d, approxSync, queueSize, odomSub_, scan3dSub_);
+				}
+			}
+		}
+#ifdef RTABMAP_SYNC_USER_DATA
+		else if(subscribeUserData)
+		{
+			userDataSub_.subscribe(nh, "user_data", queueSize);
+
+			if(scanDescTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync, queueSize, userDataSub_, scanDescSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync, queueSize, userDataSub_, scanDescSub_);
+				}
+			}
+			else if(scan2dTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync, queueSize, userDataSub_, scanSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync, queueSize, userDataSub_, scanSub_);
+				}
+			}
+			else
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+					SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync, queueSize, userDataSub_, scan3dSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(CommonDataSubscriber, dataScan3d, approxSync, queueSize, userDataSub_, scan3dSub_);
+				}
+			}
+		}
+#endif
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			if(scanDescTopic)
+			{
+				SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync, queueSize, scanDescSub_, odomInfoSub_);
+			}
+			else if(scan2dTopic)
+			{
+				SYNC_DECL2(CommonDataSubscriber, scan2dInfo, approxSync, queueSize, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL2(CommonDataSubscriber, scan3dInfo, approxSync, queueSize, scan3dSub_, odomInfoSub_);
+			}
+		}
+	}
+	else
+	{
+		if(scanDescTopic)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSubOnly_ = nh.subscribe("scan_descriptor", queueSize, &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", queueSize, &CommonDataSubscriber::scan2dCallback, this);
+			subscribedTopicsMsg_ =
+					uFormat("\n%s subscribed to:\n   %s",
+					ros::this_node::getName().c_str(),
+					scan2dSubOnly_.getTopic().c_str());
+		}
+		else
+		{
+			subscribedToScan3d_ = true;
+			scan3dSubOnly_ = nh.subscribe("scan_cloud", queueSize, &CommonDataSubscriber::scan3dCallback, this);
+			subscribedTopicsMsg_ =
+					uFormat("\n%s subscribed to:\n   %s",
+					ros::this_node::getName().c_str(),
+					scan3dSubOnly_.getTopic().c_str());
+		}
+	}
+}
+
+} /* namespace rtabmap_sync */

rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp

@@ -0,0 +1,146 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_sync/CommonDataSubscriber.h>
+
+namespace rtabmap_sync {
+
+// Stereo
+void CommonDataSubscriber::stereoCallback(
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+	   const sensor_msgs::ImageConstPtr& rightImageMsg,
+	   const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+	   const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
+{
+	callbackCalled();
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::stereoInfoCallback(
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+	   const sensor_msgs::ImageConstPtr& rightImageMsg,
+	   const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+	   const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+	   const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+
+// Stereo + Odom
+void CommonDataSubscriber::stereoOdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+		const sensor_msgs::ImageConstPtr& rightImageMsg,
+		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // null
+	rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::stereoOdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+		const sensor_msgs::ImageConstPtr& rightImageMsg,
+		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+		const rtabmap_msgs::OdomInfoConstPtr & odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scan2dMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::setupStereoCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup stereo callback");
+
+	ros::NodeHandle left_nh(nh, "left");
+	ros::NodeHandle right_nh(nh, "right");
+	ros::NodeHandle left_pnh(pnh, "left");
+	ros::NodeHandle right_pnh(pnh, "right");
+	image_transport::ImageTransport left_it(left_nh);
+	image_transport::ImageTransport right_it(right_nh);
+	image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
+	image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
+
+	imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), queueSize, hintsLeft);
+	imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), queueSize, hintsRight);
+	cameraInfoLeft_.subscribe(left_nh, "camera_info", queueSize);
+	cameraInfoRight_.subscribe(right_nh, "camera_info", queueSize);
+
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+
+		if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL5(CommonDataSubscriber, stereoOdom, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+		}
+	}
+	else
+	{
+		if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(CommonDataSubscriber, stereo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+		}
+	}
+}
+
+} /* namespace rtabmap_sync */

rtabmap_sync/src/nodelets/rgb_sync.cpp

@@ -0,0 +1,234 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CompressedImage.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <boost/thread.hpp>
+
+#include "rtabmap_msgs/RGBDImage.h"
+
+#include "rtabmap/core/Compression.h"
+#include "rtabmap/utilite/UConversion.h"
+
+namespace rtabmap_sync
+{
+
+class RgbSync : public nodelet::Nodelet
+{
+public:
+	RgbSync() :
+		compressedRate_(0),
+		warningThread_(0),
+		callbackCalled_(false),
+		approxSync_(0),
+		exactSync_(0)
+	{}
+
+	virtual ~RgbSync()
+	{
+		if(approxSync_)
+			delete approxSync_;
+		if(exactSync_)
+			delete exactSync_;
+
+		if(warningThread_)
+		{
+			callbackCalled_=true;
+			warningThread_->join();
+			delete warningThread_;
+		}
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		bool approxSync = false;
+		double approxSyncMaxInterval = 0.0;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("compressed_rate", compressedRate_, compressedRate_);
+
+		NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
+		if(approxSync)
+			NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval);
+		NODELET_INFO("%s: queue_size  = %d", getName().c_str(), queueSize);
+		NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
+
+		rgbdImagePub_ = nh.advertise<rtabmap_msgs::RGBDImage>("rgbd_image", 1);
+		rgbdImageCompressedPub_ = nh.advertise<rtabmap_msgs::RGBDImage>("rgbd_image/compressed", 1);
+
+		if(approxSync)
+		{
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageSub_, cameraInfoSub_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			approxSync_->registerCallback(boost::bind(&RgbSync::callback, this, boost::placeholders::_1, boost::placeholders::_2));
+		}
+		else
+		{
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageSub_, cameraInfoSub_);
+			exactSync_->registerCallback(boost::bind(&RgbSync::callback, this, boost::placeholders::_1, boost::placeholders::_2));
+		}
+
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle rgb_pnh(pnh, "rgb");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+
+		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image_rect"), 1, hintsRgb);
+		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+
+		std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s",
+							getName().c_str(),
+							approxSync?"approx":"exact",
+							approxSync&&approxSyncMaxInterval!=std::numeric_limits<double>::max()?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+							imageSub_.getTopic().c_str(),
+							cameraInfoSub_.getTopic().c_str());
+
+		warningThread_ = new boost::thread(boost::bind(&RgbSync::warningLoop, this, subscribedTopicsMsg, approxSync));
+		NODELET_INFO("%s", subscribedTopicsMsg.c_str());
+	}
+
+	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
+	{
+		ros::Duration r(5.0);
+		while(!callbackCalled_)
+		{
+			r.sleep();
+			if(!callbackCalled_)
+			{
+				ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+						"header are set. %s%s",
+						getName().c_str(),
+						approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
+							"topics should have all the exact timestamp for the callback to be called.",
+						subscribedTopicsMsg.c_str());
+			}
+		}
+	}
+
+	void callback(
+			  const sensor_msgs::ImageConstPtr& image,
+			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		callbackCalled_ = true;
+		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
+		{
+			double stamp = image->header.stamp.toSec();
+
+			rtabmap_msgs::RGBDImage msg;
+			msg.header.frame_id = cameraInfo->header.frame_id;
+			msg.header.stamp = image->header.stamp;
+			msg.rgb_camera_info = *cameraInfo;
+
+			if(rgbdImageCompressedPub_.getNumSubscribers())
+			{
+				bool publishCompressed = true;
+				if (compressedRate_ > 0.0)
+				{
+					if ( lastCompressedPublished_ + ros::Duration(1.0/compressedRate_) > ros::Time::now())
+					{
+						NODELET_DEBUG("throttle last update at %f skipping", lastCompressedPublished_.toSec());
+						publishCompressed = false;
+					}
+				}
+
+				if(publishCompressed)
+				{
+					lastCompressedPublished_ = ros::Time::now();
+
+					rtabmap_msgs::RGBDImage msgCompressed = msg;
+
+					cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+					imagePtr->toCompressedImageMsg(msgCompressed.rgb_compressed, cv_bridge::JPG);
+
+					rgbdImageCompressedPub_.publish(msgCompressed);
+				}
+			}
+
+			if(rgbdImagePub_.getNumSubscribers())
+			{
+				msg.rgb = *image;
+				rgbdImagePub_.publish(msg);
+			}
+
+			if( stamp != image->header.stamp.toSec())
+			{
+				NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+						"sure the node publishing the topics doesn't override the same data after publishing them. A "
+						"solution is to use this node within another nodelet manager. Stamps: "
+						"%f->%f",
+						stamp, image->header.stamp.toSec());
+			}
+		}
+	}
+
+private:
+	double compressedRate_;
+	boost::thread * warningThread_;
+	bool callbackCalled_;
+	ros::Time lastCompressedPublished_;
+
+	ros::Publisher rgbdImagePub_;
+	ros::Publisher rgbdImageCompressedPub_;
+
+	image_transport::SubscriberFilter imageSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
+	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_sync::RgbSync, nodelet::Nodelet);
+}
+

rtabmap_sync/src/nodelets/rgbd_sync.cpp

@@ -0,0 +1,333 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CompressedImage.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <boost/thread.hpp>
+
+#include "rtabmap_msgs/RGBDImage.h"
+#include "rtabmap_conversions/MsgConversion.h"
+
+#include "rtabmap/core/Compression.h"
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/utilite/UConversion.h"
+
+namespace rtabmap_sync
+{
+
+class RGBDSync : public nodelet::Nodelet
+{
+public:
+	RGBDSync() :
+		depthScale_(1.0),
+		decimation_(1),
+		compressedRate_(0),
+		warningThread_(0),
+		callbackCalled_(false),
+		approxSyncDepth_(0),
+		exactSyncDepth_(0)
+	{}
+
+	virtual ~RGBDSync()
+	{
+		if(approxSyncDepth_)
+			delete approxSyncDepth_;
+		if(exactSyncDepth_)
+			delete exactSyncDepth_;
+
+		if(warningThread_)
+		{
+			callbackCalled_=true;
+			warningThread_->join();
+			delete warningThread_;
+		}
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		bool approxSync = true;
+		double approxSyncMaxInterval = 0.0;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("depth_scale", depthScale_, depthScale_);
+		pnh.param("decimation", decimation_, decimation_);
+		pnh.param("compressed_rate", compressedRate_, compressedRate_);
+
+		if(decimation_<1)
+		{
+			decimation_ = 1;
+		}
+
+		NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
+		if(approxSync)
+			NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval);
+		NODELET_INFO("%s: queue_size  = %d", getName().c_str(), queueSize);
+		NODELET_INFO("%s: depth_scale = %f", getName().c_str(), depthScale_);
+		NODELET_INFO("%s: decimation = %d", getName().c_str(), decimation_);
+		NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
+
+		rgbdImagePub_ = nh.advertise<rtabmap_msgs::RGBDImage>("rgbd_image", 1);
+		rgbdImageCompressedPub_ = nh.advertise<rtabmap_msgs::RGBDImage>("rgbd_image/compressed", 1);
+
+		if(approxSync)
+		{
+			approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+		}
+		else
+		{
+			exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+			exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+		}
+
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle rgb_pnh(pnh, "rgb");
+		ros::NodeHandle depth_pnh(pnh, "depth");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::ImageTransport depth_it(depth_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+
+		std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s",
+							getName().c_str(),
+							approxSync?"approx":"exact",
+							approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+							imageSub_.getTopic().c_str(),
+							imageDepthSub_.getTopic().c_str(),
+							cameraInfoSub_.getTopic().c_str());
+
+		warningThread_ = new boost::thread(boost::bind(&RGBDSync::warningLoop, this, subscribedTopicsMsg, approxSync));
+		NODELET_INFO("%s", subscribedTopicsMsg.c_str());
+	}
+
+	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
+	{
+		ros::Duration r(5.0);
+		while(!callbackCalled_)
+		{
+			r.sleep();
+			if(!callbackCalled_)
+			{
+				ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+						"header are set. %s%s",
+						getName().c_str(),
+						approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
+							"topics should have all the exact timestamp for the callback to be called.",
+						subscribedTopicsMsg.c_str());
+			}
+		}
+	}
+
+	void callback(
+			  const sensor_msgs::ImageConstPtr& image,
+			  const sensor_msgs::ImageConstPtr& depth,
+			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		callbackCalled_ = true;
+		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
+		{
+			double rgbStamp = image->header.stamp.toSec();
+			double depthStamp = depth->header.stamp.toSec();
+			double infoStamp = cameraInfo->header.stamp.toSec();
+
+			double stampDiff = fabs(rgbStamp - depthStamp);
+			if(stampDiff > 0.010)
+			{
+				NODELET_WARN("The time difference between rgb and depth frames is "
+						"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
+						"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use "
+						"approx_sync=false if streams have all the exact same timestamp.",
+						stampDiff,
+						rgbStamp,
+						depthStamp);
+			}
+
+			rtabmap_msgs::RGBDImage msg;
+			msg.header.frame_id = cameraInfo->header.frame_id;
+			msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
+			if(decimation_>1 && !(depth->width % decimation_ == 0 && depth->height % decimation_ == 0))
+			{
+				ROS_WARN("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
+					   "Images won't be resized.", decimation_, depth->width, depth->height);
+				decimation_ = 1;
+			}
+			if(decimation_>1)
+			{
+				rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
+				sensor_msgs::CameraInfo info;
+				rtabmap_conversions::cameraModelToROS(model.scaled(1.0f/float(decimation_)), info);
+				info.header = cameraInfo->header;
+				msg.rgb_camera_info = info;
+				msg.depth_camera_info = info;
+			}
+			else
+			{
+				msg.rgb_camera_info = *cameraInfo;
+				msg.depth_camera_info = *cameraInfo;
+			}
+
+			cv::Mat rgbMat;
+			cv::Mat depthMat;
+			cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+			cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
+			rgbMat = imagePtr->image;
+			depthMat = imageDepthPtr->image;
+
+			if(decimation_>1)
+			{
+				rgbMat = rtabmap::util2d::decimate(rgbMat, decimation_);
+				depthMat = rtabmap::util2d::decimate(depthMat, decimation_);
+			}
+
+			if(depthScale_ != 1.0)
+			{
+				depthMat*=depthScale_;
+			}
+
+			if(rgbdImageCompressedPub_.getNumSubscribers())
+			{
+				bool publishCompressed = true;
+				if (compressedRate_ > 0.0)
+				{
+					if ( lastCompressedPublished_ + ros::Duration(1.0/compressedRate_) > ros::Time::now())
+					{
+						NODELET_DEBUG("throttle last update at %f skipping", lastCompressedPublished_.toSec());
+						publishCompressed = false;
+					}
+				}
+
+				if(publishCompressed)
+				{
+					lastCompressedPublished_ = ros::Time::now();
+
+					rtabmap_msgs::RGBDImage msgCompressed;
+					msgCompressed.header = msg.header;
+					msgCompressed.rgb_camera_info = msg.rgb_camera_info;
+					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.rgb_compressed, cv_bridge::JPG);
+
+					msgCompressed.depth_compressed.header = imageDepthPtr->header;
+					msgCompressed.depth_compressed.data = rtabmap::compressImage(depthMat, ".png");
+
+					msgCompressed.depth_compressed.format = "png";
+
+					rgbdImageCompressedPub_.publish(msgCompressed);
+				}
+			}
+
+			if(rgbdImagePub_.getNumSubscribers())
+			{
+				cv_bridge::CvImage cvImg;
+				cvImg.header = image->header;
+				cvImg.image = rgbMat;
+				cvImg.encoding = image->encoding;
+				cvImg.toImageMsg(msg.rgb);
+
+				cv_bridge::CvImage cvDepth;
+				cvDepth.header = depth->header;
+				cvDepth.image = depthMat;
+				cvDepth.encoding = depth->encoding;
+				cvDepth.toImageMsg(msg.depth);
+
+				rgbdImagePub_.publish(msg);
+			}
+
+			if( rgbStamp != image->header.stamp.toSec() ||
+				depthStamp != depth->header.stamp.toSec())
+			{
+				NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+						"sure the node publishing the topics doesn't override the same data after publishing them. A "
+						"solution is to use this node within another nodelet manager. Stamps: "
+						"rgb=%f->%f depth=%f->%f",
+						rgbStamp, image->header.stamp.toSec(),
+						depthStamp, depth->header.stamp.toSec());
+			}
+		}
+	}
+
+private:
+	double depthScale_;
+	int decimation_;
+	double compressedRate_;
+	boost::thread * warningThread_;
+	bool callbackCalled_;
+
+	ros::Time lastCompressedPublished_;
+
+	ros::Publisher rgbdImagePub_;
+	ros::Publisher rgbdImageCompressedPub_;
+
+	image_transport::SubscriberFilter imageSub_;
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
+	message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
+	message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_sync::RGBDSync, nodelet::Nodelet);
+}
+

rtabmap_sync/src/nodelets/rgbdx_sync.cpp

@@ -0,0 +1,340 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+
+#include <boost/thread.hpp>
+
+#include "rtabmap_msgs/RGBDImages.h"
+#include "rtabmap_sync/CommonDataSubscriber.h"
+
+namespace rtabmap_sync
+{
+
+class RGBDXSync : public nodelet::Nodelet
+{
+public:
+	RGBDXSync() :
+		warningThread_(0),
+		callbackCalled_(false),
+		SYNC_INIT(rgbd2),
+		SYNC_INIT(rgbd3),
+		SYNC_INIT(rgbd4),
+		SYNC_INIT(rgbd5),
+		SYNC_INIT(rgbd6),
+		SYNC_INIT(rgbd7),
+		SYNC_INIT(rgbd8)
+	{}
+
+	virtual ~RGBDXSync()
+	{
+		SYNC_DEL(rgbd2);
+
+		if(warningThread_)
+		{
+			callbackCalled_=true;
+			warningThread_->join();
+			delete warningThread_;
+		}
+	}
+
+private:
+
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		bool approxSync = true;
+		int rgbdCameras = 2;
+		double approxSyncMaxInterval = 0.0;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
+
+		NODELET_INFO("%s: approx_sync  = %s", getName().c_str(), approxSync?"true":"false");
+		if(approxSync)
+			NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval);
+		NODELET_INFO("%s: queue_size   = %d", getName().c_str(), queueSize);
+		NODELET_INFO("%s: rgbd_cameras = %d", getName().c_str(), rgbdCameras);
+
+		rgbdImagesPub_ = nh.advertise<rtabmap_msgs::RGBDImages>("rgbd_images", 1);
+
+		ROS_ASSERT(rgbdCameras>=2 && rgbdCameras<=8);
+
+		rgbdSubs_.resize(rgbdCameras);
+		for(int i=0; i<rgbdCameras; ++i)
+		{
+			rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::RGBDImage>;
+			rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize);
+		}
+
+		std::string name_ = this->getName();
+		std::string subscribedTopicsMsg_;
+		if(rgbdCameras==2)
+		{
+			SYNC_DECL2(RGBDXSync, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+			if(approxSync && approxSyncMaxInterval>0.0)
+			{
+				rgbd2ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			}
+		}
+		else if(rgbdCameras==3)
+		{
+			SYNC_DECL3(RGBDXSync, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
+			if(approxSync && approxSyncMaxInterval>0.0)
+			{
+				rgbd3ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			}
+		}
+		else if(rgbdCameras==4)
+		{
+			SYNC_DECL4(RGBDXSync, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
+			if(approxSync && approxSyncMaxInterval>0.0)
+			{
+				rgbd4ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			}
+		}
+		else if(rgbdCameras==5)
+		{
+			SYNC_DECL5(RGBDXSync, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
+			if(approxSync && approxSyncMaxInterval>0.0)
+			{
+				rgbd5ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			}
+		}
+		else if(rgbdCameras==6)
+		{
+			SYNC_DECL6(RGBDXSync, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
+			if(approxSync && approxSyncMaxInterval>0.0)
+			{
+				rgbd6ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			}
+		}
+		else if(rgbdCameras==7)
+		{
+			SYNC_DECL7(RGBDXSync, rgbd7, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]));
+			if(approxSync && approxSyncMaxInterval>0.0)
+			{
+				rgbd7ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			}
+		}
+		else if(rgbdCameras==8)
+		{
+			SYNC_DECL8(RGBDXSync, rgbd8, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7]));
+			if(approxSync && approxSyncMaxInterval>0.0)
+			{
+				rgbd8ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			}
+		}
+
+		warningThread_ = new boost::thread(boost::bind(&RGBDXSync::warningLoop, this, subscribedTopicsMsg_, approxSync));
+		NODELET_INFO("%s%s", subscribedTopicsMsg_.c_str(),
+				approxSync&&approxSyncMaxInterval!=0.0?uFormat(" (approx sync max interval=%fs)", approxSyncMaxInterval).c_str():"");
+	}
+
+	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
+	{
+		ros::Duration r(5.0);
+		while(!callbackCalled_)
+		{
+			r.sleep();
+			if(!callbackCalled_)
+			{
+				ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+						"header are set. %s%s",
+						getName().c_str(),
+						approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
+							"topics should have all the exact timestamp for the callback to be called.",
+						subscribedTopicsMsg.c_str());
+			}
+		}
+	}
+
+	DATA_SYNCS2(rgbd2, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage);
+	DATA_SYNCS3(rgbd3, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage);
+	DATA_SYNCS4(rgbd4, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage);
+	DATA_SYNCS5(rgbd5, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage);
+	DATA_SYNCS6(rgbd6, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage);
+	DATA_SYNCS7(rgbd7, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage);
+	DATA_SYNCS8(rgbd8, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage);
+
+private:
+	boost::thread * warningThread_;
+	bool callbackCalled_;
+
+	ros::Publisher rgbdImagesPub_;
+
+	std::vector<message_filters::Subscriber<rtabmap_msgs::RGBDImage>*> rgbdSubs_;
+};
+
+
+void RGBDXSync::rgbd2Callback(
+		  const rtabmap_msgs::RGBDImageConstPtr& image0,
+		  const rtabmap_msgs::RGBDImageConstPtr& image1)
+{
+	callbackCalled_ = true;
+	rtabmap_msgs::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(2);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	rgbdImagesPub_.publish(output);
+}
+
+void RGBDXSync::rgbd3Callback(
+		  const rtabmap_msgs::RGBDImageConstPtr& image0,
+		  const rtabmap_msgs::RGBDImageConstPtr& image1,
+		  const rtabmap_msgs::RGBDImageConstPtr& image2)
+{
+	callbackCalled_ = true;
+	rtabmap_msgs::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(3);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	rgbdImagesPub_.publish(output);
+}
+
+void RGBDXSync::rgbd4Callback(
+		  const rtabmap_msgs::RGBDImageConstPtr& image0,
+		  const rtabmap_msgs::RGBDImageConstPtr& image1,
+		  const rtabmap_msgs::RGBDImageConstPtr& image2,
+		  const rtabmap_msgs::RGBDImageConstPtr& image3)
+{
+	callbackCalled_ = true;
+	rtabmap_msgs::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(4);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	output.rgbd_images[3]=(*image3);
+	rgbdImagesPub_.publish(output);
+}
+
+void RGBDXSync::rgbd5Callback(
+		  const rtabmap_msgs::RGBDImageConstPtr& image0,
+		  const rtabmap_msgs::RGBDImageConstPtr& image1,
+		  const rtabmap_msgs::RGBDImageConstPtr& image2,
+		  const rtabmap_msgs::RGBDImageConstPtr& image3,
+		  const rtabmap_msgs::RGBDImageConstPtr& image4)
+{
+	callbackCalled_ = true;
+	rtabmap_msgs::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(5);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	output.rgbd_images[3]=(*image3);
+	output.rgbd_images[4]=(*image4);
+	rgbdImagesPub_.publish(output);
+}
+
+void RGBDXSync::rgbd6Callback(
+		  const rtabmap_msgs::RGBDImageConstPtr& image0,
+		  const rtabmap_msgs::RGBDImageConstPtr& image1,
+		  const rtabmap_msgs::RGBDImageConstPtr& image2,
+		  const rtabmap_msgs::RGBDImageConstPtr& image3,
+		  const rtabmap_msgs::RGBDImageConstPtr& image4,
+		  const rtabmap_msgs::RGBDImageConstPtr& image5)
+{
+	callbackCalled_ = true;
+	rtabmap_msgs::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(6);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	output.rgbd_images[3]=(*image3);
+	output.rgbd_images[4]=(*image4);
+	output.rgbd_images[5]=(*image5);
+	rgbdImagesPub_.publish(output);
+}
+
+void RGBDXSync::rgbd7Callback(
+		  const rtabmap_msgs::RGBDImageConstPtr& image0,
+		  const rtabmap_msgs::RGBDImageConstPtr& image1,
+		  const rtabmap_msgs::RGBDImageConstPtr& image2,
+		  const rtabmap_msgs::RGBDImageConstPtr& image3,
+		  const rtabmap_msgs::RGBDImageConstPtr& image4,
+		  const rtabmap_msgs::RGBDImageConstPtr& image5,
+		  const rtabmap_msgs::RGBDImageConstPtr& image6)
+{
+	callbackCalled_ = true;
+	rtabmap_msgs::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(7);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	output.rgbd_images[3]=(*image3);
+	output.rgbd_images[4]=(*image4);
+	output.rgbd_images[5]=(*image5);
+	output.rgbd_images[6]=(*image6);
+	rgbdImagesPub_.publish(output);
+}
+
+void RGBDXSync::rgbd8Callback(
+		  const rtabmap_msgs::RGBDImageConstPtr& image0,
+		  const rtabmap_msgs::RGBDImageConstPtr& image1,
+		  const rtabmap_msgs::RGBDImageConstPtr& image2,
+		  const rtabmap_msgs::RGBDImageConstPtr& image3,
+		  const rtabmap_msgs::RGBDImageConstPtr& image4,
+		  const rtabmap_msgs::RGBDImageConstPtr& image5,
+		  const rtabmap_msgs::RGBDImageConstPtr& image6,
+		  const rtabmap_msgs::RGBDImageConstPtr& image7)
+{
+	callbackCalled_ = true;
+	rtabmap_msgs::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(8);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	output.rgbd_images[3]=(*image3);
+	output.rgbd_images[4]=(*image4);
+	output.rgbd_images[5]=(*image5);
+	output.rgbd_images[6]=(*image6);
+	output.rgbd_images[7]=(*image7);
+	rgbdImagesPub_.publish(output);
+}
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_sync::RGBDXSync, nodelet::Nodelet);
+}
+

rtabmap_sync/src/nodelets/stereo_sync.cpp

@@ -0,0 +1,268 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CompressedImage.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <boost/thread.hpp>
+
+#include "rtabmap_msgs/RGBDImage.h"
+
+#include "rtabmap/core/Compression.h"
+#include "rtabmap/utilite/UConversion.h"
+
+namespace rtabmap_sync
+{
+
+class StereoSync : public nodelet::Nodelet
+{
+public:
+	StereoSync() :
+		compressedRate_(0),
+		warningThread_(0),
+		callbackCalled_(false),
+		approxSync_(0),
+		exactSync_(0)
+	{}
+
+	virtual ~StereoSync()
+	{
+		if(approxSync_)
+			delete approxSync_;
+		if(exactSync_)
+			delete exactSync_;
+
+		if(warningThread_)
+		{
+			callbackCalled_=true;
+			warningThread_->join();
+			delete warningThread_;
+		}
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		bool approxSync = false;
+		double approxSyncMaxInterval = 0.0;
+		pnh.param("approx_sync", approxSync, approxSync);
+		if(approxSync)
+			pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("compressed_rate", compressedRate_, compressedRate_);
+
+		NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
+		NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval);
+		NODELET_INFO("%s: queue_size  = %d", getName().c_str(), queueSize);
+		NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
+
+		rgbdImagePub_ = nh.advertise<rtabmap_msgs::RGBDImage>("rgbd_image", 1);
+		rgbdImageCompressedPub_ = nh.advertise<rtabmap_msgs::RGBDImage>("rgbd_image/compressed", 1);
+
+		if(approxSync)
+		{
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
+			if(approxSyncMaxInterval>0.0)
+				approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			approxSync_->registerCallback(boost::bind(&StereoSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+		}
+		else
+		{
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
+			exactSync_->registerCallback(boost::bind(&StereoSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+		}
+
+		ros::NodeHandle left_nh(nh, "left");
+		ros::NodeHandle right_nh(nh, "right");
+		ros::NodeHandle left_pnh(pnh, "left");
+		ros::NodeHandle right_pnh(pnh, "right");
+		image_transport::ImageTransport rgb_it(left_nh);
+		image_transport::ImageTransport depth_it(right_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), left_pnh);
+		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), right_pnh);
+
+		imageLeftSub_.subscribe(rgb_it, left_nh.resolveName("image_rect"), 1, hintsRgb);
+		imageRightSub_.subscribe(depth_it, right_nh.resolveName("image_rect"), 1, hintsDepth);
+		cameraInfoLeftSub_.subscribe(left_nh, "camera_info", 1);
+		cameraInfoRightSub_.subscribe(right_nh, "camera_info", 1);
+
+		std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s \\\n   %s",
+							getName().c_str(),
+							approxSync?"approx":"exact",
+							approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+							imageLeftSub_.getTopic().c_str(),
+							imageRightSub_.getTopic().c_str(),
+							cameraInfoLeftSub_.getTopic().c_str(),
+							cameraInfoRightSub_.getTopic().c_str());
+
+		warningThread_ = new boost::thread(boost::bind(&StereoSync::warningLoop, this, subscribedTopicsMsg, approxSync));
+		NODELET_INFO("%s", subscribedTopicsMsg.c_str());
+	}
+
+	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
+	{
+		ros::Duration r(5.0);
+		while(!callbackCalled_)
+		{
+			r.sleep();
+			if(!callbackCalled_)
+			{
+				ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+						"header are set. %s%s",
+						getName().c_str(),
+						approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
+							"topics should have all the exact timestamp for the callback to be called.",
+						subscribedTopicsMsg.c_str());
+			}
+		}
+	}
+
+	void callback(
+			  const sensor_msgs::ImageConstPtr& imageLeft,
+			  const sensor_msgs::ImageConstPtr& imageRight,
+			  const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
+			  const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
+	{
+		callbackCalled_ = true;
+		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
+		{
+			double leftStamp = imageLeft->header.stamp.toSec();
+			double rightStamp = imageRight->header.stamp.toSec();
+			double leftInfoStamp = cameraInfoLeft->header.stamp.toSec();
+			double rightInfoStamp = cameraInfoRight->header.stamp.toSec();
+
+			double stampDiff = fabs(leftStamp - rightStamp);
+			if(stampDiff > 0.010)
+			{
+				NODELET_WARN("The time difference between left and right frames is "
+						"high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware "
+						"synchronized, use approx_sync:=false. Otherwise, you may want "
+						"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations.",
+						stampDiff,
+						leftStamp,
+						rightStamp);
+			}
+
+			rtabmap_msgs::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.rgb_camera_info = *cameraInfoLeft;
+			msg.depth_camera_info = *cameraInfoRight;
+
+			if(rgbdImageCompressedPub_.getNumSubscribers())
+			{
+				bool publishCompressed = true;
+				if (compressedRate_ > 0.0)
+				{
+					if ( lastCompressedPublished_ + ros::Duration(1.0/compressedRate_) > ros::Time::now())
+					{
+						NODELET_DEBUG("throttle last update at %f skipping", lastCompressedPublished_.toSec());
+						publishCompressed = false;
+					}
+				}
+
+				if(publishCompressed)
+				{
+					lastCompressedPublished_ = ros::Time::now();
+
+					rtabmap_msgs::RGBDImage msgCompressed = msg;
+
+					cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
+					imagePtr->toCompressedImageMsg(msgCompressed.rgb_compressed, cv_bridge::JPG);
+
+					cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageRight);
+					imageDepthPtr->toCompressedImageMsg(msgCompressed.depth_compressed, cv_bridge::JPG);
+
+					rgbdImageCompressedPub_.publish(msgCompressed);
+				}
+			}
+
+			if(rgbdImagePub_.getNumSubscribers())
+			{
+				msg.rgb = *imageLeft;
+				msg.depth = *imageRight;
+				rgbdImagePub_.publish(msg);
+			}
+
+			if( leftStamp != imageLeft->header.stamp.toSec() ||
+				rightStamp != imageRight->header.stamp.toSec())
+			{
+				NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+						"sure the node publishing the topics doesn't override the same data after publishing them. A "
+						"solution is to use this node within another nodelet manager. Stamps: "
+						"left%f->%f right=%f->%f",
+						leftStamp, imageLeft->header.stamp.toSec(),
+						rightStamp, imageRight->header.stamp.toSec());
+			}
+		}
+	}
+
+private:
+	double compressedRate_;
+	boost::thread * warningThread_;
+	bool callbackCalled_;
+	ros::Time lastCompressedPublished_;
+
+	ros::Publisher rgbdImagePub_;
+	ros::Publisher rgbdImageCompressedPub_;
+
+	image_transport::SubscriberFilter imageLeftSub_;
+	image_transport::SubscriberFilter imageRightSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeftSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRightSub_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
+	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_sync::StereoSync, nodelet::Nodelet);
+}
+