First version rtabmap_launch working

rtabmap_odom/CMakeLists.txt

@@ -0,0 +1,97 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rtabmap_odom)
+
+find_package(catkin REQUIRED COMPONENTS
+             cv_bridge image_geometry laser_geometry message_filters 
+             nav_msgs nodelet pcl_conversions pcl_ros pluginlib roscpp 
+             sensor_msgs rtabmap_conversions rtabmap_msgs rtabmap_util
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES rtabmap_odom rtabmap_odom_plugins
+  CATKIN_DEPENDS cv_bridge image_geometry laser_geometry message_filters 
+             nav_msgs nodelet pcl_conversions pcl_ros pluginlib roscpp 
+             sensor_msgs rtabmap_conversions rtabmap_msgs rtabmap_util
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+  ${catkin_INCLUDE_DIRS}
+)
+
+SET(rtabmap_odom_lib_src
+   src/OdometryROS.cpp
+   src/PluginInterface.cpp
+)
+  
+SET(rtabmap_odom_plugins_lib_src
+   src/nodelets/rgbd_odometry.cpp
+   src/nodelets/stereo_odometry.cpp
+   src/nodelets/rgbdicp_odometry.cpp
+   src/nodelets/icp_odometry.cpp
+)
+
+############################
+## Declare a cpp library
+############################
+add_library(rtabmap_odom
+   ${rtabmap_odom_lib_src}
+)
+add_library(rtabmap_odom_plugins
+   ${rtabmap_odom_plugins_lib_src}
+)
+
+target_link_libraries(rtabmap_odom
+  ${catkin_LIBRARIES}
+)
+target_link_libraries(rtabmap_odom_plugins
+  rtabmap_odom
+  ${catkin_LIBRARIES}
+)
+
+add_executable(rtabmap_rgbd_odometry src/RGBDOdometryNode.cpp)
+target_link_libraries(rtabmap_rgbd_odometry rtabmap_odom_plugins)
+set_target_properties(rtabmap_rgbd_odometry PROPERTIES OUTPUT_NAME "rgbd_odometry")
+
+add_executable(rtabmap_stereo_odometry src/StereoOdometryNode.cpp)
+target_link_libraries(rtabmap_stereo_odometry rtabmap_odom_plugins)
+set_target_properties(rtabmap_stereo_odometry PROPERTIES OUTPUT_NAME "stereo_odometry")
+
+add_executable(rtabmap_rgbdicp_odometry src/RGBDICPOdometryNode.cpp)
+target_link_libraries(rtabmap_rgbdicp_odometry rtabmap_odom_plugins)
+set_target_properties(rtabmap_rgbdicp_odometry PROPERTIES OUTPUT_NAME "rgbdicp_odometry")
+
+add_executable(rtabmap_icp_odometry src/ICPOdometryNode.cpp)
+target_link_libraries(rtabmap_icp_odometry rtabmap_odom_plugins)
+set_target_properties(rtabmap_icp_odometry PROPERTIES OUTPUT_NAME "icp_odometry")
+
+#############
+## Install ##
+#############
+
+install(TARGETS 
+   rtabmap_odom
+   rtabmap_odom_plugins 
+   rtabmap_rgbd_odometry 
+   rtabmap_icp_odometry
+   rtabmap_rgbdicp_odometry 
+   rtabmap_stereo_odometry
+   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+   FILES_MATCHING PATTERN "*.h"
+)
+
+install(FILES
+   nodelet_plugins.xml
+   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

rtabmap_odom/include/rtabmap_odom/OdometryROS.h

@@ -0,0 +1,161 @@
+/*
+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.
+*/
+
+#ifndef ODOMETRYROS_H_
+#define ODOMETRYROS_H_
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+
+#include <std_srvs/Empty.h>
+#include <std_msgs/Header.h>
+#include <sensor_msgs/Imu.h>
+
+#include <rtabmap_msgs/ResetPose.h>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Parameters.h>
+
+#include <boost/thread.hpp>
+
+#include "rtabmap_util/ULogToRosout.h"
+
+namespace rtabmap {
+class Odometry;
+}
+
+namespace rtabmap_odom {
+
+class OdometryROS : public nodelet::Nodelet
+{
+
+public:
+	OdometryROS(bool stereoParams, bool visParams, bool icpParams);
+	virtual ~OdometryROS();
+
+	void processData(rtabmap::SensorData & data, const std_msgs::Header & header);
+
+	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool resetToPose(rtabmap_msgs::ResetPose::Request&, rtabmap_msgs::ResetPose::Response&);
+	bool pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+
+	const std::string & frameId() const {return frameId_;}
+	const std::string & odomFrameId() const {return odomFrameId_;}
+	const std::string & guessFrameId() const {return guessFrameId_;}
+	const rtabmap::ParametersMap & parameters() const {return parameters_;}
+	bool isPaused() const {return paused_;}
+
+protected:
+	void startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync);
+	void callbackCalled() {callbackCalled_ = true;}
+
+	virtual void flushCallbacks() = 0;
+	tf::TransformListener & tfListener() {return tfListener_;}
+	double waitForTransformDuration() const {return waitForTransform_?waitForTransformDuration_:0.0;}
+	rtabmap::Transform velocityGuess() const;
+	double previousStamp() const {return previousStamp_;}
+	virtual void postProcessData(const rtabmap::SensorData & data, const std_msgs::Header & header) const {}
+
+private:
+	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync);
+	virtual void onInit();
+	virtual void onOdomInit() = 0;
+	virtual void updateParameters(rtabmap::ParametersMap & parameters) {}
+
+	void callbackIMU(const sensor_msgs::ImuConstPtr& msg);
+	void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity());
+
+private:
+	rtabmap::Odometry * odometry_;
+	boost::thread * warningThread_;
+	bool callbackCalled_;
+
+	// parameters
+	std::string frameId_;
+	std::string odomFrameId_;
+	std::string groundTruthFrameId_;
+	std::string groundTruthBaseFrameId_;
+	std::string guessFrameId_;
+	double guessMinTranslation_;
+	double guessMinRotation_;
+	double guessMinTime_;
+	bool publishTf_;
+	bool waitForTransform_;
+	double waitForTransformDuration_;
+	bool publishNullWhenLost_;
+	rtabmap::ParametersMap parameters_;
+
+	ros::Publisher odomPub_;
+	ros::Publisher odomInfoPub_;
+	ros::Publisher odomInfoLitePub_;
+	ros::Publisher odomLocalMap_;
+	ros::Publisher odomLocalScanMap_;
+	ros::Publisher odomLastFrame_;
+	ros::Publisher odomRgbdImagePub_;
+	ros::ServiceServer resetSrv_;
+	ros::ServiceServer resetToPoseSrv_;
+	ros::ServiceServer pauseSrv_;
+	ros::ServiceServer resumeSrv_;
+	ros::ServiceServer setLogDebugSrv_;
+	ros::ServiceServer setLogInfoSrv_;
+	ros::ServiceServer setLogWarnSrv_;
+	ros::ServiceServer setLogErrorSrv_;
+	tf2_ros::TransformBroadcaster tfBroadcaster_;
+	tf::TransformListener tfListener_;
+	ros::Subscriber imuSub_;
+
+	bool paused_;
+	int resetCountdown_;
+	int resetCurrentCount_;
+	bool stereoParams_;
+	bool visParams_;
+	bool icpParams_;
+	rtabmap::Transform guess_;
+	rtabmap::Transform guessPreviousPose_;
+	double previousStamp_;
+	double expectedUpdateRate_;
+	double maxUpdateRate_;
+	double minUpdateRate_;
+	int odomStrategy_;
+	bool waitIMUToinit_;
+	bool imuProcessed_;
+	std::map<double, rtabmap::IMU> imus_;
+	std::pair<rtabmap::SensorData, std_msgs::Header > bufferedData_;
+
+	rtabmap_util::ULogToRosout ulogToRosout_;
+};
+
+}
+
+#endif

rtabmap_odom/include/rtabmap_odom/PluginInterface.h

@@ -0,0 +1,45 @@
+#ifndef PLUGIN_INTERFACE_H_
+#define PLUGIN_INTERFACE_H_
+
+#include <ros/ros.h>
+#include <string>
+#include <sensor_msgs/PointCloud2.h>
+
+namespace rtabmap_odom
+{
+
+class PluginInterface
+{
+public:
+  PluginInterface();
+  virtual ~PluginInterface() {}
+
+  const std::string getName() const
+  {
+    return name_;
+  }
+
+  bool isEnabled() {
+    return enabled_;
+  }
+
+  void initialize(const std::string name, ros::NodeHandle & nh);
+
+  virtual sensor_msgs::PointCloud2 filterPointCloud(const sensor_msgs::PointCloud2 msg) = 0;
+
+protected:
+  /** @brief This is called at the end of initialize().  Override to
+   * implement subclass-specific initialization.
+   **/
+  virtual void onInitialize() {}
+
+  bool enabled_;  ///< Currently this var is managed by subclasses. TODO: make this managed by this class and/or container class.
+  std::string name_;
+  ros::NodeHandle nh_;
+
+};
+
+}  // namespace rtabmap_odom
+
+#endif  // PLUGIN_INTERFACE_H_
+

rtabmap_odom/nodelet_plugins.xml

@@ -0,0 +1,35 @@
+<library path="lib/librtabmap_odom_plugins"> 
+
+  <class name="rtabmap_odom/rgbd_odometry" 
+         type="rtabmap_odom::RGBDOdometry" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
+  
+  <class name="rtabmap_odom/stereo_odometry" 
+         type="rtabmap_odom::StereoOdometry" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
+  
+  <class name="rtabmap_odom/rgbdicp_odometry" 
+         type="rtabmap_odom::RGBDICPOdometry" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
+  
+  <class name="rtabmap_odom/icp_odometry" 
+         type="rtabmap_odom::ICPOdometry" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
+
+</library>

rtabmap_odom/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>rtabmap_odom</name>
+  <version>0.1.0</version>
+  <description>RTAB-Map's odometry package.</description>
+  <maintainer email="[email protected]">Mathieu Labbe</maintainer>
+  <author>Mathieu Labbe</author>
+  <license>BSD</license>
+  <url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
+  <url type="repository">https://github.com/introlab/rtabmap_ros</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>cv_bridge</depend>
+  <depend>image_geometry</depend>
+  <depend>laser_geometry</depend>
+  <depend>message_filters</depend>
+  <depend>nav_msgs</depend>
+  <depend>nodelet</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
+  <depend>pluginlib</depend>
+  <depend>roscpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>rtabmap_conversions</depend>
+  <depend>rtabmap_msgs</depend>
+  <depend>rtabmap_util</depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+  </export>
+</package>

rtabmap_odom/src/ICPOdometryNode.cpp

@@ -0,0 +1,78 @@
+/*
+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"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/core/Parameters.h>
+
+int main(int argc, char **argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+	ros::init(argc, argv, "icp_odometry");
+
+	// process "--params" argument
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--params") == 0)
+		{
+			rtabmap::ParametersMap parametersOdom = rtabmap::Parameters::getDefaultOdometryParameters(false, false, true);
+			for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
+			{
+				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
+				std::cout <<
+						str <<
+						std::setw(60 - str.size()) <<
+						" [" <<
+						rtabmap::Parameters::getDescription(iter->first).c_str() <<
+						"]" <<
+						std::endl;
+			}
+			ROS_WARN("Node will now exit after showing default odometry parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
+		else if(strcmp(argv[i], "--udebug") == 0)
+		{
+			ULogger::setLevel(ULogger::kDebug);
+		}
+		else if(strcmp(argv[i], "--uinfo") == 0)
+		{
+			ULogger::setLevel(ULogger::kInfo);
+		}
+		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_odom/icp_odometry", remap, nargv);
+	ros::spin();
+	return 0;
+}

rtabmap_odom/src/PluginInterface.cpp

@@ -0,0 +1,21 @@
+#include "rtabmap_odom/PluginInterface.h"
+
+namespace rtabmap_odom
+{
+
+PluginInterface::PluginInterface()
+  :  enabled_(false)
+  , name_()
+{
+}
+
+void PluginInterface::initialize(const std::string name, ros::NodeHandle & nh)
+{
+    name_ = name;
+    nh_ = ros::NodeHandle(nh, name);
+    onInitialize();
+}
+
+
+}  // end namespace rtabmap_odom
+

rtabmap_odom/src/RGBDICPOdometryNode.cpp

@@ -0,0 +1,78 @@
+/*
+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"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/core/Parameters.h>
+
+int main(int argc, char **argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+	ros::init(argc, argv, "rgbdicp_odometry");
+
+	// process "--params" argument
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--params") == 0)
+		{
+			rtabmap::ParametersMap parametersOdom = rtabmap::Parameters::getDefaultOdometryParameters(false, true, true);
+			for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
+			{
+				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
+				std::cout <<
+						str <<
+						std::setw(60 - str.size()) <<
+						" [" <<
+						rtabmap::Parameters::getDescription(iter->first).c_str() <<
+						"]" <<
+						std::endl;
+			}
+			ROS_WARN("Node will now exit after showing default odometry parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
+		else if(strcmp(argv[i], "--udebug") == 0)
+		{
+			ULogger::setLevel(ULogger::kDebug);
+		}
+		else if(strcmp(argv[i], "--uinfo") == 0)
+		{
+			ULogger::setLevel(ULogger::kInfo);
+		}
+		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_odom/rgbdicp_odometry", remap, nargv);
+	ros::spin();
+	return 0;
+}

rtabmap_odom/src/RGBDOdometryNode.cpp

@@ -0,0 +1,78 @@
+/*
+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"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/core/Parameters.h>
+
+int main(int argc, char **argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+	ros::init(argc, argv, "rgbd_odometry");
+
+	// process "--params" argument
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--params") == 0)
+		{
+			rtabmap::ParametersMap parametersOdom = rtabmap::Parameters::getDefaultOdometryParameters(false);
+			for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
+			{
+				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
+				std::cout <<
+						str <<
+						std::setw(60 - str.size()) <<
+						" [" <<
+						rtabmap::Parameters::getDescription(iter->first).c_str() <<
+						"]" <<
+						std::endl;
+			}
+			ROS_WARN("Node will now exit after showing default odometry parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
+		else if(strcmp(argv[i], "--udebug") == 0)
+		{
+			ULogger::setLevel(ULogger::kDebug);
+		}
+		else if(strcmp(argv[i], "--uinfo") == 0)
+		{
+			ULogger::setLevel(ULogger::kInfo);
+		}
+		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_odom/rgbd_odometry", remap, nargv);
+	ros::spin();
+	return 0;
+}

rtabmap_odom/src/StereoOdometryNode.cpp

@@ -0,0 +1,79 @@
+/*
+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"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/core/Parameters.h>
+
+int main(int argc, char **argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+	ros::init(argc, argv, "stereo_odometry");
+
+	// process "--params" argument
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--params") == 0)
+		{
+			rtabmap::ParametersMap parametersOdom = rtabmap::Parameters::getDefaultOdometryParameters(true);
+			for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
+			{
+				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
+				std::cout <<
+						str <<
+						std::setw(60 - str.size()) <<
+						" [" <<
+						rtabmap::Parameters::getDescription(iter->first).c_str() <<
+						"]" <<
+						std::endl;
+			}
+			ROS_WARN("Node will now exit after showing default odometry parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
+		else if(strcmp(argv[i], "--udebug") == 0)
+		{
+			ULogger::setLevel(ULogger::kDebug);
+		}
+		else if(strcmp(argv[i], "--uinfo") == 0)
+		{
+			ULogger::setLevel(ULogger::kInfo);
+		}
+		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_odom/stereo_odometry", remap, nargv);
+	ros::spin();
+	return 0;
+}

rtabmap_odom/src/nodelets/icp_odometry.cpp

@@ -0,0 +1,910 @@
+/*
+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_odom/OdometryROS.h>
+
+#include <pluginlib/class_list_macros.hpp>
+#include <pluginlib/class_loader.hpp>
+
+#include <nodelet/nodelet.h>
+
+#include <laser_geometry/laser_geometry.h>
+
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/transforms.h>
+
+#include "rtabmap_conversions/MsgConversion.h"
+#include "rtabmap_odom/PluginInterface.h"
+
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_surface.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+
+using namespace rtabmap;
+
+namespace rtabmap_odom
+{
+
+class ICPOdometry : public OdometryROS
+{
+public:
+	ICPOdometry() :
+		OdometryROS(false, false, true),
+		scanCloudMaxPoints_(0),
+		scanCloudIs2d_(false),
+		scanDownsamplingStep_(1),
+		scanRangeMin_(0),
+		scanRangeMax_(0),
+		scanVoxelSize_(0.0),
+		scanNormalK_(0),
+		scanNormalRadius_(0.0),
+		scanNormalGroundUp_(0.0),
+		deskewing_(false),
+		deskewingSlerp_(false),
+		plugin_loader_("rtabmap_odom", "rtabmap_odom::PluginInterface"),
+		scanReceived_(false),
+		cloudReceived_(false)
+	{
+	}
+
+	virtual ~ICPOdometry()
+	{
+		plugins_.clear();
+	}
+
+private:
+
+	virtual void onOdomInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 1;
+		pnh.param("queue_size",  queueSize, queueSize);
+		pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
+		pnh.param("scan_cloud_is_2d",  scanCloudIs2d_, scanCloudIs2d_);
+		pnh.param("scan_downsampling_step", scanDownsamplingStep_, scanDownsamplingStep_);
+		pnh.param("scan_range_min",  scanRangeMin_, scanRangeMin_);
+		pnh.param("scan_range_max",  scanRangeMax_, scanRangeMax_);
+		pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
+		pnh.param("scan_normal_k",   scanNormalK_, scanNormalK_);
+		pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
+		pnh.param("scan_normal_ground_up", scanNormalGroundUp_, scanNormalGroundUp_);
+		pnh.param("deskewing",  deskewing_, deskewing_);
+		pnh.param("deskewing_slerp",  deskewingSlerp_, deskewingSlerp_);
+
+		if (pnh.hasParam("plugins"))
+		{
+			XmlRpc::XmlRpcValue pluginsList;
+			pnh.getParam("plugins", pluginsList);
+
+			for (int32_t i = 0; i < pluginsList.size(); ++i)
+			{
+				std::string pluginName = static_cast<std::string>(pluginsList[i]["name"]);
+				std::string type = static_cast<std::string>(pluginsList[i]["type"]);
+				NODELET_INFO("IcpOdometry: Using plugin %s of type \"%s\"", pluginName.c_str(), type.c_str());
+				try {
+					boost::shared_ptr<rtabmap_odom::PluginInterface> plugin = plugin_loader_.createInstance(type);
+					plugins_.push_back(plugin);
+					plugin->initialize(pluginName, pnh);
+					if(!plugin->isEnabled())
+					{
+						NODELET_WARN("Plugin: %s is not enabled, filtering will not occur. \"enabled_\" member "
+									 "should be managed in subclasses. This can be ignored if the "
+									 "plugin should really be initialized as disabled.",
+									 plugin->getName().c_str());
+					}
+				}
+				catch(pluginlib::PluginlibException & ex) {
+					ROS_ERROR("Failed to load plugin %s. Error: %s", pluginName.c_str(), ex.what());
+				}
+
+			}
+		}
+
+		if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
+		{
+			ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
+					"The value is still used. Use \"scan_normal_k\" to avoid this warning.");
+			pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_);
+		}
+
+		NODELET_INFO("IcpOdometry: queue_size             = %d", queueSize);
+		NODELET_INFO("IcpOdometry: scan_cloud_max_points  = %d", scanCloudMaxPoints_);
+		NODELET_INFO("IcpOdometry: scan_cloud_is_2d       = %s", scanCloudIs2d_?"true":"false");
+		NODELET_INFO("IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
+		NODELET_INFO("IcpOdometry: scan_range_min         = %f m", scanRangeMin_);
+		NODELET_INFO("IcpOdometry: scan_range_max         = %f m", scanRangeMax_);
+		NODELET_INFO("IcpOdometry: scan_voxel_size        = %f m", scanVoxelSize_);
+		NODELET_INFO("IcpOdometry: scan_normal_k          = %d", scanNormalK_);
+		NODELET_INFO("IcpOdometry: scan_normal_radius     = %f m", scanNormalRadius_);
+		NODELET_INFO("IcpOdometry: scan_normal_ground_up  = %f", scanNormalGroundUp_);
+		NODELET_INFO("IcpOdometry: deskewing              = %s", deskewing_?"true":"false");
+		NODELET_INFO("IcpOdometry: deskewing_slerp        = %s", deskewingSlerp_?"true":"false");
+
+		scan_sub_ = nh.subscribe("scan", queueSize, &ICPOdometry::callbackScan, this);
+		cloud_sub_ = nh.subscribe("scan_cloud", queueSize, &ICPOdometry::callbackCloud, this);
+
+		filtered_scan_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_filtered_input_scan", 1);
+	}
+
+	virtual void updateParameters(ParametersMap & parameters)
+	{
+		//make sure we are using Reg/Strategy=0
+		ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
+		if(iter != parameters.end() && iter->second.compare("1") != 0)
+		{
+			ROS_WARN("ICP odometry works only with \"Reg/Strategy\"=1. Ignoring value %s.", iter->second.c_str());
+		}
+		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "1"));
+
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+		iter = parameters.find(Parameters::kIcpDownsamplingStep());
+		if(iter != parameters.end())
+		{
+			int value = uStr2Int(iter->second);
+			if(value > 1)
+			{
+				if(!pnh.hasParam("scan_downsampling_step"))
+				{
+					ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_downsampling_step\" for convenience. \"%s\" is set to 1.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
+					scanDownsamplingStep_ = value;
+					iter->second = "1";
+				}
+				else
+				{
+					ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_downsampling_step\" are set.", iter->first.c_str());
+				}
+			}
+		}
+		iter = parameters.find(Parameters::kIcpRangeMin());
+		if(iter != parameters.end())
+		{
+			float value = uStr2Float(iter->second);
+			if(value != 0.0f)
+			{
+				if(!pnh.hasParam("scan_range_min"))
+				{
+					ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_range_min\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
+					scanRangeMin_ = value;
+					iter->second = "0";
+				}
+				else
+				{
+					ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_min\" are set.", iter->first.c_str());
+				}
+			}
+		}
+		iter = parameters.find(Parameters::kIcpRangeMax());
+		if(iter != parameters.end())
+		{
+			float value = uStr2Float(iter->second);
+			if(value != 0.0f)
+			{
+				if(!pnh.hasParam("scan_range_max"))
+				{
+					ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_range_max\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
+					scanRangeMax_ = value;
+					iter->second = "0";
+				}
+				else
+				{
+					ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_max\" are set.", iter->first.c_str());
+				}
+			}
+		}
+		iter = parameters.find(Parameters::kIcpVoxelSize());
+		if(iter != parameters.end())
+		{
+			float value = uStr2Float(iter->second);
+			if(value != 0.0f)
+			{
+				if(!pnh.hasParam("scan_voxel_size"))
+				{
+					ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_voxel_size\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
+					scanVoxelSize_ = value;
+					iter->second = "0";
+				}
+				else
+				{
+					ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_voxel_size\" are set.", iter->first.c_str());
+				}
+			}
+		}
+		else if(pnh.hasParam("scan_voxel_size"))
+		{
+			NODELET_INFO("IcpOdometry: scan_voxel_size is set (%f), setting %s to 0", scanVoxelSize_, Parameters::kIcpVoxelSize().c_str());
+			parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0"));
+		}
+		iter = parameters.find(Parameters::kIcpPointToPlaneK());
+		if(iter != parameters.end())
+		{
+			int value = uStr2Int(iter->second);
+			if(value != 0)
+			{
+				if(!pnh.hasParam("scan_normal_k"))
+				{
+					ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_k\" for convenience.", iter->second.c_str(), iter->first.c_str());
+					scanNormalK_ = value;
+				}
+				else
+				{
+					NODELET_INFO("IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
+					iter->second = uNumber2Str(scanNormalK_);
+				}
+			}
+		}
+		else if(pnh.hasParam("scan_normal_k"))
+		{
+			NODELET_INFO("IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
+			parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneK(), uNumber2Str(scanNormalK_)));
+		}
+		iter = parameters.find(Parameters::kIcpPointToPlaneRadius());
+		if(iter != parameters.end())
+		{
+			float value = uStr2Float(iter->second);
+			if(value != 0.0f)
+			{
+				if(!pnh.hasParam("scan_normal_radius"))
+				{
+					ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_radius\" for convenience.", iter->second.c_str(), iter->first.c_str());
+					scanNormalRadius_ = value;
+				}
+				else
+				{
+					NODELET_INFO("IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
+					iter->second = uNumber2Str(scanNormalK_);
+				}
+			}
+		}
+		else if(pnh.hasParam("scan_normal_radius"))
+		{
+			NODELET_INFO("IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
+			parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneRadius(), uNumber2Str(scanNormalRadius_)));
+		}
+		iter = parameters.find(Parameters::kIcpPointToPlaneGroundNormalsUp());
+		if(iter != parameters.end())
+		{
+			float value = uStr2Float(iter->second);
+			if(value != 0.0f)
+			{
+				if(!pnh.hasParam("scan_normal_ground_up"))
+				{
+					ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_ground_up\" for convenience.", iter->second.c_str(), iter->first.c_str());
+					scanNormalGroundUp_ = value;
+				}
+				else
+				{
+					NODELET_INFO("IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
+					iter->second = uNumber2Str(scanNormalK_);
+				}
+			}
+		}
+		else if(pnh.hasParam("scan_normal_ground_up"))
+		{
+			NODELET_INFO("IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
+			parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(scanNormalGroundUp_)));
+		}
+	}
+
+	void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg)
+	{
+		if(cloudReceived_)
+		{
+			ROS_ERROR("%s is already receiving clouds on \"%s\", but also "
+					"just received a scan on \"%s\". Both subscribers cannot be "
+					"used at the same time! Disabling scan subscriber.",
+					this->getName().c_str(), cloud_sub_.getTopic().c_str(), scan_sub_.getTopic().c_str());
+			scan_sub_.shutdown();
+			return;
+		}
+		scanReceived_ = true;
+		if(this->isPaused())
+		{
+			return;
+		}
+
+		// make sure the frame of the laser is updated
+		Transform localScanTransform = rtabmap_conversions::getTransform(this->frameId(),
+				scanMsg->header.frame_id,
+				scanMsg->header.stamp,
+				this->tfListener(),
+				this->waitForTransformDuration());
+		if(localScanTransform.isNull())
+		{
+			ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec());
+			return;
+		}
+
+		//transform in scan frame
+		sensor_msgs::PointCloud2 scanOut;
+		laser_geometry::LaserProjection projection;
+
+		if(deskewing_ && (!guessFrameId().empty() || (frameId().compare(scanMsg->header.frame_id) != 0)))
+		{
+			// make sure the frame of the laser is updated during the whole scan time
+			rtabmap::Transform tmpT = rtabmap_conversions::getTransform(
+					scanMsg->header.frame_id,
+					guessFrameId().empty()?frameId():guessFrameId(),
+					scanMsg->header.stamp,
+					scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment),
+					this->tfListener(),
+					this->waitForTransformDuration());
+			if(tmpT.isNull())
+			{
+				return;
+			}
+
+			projection.transformLaserScanToPointCloud(
+					guessFrameId().empty()?frameId():guessFrameId(),
+					*scanMsg, 
+					scanOut,
+					this->tfListener(),
+					laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
+
+			if(guessFrameId().empty() && previousStamp() > 0 && !velocityGuess().isNull())
+			{
+				// deskew with constant velocity model (we are in frameId)
+				sensor_msgs::PointCloud2 scanOutDeskewed;
+				if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
+				{
+					ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
+					return;
+				}
+				scanOut = scanOutDeskewed;
+			}
+
+			sensor_msgs::PointCloud2 scanOutDeskewed;
+			if(!pcl_ros::transformPointCloud(scanMsg->header.frame_id, scanOut, scanOutDeskewed, this->tfListener()))
+			{
+				ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
+						(guessFrameId().empty()?frameId():guessFrameId()).c_str(), scanMsg->header.frame_id.c_str(), scanMsg->header.stamp.toSec());
+				return;
+			}
+			scanOut = scanOutDeskewed;
+		}
+		else
+		{
+			projection.projectLaser(*scanMsg, scanOut, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
+
+			if(deskewing_ && previousStamp() > 0 && !velocityGuess().isNull())
+			{
+				// deskew with constant velocity model
+				sensor_msgs::PointCloud2 scanOutDeskewed;
+				if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
+				{
+					ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
+					return;
+				}
+				scanOut = scanOutDeskewed;
+			}
+		}
+
+		bool hasIntensity = false;
+		for(unsigned int i=0; i<scanOut.fields.size(); ++i)
+		{
+			if(scanOut.fields[i].name.compare("intensity") == 0)
+			{
+				if(scanOut.fields[i].datatype == sensor_msgs::PointField::FLOAT32)
+				{
+					hasIntensity = true;
+				}
+				else
+				{
+					static bool warningShown = false;
+					if(!warningShown)
+					{
+						ROS_WARN("The input scan cloud has an \"intensity\" field "
+								"but the datatype (%d) is not supported. Intensity will be ignored. "
+								"This message is only shown once.", scanOut.fields[i].datatype);
+						warningShown = true;
+					}
+				}
+			}
+		}
+
+		pcl::PointCloud<pcl::PointXYZI>::Ptr pclScanI(new pcl::PointCloud<pcl::PointXYZI>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+
+		if(hasIntensity)
+		{
+			pcl::fromROSMsg(scanOut, *pclScanI);
+			pclScanI->is_dense = true;
+		}
+		else
+		{
+			pcl::fromROSMsg(scanOut, *pclScan);
+			pclScan->is_dense = true;
+		}
+
+		LaserScan scan;
+		int maxLaserScans = (int)scanMsg->ranges.size();
+		if(!pclScan->empty() || !pclScanI->empty())
+		{
+			if(scanDownsamplingStep_ > 1)
+			{
+				if(hasIntensity)
+				{
+					pclScanI = util3d::downsample(pclScanI, scanDownsamplingStep_);
+				}
+				else
+				{
+					pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+				}
+				maxLaserScans /= scanDownsamplingStep_;
+			}
+			if(scanVoxelSize_ > 0.0f)
+			{
+				float pointsBeforeFiltering;
+				float pointsAfterFiltering;
+				if(hasIntensity)
+				{
+					pointsBeforeFiltering = (float)pclScanI->size();
+					pclScanI = util3d::voxelize(pclScanI, scanVoxelSize_);
+					pointsAfterFiltering = (float)pclScanI->size();
+				}
+				else
+				{
+					pointsBeforeFiltering = (float)pclScan->size();
+					pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
+					pointsAfterFiltering = (float)pclScan->size();
+				}
+				float ratio = pointsAfterFiltering / pointsBeforeFiltering;
+				maxLaserScans = int(float(maxLaserScans) * ratio);
+			}
+			if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
+			{
+				//compute normals
+				pcl::PointCloud<pcl::Normal>::Ptr normals;
+				if(scanVoxelSize_ > 0.0f)
+				{
+					if(hasIntensity)
+					{
+						normals = util3d::computeNormals2D(pclScanI, scanNormalK_, scanNormalRadius_);
+					}
+					else
+					{
+						normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+					}
+				}
+				else
+				{
+					if(hasIntensity)
+					{
+						normals = util3d::computeFastOrganizedNormals2D(pclScanI, scanNormalK_, scanNormalRadius_);
+					}
+					else
+					{
+						normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+					}
+				}
+				pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanINormal;
+				pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal;
+				if(hasIntensity)
+				{
+					pclScanINormal.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
+					pcl::concatenateFields(*pclScanI, *normals, *pclScanINormal);
+					scan = util3d::laserScan2dFromPointCloud(*pclScanINormal);
+				}
+				else
+				{
+					pclScanNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
+					pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+					scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
+				}
+			}
+			else
+			{
+				if(hasIntensity)
+				{
+					scan = util3d::laserScan2dFromPointCloud(*pclScanI);
+				}
+				else
+				{
+					scan = util3d::laserScan2dFromPointCloud(*pclScan);
+				}
+			}
+		}
+
+		if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
+		{
+			scan = util3d::rangeFiltering(scan, scanRangeMin_, scanRangeMax_);
+		}
+
+		rtabmap::SensorData data(
+				LaserScan(scan,
+						maxLaserScans,
+						scanRangeMax_>0&&scanRangeMax_<scanMsg->range_max?scanRangeMax_:scanMsg->range_max,
+						localScanTransform),
+				cv::Mat(),
+				cv::Mat(),
+				rtabmap::CameraModel(),
+				0,
+				rtabmap_conversions::timestampFromROS(scanMsg->header.stamp));
+
+		this->processData(data, scanMsg->header);
+	}
+
+	void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg)
+	{
+		UASSERT_MSG(pointCloudMsg->data.size() == pointCloudMsg->row_step*pointCloudMsg->height,
+				uFormat("data=%d row_step=%d height=%d", pointCloudMsg->data.size(), pointCloudMsg->row_step, pointCloudMsg->height).c_str());
+		
+		if(scanReceived_)
+		{
+			ROS_ERROR("%s is already receiving scans on \"%s\", but also "
+					"just received a cloud on \"%s\". Both subscribers cannot be "
+					"used at the same time! Disabling cloud subscriber.",
+					this->getName().c_str(), scan_sub_.getTopic().c_str(), cloud_sub_.getTopic().c_str());
+			cloud_sub_.shutdown();
+			return;
+		}
+		cloudReceived_ = true;
+		if(this->isPaused())
+		{
+			return;
+		}
+
+		sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
+		if (!plugins_.empty())
+		{
+			if (plugins_[0]->isEnabled())
+			{
+				*cloudMsg = plugins_[0]->filterPointCloud(*pointCloudMsg);
+			}
+			else
+			{
+				*cloudMsg = *pointCloudMsg;
+			}
+
+			if (plugins_.size() > 1)
+			{
+				for (int i = 1; i < plugins_.size(); i++) {
+					if (plugins_[i]->isEnabled()) {
+						*cloudMsg = plugins_[i]->filterPointCloud(*cloudMsg);
+					}
+				}
+			}
+		}
+		else
+		{
+			*cloudMsg = *pointCloudMsg;
+		}
+
+		Transform localScanTransform = rtabmap_conversions::getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp, this->tfListener(), this->waitForTransformDuration());
+		if(localScanTransform.isNull())
+		{
+			ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec());
+			return;
+		}
+
+		if(deskewing_)
+		{
+			if(!guessFrameId().empty())
+			{
+				// deskew with TF
+				if(!rtabmap_conversions::deskew(*pointCloudMsg, *cloudMsg, guessFrameId(), tfListener(), waitForTransformDuration(), deskewingSlerp_))
+				{
+					ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
+					return;
+				}
+			}
+			else if(previousStamp() > 0 && !velocityGuess().isNull())
+			{
+				// deskew with constant velocity model
+				bool alreadyInBaseFrame = frameId().compare(pointCloudMsg->header.frame_id) == 0;
+				sensor_msgs::PointCloud2Ptr cloudInBaseFrame;
+				sensor_msgs::PointCloud2Ptr cloudPtr = cloudMsg;
+				if(!alreadyInBaseFrame)
+				{
+					// transform in base frame
+					cloudInBaseFrame.reset(new sensor_msgs::PointCloud2);
+					if(!pcl_ros::transformPointCloud(frameId(), *pointCloudMsg, *cloudInBaseFrame, this->tfListener()))
+					{
+						ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
+								pointCloudMsg->header.frame_id.c_str(), frameId().c_str(), pointCloudMsg->header.stamp.toSec());
+						return;
+					}
+					cloudPtr = cloudInBaseFrame;
+				}
+
+				sensor_msgs::PointCloud2::Ptr cloudDeskewed(new sensor_msgs::PointCloud2);
+				if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp(), velocityGuess()))
+				{
+					ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
+					return;
+				}
+
+				if(!alreadyInBaseFrame)
+				{
+					// put back in scan frame
+					if(!pcl_ros::transformPointCloud(pointCloudMsg->header.frame_id.c_str(), *cloudDeskewed, *cloudMsg, this->tfListener()))
+					{
+						ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
+								frameId().c_str(), pointCloudMsg->header.frame_id.c_str(), pointCloudMsg->header.stamp.toSec());
+						return;
+					}
+				}
+				else
+				{
+					cloudMsg = cloudDeskewed;
+				}
+			}
+		}
+
+		LaserScan scan;
+		bool hasNormals = false;
+		bool hasIntensity = false;
+		bool is3D = false;
+		for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
+		{
+			if(scanVoxelSize_ == 0.0f && cloudMsg->fields[i].name.compare("normal_x") == 0)
+			{
+				hasNormals = true;
+			}
+			if(cloudMsg->fields[i].name.compare("z") == 0 && !scanCloudIs2d_)
+			{
+				is3D = true;
+			}
+			if(cloudMsg->fields[i].name.compare("intensity") == 0)
+			{
+				if(cloudMsg->fields[i].datatype == sensor_msgs::PointField::FLOAT32)
+				{
+					hasIntensity = true;
+				}
+				else
+				{
+					static bool warningShown = false;
+					if(!warningShown)
+					{
+						ROS_WARN("The input scan cloud has an \"intensity\" field "
+								"but the datatype (%d) is not supported. Intensity will be ignored. "
+								"This message is only shown once.", cloudMsg->fields[i].datatype);
+						warningShown = true;
+					}
+				}
+			}
+		}
+
+		if(scanCloudMaxPoints_ == 0 && cloudMsg->height > 1)
+		{
+			scanCloudMaxPoints_ = cloudMsg->height * cloudMsg->width;
+			NODELET_WARN("IcpOdometry: \"scan_cloud_max_points\" is not set but input "
+					"cloud is not dense, for convenience it will be set to %d (%dx%d)",
+					scanCloudMaxPoints_, cloudMsg->width, cloudMsg->height);
+		}
+		else if(cloudMsg->height > 1 && scanCloudMaxPoints_ < cloudMsg->height * cloudMsg->width)
+		{
+			NODELET_WARN("IcpOdometry: \"scan_cloud_max_points\" is set to %d but input "
+					"cloud is not dense and has a size of %d (%dx%d), setting to this later size.",
+					scanCloudMaxPoints_, cloudMsg->width *cloudMsg->height, cloudMsg->width, cloudMsg->height);
+			scanCloudMaxPoints_ = cloudMsg->width *cloudMsg->height;
+		}
+		int maxLaserScans = scanCloudMaxPoints_;
+
+		if(hasNormals && hasIntensity)
+		{
+			pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
+			pcl::fromROSMsg(*cloudMsg, *pclScan);
+			if(pclScan->size() && scanDownsamplingStep_ > 1)
+			{
+				pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+				if(pclScan->height>1)
+				{
+					maxLaserScans = pclScan->height * pclScan->width;
+				}
+				else
+				{
+					maxLaserScans /= scanDownsamplingStep_;
+				}
+			}
+			scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
+		}
+		else if(hasNormals)
+		{
+			pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
+			pcl::fromROSMsg(*cloudMsg, *pclScan);
+			if(pclScan->size() && scanDownsamplingStep_ > 1)
+			{
+				pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+				if(pclScan->height>1)
+				{
+					maxLaserScans = pclScan->height * pclScan->width;
+				}
+				else
+				{
+					maxLaserScans /= scanDownsamplingStep_;
+				}
+			}
+			scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
+		}
+		else if(hasIntensity)
+		{
+			pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
+			pcl::fromROSMsg(*cloudMsg, *pclScan);
+			if(pclScan->size() && scanDownsamplingStep_ > 1)
+			{
+				pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+				if(pclScan->height>1)
+				{
+					maxLaserScans = pclScan->height * pclScan->width;
+				}
+				else
+				{
+					maxLaserScans /= scanDownsamplingStep_;
+				}
+			}
+			if(!pclScan->is_dense)
+			{
+				pclScan = util3d::removeNaNFromPointCloud(pclScan);
+			}
+
+			if(pclScan->size())
+			{
+				if(scanVoxelSize_ > 0.0f)
+				{
+					float pointsBeforeFiltering = (float)pclScan->size();
+					pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
+					float ratio = float(pclScan->size()) / pointsBeforeFiltering;
+					maxLaserScans = int(float(maxLaserScans) * ratio);
+				}
+				if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
+				{
+					//compute normals
+					pcl::PointCloud<pcl::Normal>::Ptr normals = is3D?
+							util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_):
+							util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+					pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointXYZINormal>);
+					pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+					scan = is3D?util3d::laserScanFromPointCloud(*pclScanNormal):util3d::laserScan2dFromPointCloud(*pclScanNormal);
+				}
+				else
+				{
+					scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
+				}
+			}
+		}
+		else
+		{
+			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::fromROSMsg(*cloudMsg, *pclScan);
+			if(pclScan->size() && scanDownsamplingStep_ > 1)
+			{
+				pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+				if(pclScan->height>1)
+				{
+					maxLaserScans = pclScan->height * pclScan->width;
+				}
+				else
+				{
+					maxLaserScans /= scanDownsamplingStep_;
+				}
+			}
+			if(!pclScan->is_dense)
+			{
+				pclScan = util3d::removeNaNFromPointCloud(pclScan);
+			}
+
+			if(pclScan->size())
+			{
+				if(scanVoxelSize_ > 0.0f)
+				{
+					float pointsBeforeFiltering = (float)pclScan->size();
+					pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
+					float ratio = float(pclScan->size()) / pointsBeforeFiltering;
+					maxLaserScans = int(float(maxLaserScans) * ratio);
+				}
+				if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
+				{
+					//compute normals
+					pcl::PointCloud<pcl::Normal>::Ptr normals = is3D?
+							util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_):
+							util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+					pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+					pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+					scan = is3D?util3d::laserScanFromPointCloud(*pclScanNormal):util3d::laserScan2dFromPointCloud(*pclScanNormal);
+				}
+				else
+				{
+					scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
+				}
+			}
+		}
+
+		LaserScan laserScan(scan,
+				maxLaserScans,
+				0,
+				localScanTransform);
+		if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
+		{
+			laserScan = util3d::rangeFiltering(laserScan, scanRangeMin_, scanRangeMax_);
+		}
+		if(!laserScan.isEmpty() && laserScan.hasNormals() && !laserScan.is2d() && scanNormalGroundUp_)
+		{
+			laserScan = util3d::adjustNormalsToViewPoint(laserScan, Eigen::Vector3f(0,0,10), (float)scanNormalGroundUp_);
+		}
+
+		rtabmap::SensorData data(
+				laserScan,
+				cv::Mat(),
+				cv::Mat(),
+				rtabmap::CameraModel(),
+				0,
+				rtabmap_conversions::timestampFromROS(cloudMsg->header.stamp));
+
+		this->processData(data, cloudMsg->header);
+	}
+
+protected:
+	virtual void flushCallbacks()
+	{
+		// flush callbacks
+	}
+
+	void postProcessData(const SensorData & data, const std_msgs::Header & header) const
+	{
+		if(filtered_scan_pub_.getNumSubscribers())
+		{
+			sensor_msgs::PointCloud2 msg;
+			pcl_conversions::fromPCL(*rtabmap::util3d::laserScanToPointCloud2(data.laserScanRaw()), msg);
+			msg.header = header;
+			filtered_scan_pub_.publish(msg);
+		}
+	}
+
+private:
+	ros::Subscriber scan_sub_;
+	ros::Subscriber cloud_sub_;
+	ros::Publisher filtered_scan_pub_;
+	int scanCloudMaxPoints_;
+	bool scanCloudIs2d_;
+	int scanDownsamplingStep_;
+	double scanRangeMin_;
+	double scanRangeMax_;
+	double scanVoxelSize_;
+	int scanNormalK_;
+	double scanNormalRadius_;
+	double scanNormalGroundUp_;
+	bool deskewing_;
+	bool deskewingSlerp_;
+	std::vector<boost::shared_ptr<rtabmap_odom::PluginInterface> > plugins_;
+	pluginlib::ClassLoader<rtabmap_odom::PluginInterface> plugin_loader_;
+	bool scanReceived_ = false;
+	bool cloudReceived_ = false;
+
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_odom::ICPOdometry, nodelet::Nodelet);
+
+}

rtabmap_odom/src/nodelets/rgbd_odometry.cpp

@@ -0,0 +1,850 @@
+/*
+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_odom/OdometryROS.h>
+
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <image_geometry/stereo_camera_model.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include "rtabmap_conversions/MsgConversion.h"
+#include <rtabmap_msgs/RGBDImages.h>
+
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+
+using namespace rtabmap;
+
+namespace rtabmap_odom
+{
+
+class RGBDOdometry : public OdometryROS
+{
+public:
+	RGBDOdometry() :
+		OdometryROS(false, true, false),
+		approxSync_(0),
+		exactSync_(0),
+		approxSync2_(0),
+		exactSync2_(0),
+		approxSync3_(0),
+		exactSync3_(0),
+		approxSync4_(0),
+		exactSync4_(0),
+		approxSync5_(0),
+		exactSync5_(0),
+		queueSize_(5),
+		keepColor_(false)
+	{
+	}
+
+	virtual ~RGBDOdometry()
+	{
+		rgbdSub_.shutdown();
+		rgbdxSub_.shutdown();
+		if(approxSync_)
+		{
+			delete approxSync_;
+		}
+		if(exactSync_)
+		{
+			delete exactSync_;
+		}
+		if(approxSync2_)
+		{
+			delete approxSync2_;
+		}
+		if(exactSync2_)
+		{
+			delete exactSync2_;
+		}
+		if(approxSync3_)
+		{
+			delete approxSync3_;
+		}
+		if(exactSync3_)
+		{
+			delete exactSync3_;
+		}
+		if(approxSync4_)
+		{
+			delete approxSync4_;
+		}
+		if(exactSync4_)
+		{
+			delete exactSync4_;
+		}
+		if(approxSync5_)
+		{
+			delete approxSync5_;
+		}
+		if(exactSync5_)
+		{
+			delete exactSync5_;
+		}
+	}
+
+private:
+
+	virtual void onOdomInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int rgbdCameras = 1;
+		bool approxSync = true;
+		bool subscribeRGBD = 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("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
+		if(pnh.hasParam("depth_cameras"))
+		{
+			ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" with the \"rgbd_image\" input topics. \"subscribe_rgbd\" should be also set to true.");
+		}
+		pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
+		if(rgbdCameras < 0)
+		{
+			rgbdCameras = 0;
+		}
+		if(rgbdCameras > 5)
+		{
+			NODELET_FATAL("Only 5 cameras maximum supported yet. Set 0 to use rgbd_images input (for which rgbdx_sync node can sync up to 8 cameras).");
+		}
+		pnh.param("keep_color", keepColor_, keepColor_);
+
+		NODELET_INFO("RGBDOdometry: approx_sync    = %s", approxSync?"true":"false");
+		if(approxSync)
+			NODELET_INFO("RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
+		NODELET_INFO("RGBDOdometry: queue_size     = %d", queueSize_);
+		NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
+		NODELET_INFO("RGBDOdometry: rgbd_cameras   = %d", rgbdCameras);
+		NODELET_INFO("RGBDOdometry: keep_color     = %s", keepColor_?"true":"false");
+
+		std::string subscribedTopicsMsg;
+		if(subscribeRGBD)
+		{
+			if(rgbdCameras >= 2)
+			{
+				rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1);
+				rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1);
+				if(rgbdCameras >= 3)
+				{
+					rgbd_image3_sub_.subscribe(nh, "rgbd_image2", 1);
+				}
+				if(rgbdCameras >= 4)
+				{
+					rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1);
+				}
+				if(rgbdCameras >= 5)
+				{
+					rgbd_image5_sub_.subscribe(nh, "rgbd_image4", 1);
+				}
+
+				if(rgbdCameras == 2)
+				{
+					if(approxSync)
+					{
+						approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
+								MyApproxSync2Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_);
+						if(approxSyncMaxInterval > 0.0)
+							approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+						approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
+					}
+					else
+					{
+						exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
+								MyExactSync2Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_);
+						exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
+					}
+					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s",
+							getName().c_str(),
+							approxSync?"approx":"exact",
+							approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+							rgbd_image1_sub_.getTopic().c_str(),
+							rgbd_image2_sub_.getTopic().c_str());
+				}
+				else if(rgbdCameras == 3)
+				{
+					if(approxSync)
+					{
+						approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
+								MyApproxSync3Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_,
+								rgbd_image3_sub_);
+						if(approxSyncMaxInterval > 0.0)
+							approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+						approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+					}
+					else
+					{
+						exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
+								MyExactSync3Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_,
+								rgbd_image3_sub_);
+						exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+					}
+					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():"",
+							rgbd_image1_sub_.getTopic().c_str(),
+							rgbd_image2_sub_.getTopic().c_str(),
+							rgbd_image3_sub_.getTopic().c_str());
+				}
+				else if(rgbdCameras == 4)
+				{
+					if(approxSync)
+					{
+						approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
+								MyApproxSync4Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_,
+								rgbd_image3_sub_,
+								rgbd_image4_sub_);
+						if(approxSyncMaxInterval > 0.0)
+							approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+						approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+					}
+					else
+					{
+						exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
+								MyExactSync4Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_,
+								rgbd_image3_sub_,
+								rgbd_image4_sub_);
+						exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+					}
+					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():"",
+							rgbd_image1_sub_.getTopic().c_str(),
+							rgbd_image2_sub_.getTopic().c_str(),
+							rgbd_image3_sub_.getTopic().c_str(),
+							rgbd_image4_sub_.getTopic().c_str());
+				}
+				else if(rgbdCameras == 5)
+				{
+					if(approxSync)
+					{
+						approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
+								MyApproxSync5Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_,
+								rgbd_image3_sub_,
+								rgbd_image4_sub_,
+                                rgbd_image5_sub_);
+						if(approxSyncMaxInterval > 0.0)
+							approxSync5_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+						approxSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
+					}
+					else
+					{
+						exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
+								MyExactSync5Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_,
+								rgbd_image3_sub_,
+								rgbd_image4_sub_,
+								rgbd_image5_sub_);
+						exactSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
+					}
+					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %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():"",
+							rgbd_image1_sub_.getTopic().c_str(),
+							rgbd_image2_sub_.getTopic().c_str(),
+							rgbd_image3_sub_.getTopic().c_str(),
+							rgbd_image4_sub_.getTopic().c_str(),
+                                                        rgbd_image5_sub_.getTopic().c_str());
+				}
+
+			}
+			else if(rgbdCameras == 0)
+			{
+				rgbdxSub_ = nh.subscribe("rgbd_images", 1, &RGBDOdometry::callbackRGBDX, this);
+
+				subscribedTopicsMsg =
+						uFormat("\n%s subscribed to:\n   %s",
+						getName().c_str(),
+						rgbdxSub_.getTopic().c_str());
+			}
+			else
+			{
+				rgbdSub_ = nh.subscribe("rgbd_image", 1, &RGBDOdometry::callbackRGBD, this);
+
+				subscribedTopicsMsg =
+						uFormat("\n%s subscribed to:\n   %s",
+						getName().c_str(),
+						rgbdSub_.getTopic().c_str());
+			}
+		}
+		else
+		{
+			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);
+
+			image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+			image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+			info_sub_.subscribe(rgb_nh, "camera_info", 1);
+
+			if(approxSync)
+			{
+				approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+				if(approxSyncMaxInterval > 0.0)
+					approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+				approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+			}
+			else
+			{
+				exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+				exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+			}
+
+			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():"",
+					image_mono_sub_.getTopic().c_str(),
+					image_depth_sub_.getTopic().c_str(),
+					info_sub_.getTopic().c_str());
+		}
+		this->startWarningThread(subscribedTopicsMsg, approxSync);
+	}
+
+	virtual void updateParameters(ParametersMap & parameters)
+	{
+		//make sure we are using Reg/Strategy=0
+		ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
+		if(iter != parameters.end() && iter->second.compare("0") != 0)
+		{
+			ROS_WARN("RGBD odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
+		}
+		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
+
+		int estimationType = Parameters::defaultVisEstimationType();
+		Parameters::parse(parameters, Parameters::kVisEstimationType(), estimationType);
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+		int rgbdCameras = 1;
+		bool subscribeRGBD = false;
+		pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
+		pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
+	}
+
+	void commonCallback(
+				const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
+				const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
+				const std::vector<sensor_msgs::CameraInfo>& cameraInfos)
+	{
+		ROS_ASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
+		ros::Time higherStamp;
+		int imageWidth = rgbImages[0]->image.cols;
+		int imageHeight = rgbImages[0]->image.rows;
+		int depthWidth = depthImages[0]->image.cols;
+		int depthHeight = depthImages[0]->image.rows;
+
+		UASSERT_MSG(
+			imageWidth/depthWidth == imageHeight/depthHeight,
+			uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
+
+		int cameraCount = rgbImages.size();
+		cv::Mat rgb;
+		cv::Mat depth;
+		std::vector<rtabmap::CameraModel> cameraModels;
+		for(unsigned int i=0; i<rgbImages.size(); ++i)
+		{
+			if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
+				!(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+				 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
+				 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
+			{
+	 				NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and "
+	 				"image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
+	 					rgbImages[i]->encoding.c_str(),
+	 					depthImages[i]->encoding.c_str());
+	 				return;
+	 		}
+			UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
+					uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
+							imageWidth,
+							rgbImages[i]->image.cols,
+							imageHeight,
+							rgbImages[i]->image.rows).c_str());
+			UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight,
+					uFormat("depthWidth=%d vs %d depthHeight=%d vs %d",
+							depthWidth,
+							depthImages[i]->image.cols,
+							depthHeight,
+							depthImages[i]->image.rows).c_str());
+
+			ros::Time stamp = rgbImages[i]->header.stamp>depthImages[i]->header.stamp?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
+
+			if(i == 0)
+			{
+				higherStamp = stamp;
+			}
+			else if(stamp > higherStamp)
+			{
+				higherStamp = stamp;
+			}
+
+			Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp, this->tfListener(), this->waitForTransformDuration());
+			if(localTransform.isNull())
+			{
+				return;
+			}
+
+			if(i>0)
+			{
+				double stampDiff = fabs(rgbImages[i]->header.stamp.toSec() - rgbImages[i-1]->header.stamp.toSec());
+				if(stampDiff > 1.0/60.0)
+				{
+					static bool warningShown = false;
+					if(!warningShown)
+					{
+						NODELET_WARN("The time difference between cameras %d and %d is "
+								"high (diff=%fs, cam%d=%fs, cam%d=%fs). You may want "
+								"to set approx_sync_max_interval to reject bad synchronizations or use "
+								"approx_sync=false if streams have all the exact same timestamp. This "
+								"message is only printed once.",
+								i-1, i,
+								stampDiff,
+								i-1, rgbImages[i-1]->header.stamp.toSec(),
+								i, rgbImages[i]->header.stamp.toSec());
+						warningShown = true;
+					}
+				}
+			}
+
+
+			cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
+			if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+			   rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+			{
+				if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+				{
+					ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8");
+				}
+				else
+				{
+					ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
+				}
+			}
+
+			cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
+
+			// initialize
+			if(rgb.empty())
+			{
+				rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
+			}
+			if(depth.empty())
+			{
+				depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
+			}
+
+			if(ptrImage->image.type() == rgb.type())
+			{
+				ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
+			}
+			else
+			{
+				NODELET_ERROR("Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type());
+				return;
+			}
+
+			if(ptrDepth->image.type() == depth.type())
+			{
+				ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
+			}
+			else
+			{
+				NODELET_ERROR("Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type());
+				return;
+			}
+
+			cameraModels.push_back(rtabmap_conversions::cameraModelFromROS(cameraInfos[i], localTransform));
+		}
+
+		rtabmap::SensorData data(
+				rgb,
+				depth,
+				cameraModels,
+				0,
+				rtabmap_conversions::timestampFromROS(higherStamp));
+
+		std_msgs::Header header;
+		header.stamp = higherStamp;
+		header.frame_id = rgbImages.size()==1?rgbImages[0]->header.frame_id:"";
+		this->processData(data, header);
+	}
+
+	void callback(
+			const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& depth,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
+			std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
+			std::vector<sensor_msgs::CameraInfo> infoMsgs;
+			imageMsgs[0] = cv_bridge::toCvShare(image);
+			depthMsgs[0] = cv_bridge::toCvShare(depth);
+			infoMsgs.push_back(*cameraInfo);
+
+			double stampDiff = fabs(image->header.stamp.toSec() - depth->header.stamp.toSec());
+			if(stampDiff > 0.020)
+			{
+				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.02s to reject spurious bad synchronizations or use "
+						"approx_sync=false if streams have all the exact same timestamp.",
+						stampDiff,
+						image->header.stamp.toSec(),
+						depth->header.stamp.toSec());
+			}
+
+			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		}
+	}
+
+	void callbackRGBD(
+			const rtabmap_msgs::RGBDImageConstPtr& image)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
+			std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
+			std::vector<sensor_msgs::CameraInfo> infoMsgs;
+			rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
+			infoMsgs.push_back(image->rgb_camera_info);
+
+			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		}
+	}
+
+	void callbackRGBDX(
+			const rtabmap_msgs::RGBDImagesConstPtr& images)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			if(images->rgbd_images.empty())
+			{
+				NODELET_ERROR("Input topic \"%s\" doesn't contain any image(s)!", rgbdxSub_.getTopic().c_str());
+				return;
+			}
+			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(images->rgbd_images.size());
+			std::vector<cv_bridge::CvImageConstPtr> depthMsgs(images->rgbd_images.size());
+			std::vector<sensor_msgs::CameraInfo> infoMsgs;
+			for(size_t i=0; i<images->rgbd_images.size(); ++i)
+			{
+				rtabmap_conversions::toCvShare(images->rgbd_images[i], images, imageMsgs[i], depthMsgs[i]);
+				infoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
+			}
+
+			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		}
+	}
+
+	void callbackRGBD2(
+			const rtabmap_msgs::RGBDImageConstPtr& image,
+			const rtabmap_msgs::RGBDImageConstPtr& image2)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
+			std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2);
+			std::vector<sensor_msgs::CameraInfo> infoMsgs;
+			rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
+			rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
+			infoMsgs.push_back(image->rgb_camera_info);
+			infoMsgs.push_back(image2->rgb_camera_info);
+
+			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		}
+	}
+
+	void callbackRGBD3(
+			const rtabmap_msgs::RGBDImageConstPtr& image,
+			const rtabmap_msgs::RGBDImageConstPtr& image2,
+			const rtabmap_msgs::RGBDImageConstPtr& image3)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3);
+			std::vector<cv_bridge::CvImageConstPtr> depthMsgs(3);
+			std::vector<sensor_msgs::CameraInfo> infoMsgs;
+			rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
+			rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
+			rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
+			infoMsgs.push_back(image->rgb_camera_info);
+			infoMsgs.push_back(image2->rgb_camera_info);
+			infoMsgs.push_back(image3->rgb_camera_info);
+
+			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		}
+	}
+
+	void callbackRGBD4(
+			const rtabmap_msgs::RGBDImageConstPtr& image,
+			const rtabmap_msgs::RGBDImageConstPtr& image2,
+			const rtabmap_msgs::RGBDImageConstPtr& image3,
+			const rtabmap_msgs::RGBDImageConstPtr& image4)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4);
+			std::vector<cv_bridge::CvImageConstPtr> depthMsgs(4);
+			std::vector<sensor_msgs::CameraInfo> infoMsgs;
+			rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
+			rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
+			rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
+			rtabmap_conversions::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
+			infoMsgs.push_back(image->rgb_camera_info);
+			infoMsgs.push_back(image2->rgb_camera_info);
+			infoMsgs.push_back(image3->rgb_camera_info);
+			infoMsgs.push_back(image4->rgb_camera_info);
+
+			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		}
+	}
+
+	void callbackRGBD5(
+			const rtabmap_msgs::RGBDImageConstPtr& image,
+			const rtabmap_msgs::RGBDImageConstPtr& image2,
+			const rtabmap_msgs::RGBDImageConstPtr& image3,
+			const rtabmap_msgs::RGBDImageConstPtr& image4,
+			const rtabmap_msgs::RGBDImageConstPtr& image5)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5);
+			std::vector<cv_bridge::CvImageConstPtr> depthMsgs(5);
+			std::vector<sensor_msgs::CameraInfo> infoMsgs;
+			rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
+			rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
+			rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
+			rtabmap_conversions::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
+			rtabmap_conversions::toCvShare(image5, imageMsgs[4], depthMsgs[4]);
+			infoMsgs.push_back(image->rgb_camera_info);
+			infoMsgs.push_back(image2->rgb_camera_info);
+			infoMsgs.push_back(image3->rgb_camera_info);
+			infoMsgs.push_back(image4->rgb_camera_info);
+			infoMsgs.push_back(image5->rgb_camera_info);
+
+			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		}
+	}
+
+protected:
+	virtual void flushCallbacks()
+	{
+		// flush callbacks
+		if(approxSync_)
+		{
+			delete approxSync_;
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+			approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+		}
+		if(exactSync_)
+		{
+			delete exactSync_;
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+			exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+		}
+		if(approxSync2_)
+		{
+			delete approxSync2_;
+			approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
+					MyApproxSync2Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_);
+			approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
+		}
+		if(exactSync2_)
+		{
+			delete exactSync2_;
+			exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
+					MyExactSync2Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_);
+			exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
+		}
+		if(approxSync3_)
+		{
+			delete approxSync3_;
+			approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
+					MyApproxSync3Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_,
+					rgbd_image3_sub_);
+			approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+		}
+		if(exactSync3_)
+		{
+			delete exactSync3_;
+			exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
+					MyExactSync3Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_,
+					rgbd_image3_sub_);
+			exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+		}
+		if(approxSync4_)
+		{
+			delete approxSync4_;
+			approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
+					MyApproxSync4Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_,
+					rgbd_image3_sub_,
+					rgbd_image4_sub_);
+			approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+		}
+		if(exactSync4_)
+		{
+			delete exactSync4_;
+			exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
+					MyExactSync4Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_,
+					rgbd_image3_sub_,
+					rgbd_image4_sub_);
+			exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+		}
+		if(approxSync5_)
+		{
+			delete approxSync5_;
+			approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
+					MyApproxSync5Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_,
+					rgbd_image3_sub_,
+					rgbd_image4_sub_,
+                                        rgbd_image5_sub_);
+			approxSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
+		}
+		if(exactSync5_)
+		{
+			delete exactSync5_;
+			exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
+					MyExactSync5Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_,
+					rgbd_image3_sub_,
+					rgbd_image4_sub_,
+                                        rgbd_image5_sub_);
+			exactSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
+		}
+	}
+
+private:
+	image_transport::SubscriberFilter image_mono_sub_;
+	image_transport::SubscriberFilter image_depth_sub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
+
+	ros::Subscriber rgbdSub_;
+	ros::Subscriber rgbdxSub_;
+	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image1_sub_;
+	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image2_sub_;
+	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image3_sub_;
+	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image4_sub_;
+	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image5_sub_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
+	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync2Policy;
+	message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync2Policy;
+	message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync3Policy;
+	message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync3Policy;
+	message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync4Policy;
+	message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync4Policy;
+	message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync5Policy;
+	message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync5Policy;
+	message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
+	int queueSize_;
+	bool keepColor_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_odom::RGBDOdometry, nodelet::Nodelet);
+
+}

rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp

@@ -0,0 +1,511 @@
+/*
+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_odom/OdometryROS.h>
+
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <image_geometry/stereo_camera_model.h>
+#include <laser_geometry/laser_geometry.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include "rtabmap_conversions/MsgConversion.h"
+
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_surface.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+
+using namespace rtabmap;
+
+namespace rtabmap_odom
+{
+
+class RGBDICPOdometry : public OdometryROS
+{
+public:
+	RGBDICPOdometry() :
+		OdometryROS(false, true, true),
+		approxScanSync_(0),
+		exactScanSync_(0),
+		approxCloudSync_(0),
+		exactCloudSync_(0),
+		queueSize_(5),
+		keepColor_(false),
+		scanCloudMaxPoints_(0),
+		scanVoxelSize_(0.0),
+		scanNormalK_(0),
+		scanNormalRadius_(0.0)
+	{
+	}
+
+	virtual ~RGBDICPOdometry()
+	{
+		if(approxScanSync_)
+		{
+			delete approxScanSync_;
+		}
+		if(exactScanSync_)
+		{
+			delete exactScanSync_;
+		}
+		if(approxCloudSync_)
+		{
+			delete approxCloudSync_;
+		}
+		if(exactCloudSync_)
+		{
+			delete exactCloudSync_;
+		}
+	}
+
+private:
+
+	virtual void onOdomInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		bool approxSync = true;
+		bool subscribeScanCloud = 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("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud);
+		pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
+		pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
+		pnh.param("scan_normal_k", scanNormalK_, scanNormalK_);
+		if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
+		{
+			ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
+					"The value is still used. Use \"scan_normal_k\" to avoid this warning.");
+			pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_);
+		}
+		pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
+		pnh.param("keep_color", keepColor_, keepColor_);
+
+		NODELET_INFO("RGBDIcpOdometry: approx_sync           = %s", approxSync?"true":"false");
+		if(approxSync)
+			NODELET_INFO("RGBDIcpOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
+		NODELET_INFO("RGBDIcpOdometry: queue_size            = %d", queueSize_);
+		NODELET_INFO("RGBDIcpOdometry: subscribe_scan_cloud  = %s", subscribeScanCloud?"true":"false");
+		NODELET_INFO("RGBDIcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
+		NODELET_INFO("RGBDIcpOdometry: scan_voxel_size       = %f", scanVoxelSize_);
+		NODELET_INFO("RGBDIcpOdometry: scan_normal_k         = %d", scanNormalK_);
+		NODELET_INFO("RGBDIcpOdometry: scan_normal_radius    = %f", scanNormalRadius_);
+		NODELET_INFO("RGBDIcpOdometry: keep_color            = %s", keepColor_?"true":"false");
+
+		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);
+
+		image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		info_sub_.subscribe(rgb_nh, "camera_info", 1);
+
+		std::string subscribedTopicsMsg;
+		if(subscribeScanCloud)
+		{
+			cloud_sub_.subscribe(nh, "scan_cloud", 1);
+			if(approxSync)
+			{
+				approxCloudSync_ = new message_filters::Synchronizer<MyApproxCloudSyncPolicy>(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
+				if(approxSyncMaxInterval > 0.0)
+					approxCloudSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+				approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+			}
+			else
+			{
+				exactCloudSync_ = new message_filters::Synchronizer<MyExactCloudSyncPolicy>(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
+				exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+			}
+
+			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():"",
+					image_mono_sub_.getTopic().c_str(),
+					image_depth_sub_.getTopic().c_str(),
+					info_sub_.getTopic().c_str(),
+					cloud_sub_.getTopic().c_str());
+		}
+		else
+		{
+			scan_sub_.subscribe(nh, "scan", 1);
+			if(approxSync)
+			{
+				approxScanSync_ = new message_filters::Synchronizer<MyApproxScanSyncPolicy>(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
+				if(approxSyncMaxInterval > 0.0)
+					approxScanSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+				approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+			}
+			else
+			{
+				exactScanSync_ = new message_filters::Synchronizer<MyExactScanSyncPolicy>(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
+				exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+			}
+
+			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():"",
+					image_mono_sub_.getTopic().c_str(),
+					image_depth_sub_.getTopic().c_str(),
+					info_sub_.getTopic().c_str(),
+					scan_sub_.getTopic().c_str());
+		}
+		this->startWarningThread(subscribedTopicsMsg, approxSync);
+	}
+
+	virtual void updateParameters(ParametersMap & parameters)
+	{
+		//make sure we are using Reg/Strategy=0
+		ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
+		if(iter != parameters.end() && iter->second.compare("0") != 0)
+		{
+			ROS_WARN("RGBDICP odometry works only with \"Reg/Strategy\"=2. Ignoring value %s.", iter->second.c_str());
+		}
+		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
+	}
+
+	void callbackScan(
+			const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& depth,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo,
+			const sensor_msgs::LaserScanConstPtr& scanMsg)
+	{
+		sensor_msgs::PointCloud2ConstPtr cloudMsg;
+		callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg);
+	}
+
+	void callbackCloud(
+			const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& depth,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo,
+			const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
+	{
+		sensor_msgs::LaserScanConstPtr scanMsg;
+		callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg);
+	}
+
+	void callbackCommon(
+			const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& depth,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo,
+			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
+			   !(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
+				 depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
+				 depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
+			{
+				NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 "
+						  "recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s",
+						image->encoding.c_str(), depth->encoding.c_str());
+				return;
+			}
+
+			// use the highest stamp to make sure that there will be no future interpolation required when synchronized with another node
+			ros::Time stamp = image->header.stamp > depth->header.stamp? image->header.stamp : depth->header.stamp;
+			if(scanMsg.get() != 0)
+			{
+				if(stamp < scanMsg->header.stamp)
+				{
+					stamp = scanMsg->header.stamp;
+				}
+			}
+			else if(cloudMsg.get() != 0)
+			{
+				if(stamp < cloudMsg->header.stamp)
+				{
+					stamp = cloudMsg->header.stamp;
+				}
+			}
+
+			Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), image->header.frame_id, stamp, this->tfListener(), this->waitForTransformDuration());
+			if(localTransform.isNull())
+			{
+				return;
+			}
+
+			double stampDiff = fabs(image->header.stamp.toSec() - depth->header.stamp.toSec());
+			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,
+						image->header.stamp.toSec(),
+						depth->header.stamp.toSec());
+			}
+
+			if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
+			{
+				rtabmap::CameraModel rtabmapModel = rtabmap_conversions::cameraModelFromROS(*cameraInfo, localTransform);
+				cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image,
+						image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
+						image->encoding.compare(sensor_msgs::image_encodings::MONO8)==0?"":
+								keepColor_ && image->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0?"bgr8":"mono8");
+				cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
+
+				LaserScan scan;
+				Transform localScanTransform = Transform::getIdentity();
+				int maxLaserScans = 0;
+				if(scanMsg.get() != 0)
+				{
+					// make sure the frame of the laser is updated too
+					localScanTransform = rtabmap_conversions::getTransform(this->frameId(),
+							scanMsg->header.frame_id,
+							scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment),
+							this->tfListener(),
+							this->waitForTransformDuration());
+					if(localScanTransform.isNull())
+					{
+						ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec());
+						return;
+					}
+
+					//transform in frameId_ frame
+					sensor_msgs::PointCloud2 scanOut;
+					laser_geometry::LaserProjection projection;
+					projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
+					pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+					pcl::fromROSMsg(scanOut, *pclScan);
+					pclScan->is_dense = true;
+
+					maxLaserScans = (int)scanMsg->ranges.size();
+					if(pclScan->size())
+					{
+						if(scanVoxelSize_ > 0.0f)
+						{
+							float pointsBeforeFiltering = (float)pclScan->size();
+							pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
+							float ratio = float(pclScan->size()) / pointsBeforeFiltering;
+							maxLaserScans = int(float(maxLaserScans) * ratio);
+						}
+						if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
+						{
+							//compute normals
+							pcl::PointCloud<pcl::Normal>::Ptr normals;
+							if(scanVoxelSize_ > 0.0f)
+							{
+								normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+							}
+							else
+							{
+								normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+							}
+							pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+							pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+							scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
+						}
+						else
+						{
+							scan = util3d::laserScan2dFromPointCloud(*pclScan);
+						}
+					}
+				}
+				else if(cloudMsg.get() != 0)
+				{
+					UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
+							uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
+
+
+					bool containNormals = false;
+					if(scanVoxelSize_ == 0.0f)
+					{
+						for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
+						{
+							if(cloudMsg->fields[i].name.compare("normal_x") == 0)
+							{
+								containNormals = true;
+								break;
+							}
+						}
+					}
+					localScanTransform = rtabmap_conversions::getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp, this->tfListener(), this->waitForTransformDuration());
+					if(localScanTransform.isNull())
+					{
+						ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec());
+						return;
+					}
+
+					maxLaserScans = scanCloudMaxPoints_;
+					if(containNormals)
+					{
+						pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::fromROSMsg(*cloudMsg, *pclScan);
+						if(!pclScan->is_dense)
+						{
+							pclScan = util3d::removeNaNNormalsFromPointCloud(pclScan);
+						}
+						scan = util3d::laserScanFromPointCloud(*pclScan);
+					}
+					else
+					{
+						pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+						pcl::fromROSMsg(*cloudMsg, *pclScan);
+						if(!pclScan->is_dense)
+						{
+							pclScan = util3d::removeNaNFromPointCloud(pclScan);
+						}
+
+						if(pclScan->size())
+						{
+							if(scanVoxelSize_ > 0.0f)
+							{
+								float pointsBeforeFiltering = (float)pclScan->size();
+								pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
+								float ratio = float(pclScan->size()) / pointsBeforeFiltering;
+								maxLaserScans = int(float(maxLaserScans) * ratio);
+							}
+							if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
+							{
+								//compute normals
+								pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
+								pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+								pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+								scan = util3d::laserScanFromPointCloud(*pclScanNormal);
+							}
+							else
+							{
+								scan = util3d::laserScanFromPointCloud(*pclScan);
+							}
+						}
+					}
+				}
+
+				rtabmap::SensorData data(
+						LaserScan(scan,
+								scanMsg.get() != 0 || cloudMsg.get() != 0?maxLaserScans:0,
+								scanMsg.get() != 0?scanMsg->range_max:0,
+								localScanTransform),
+						ptrImage->image,
+						ptrDepth->image,
+						rtabmapModel,
+						0,
+						rtabmap_conversions::timestampFromROS(stamp));
+
+				std_msgs::Header header;
+				header.stamp = stamp;
+				header.frame_id = image->header.frame_id;
+				this->processData(data, header);
+			}
+		}
+	}
+
+protected:
+	virtual void flushCallbacks()
+	{
+		// flush callbacks
+		if(approxScanSync_)
+		{
+			delete approxScanSync_;
+			approxScanSync_ = new message_filters::Synchronizer<MyApproxScanSyncPolicy>(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
+			approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+		}
+		if(exactScanSync_)
+		{
+			delete exactScanSync_;
+			exactScanSync_ = new message_filters::Synchronizer<MyExactScanSyncPolicy>(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
+			exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+		}
+		if(approxCloudSync_)
+		{
+			delete approxCloudSync_;
+			approxCloudSync_ = new message_filters::Synchronizer<MyApproxCloudSyncPolicy>(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
+			approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+		}
+		if(exactCloudSync_)
+		{
+			delete exactCloudSync_;
+			exactCloudSync_ = new message_filters::Synchronizer<MyExactCloudSyncPolicy>(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
+			exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+		}
+	}
+
+private:
+	image_transport::SubscriberFilter image_mono_sub_;
+	image_transport::SubscriberFilter image_depth_sub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
+	message_filters::Subscriber<sensor_msgs::LaserScan> scan_sub_;
+	message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan> MyApproxScanSyncPolicy;
+	message_filters::Synchronizer<MyApproxScanSyncPolicy> * approxScanSync_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan> MyExactScanSyncPolicy;
+	message_filters::Synchronizer<MyExactScanSyncPolicy> * exactScanSync_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> MyApproxCloudSyncPolicy;
+	message_filters::Synchronizer<MyApproxCloudSyncPolicy> * approxCloudSync_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> MyExactCloudSyncPolicy;
+	message_filters::Synchronizer<MyExactCloudSyncPolicy> * exactCloudSync_;
+	int queueSize_;
+	bool keepColor_;
+	int scanCloudMaxPoints_;
+	double scanVoxelSize_;
+	int scanNormalK_;
+	double scanNormalRadius_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_odom::RGBDICPOdometry, nodelet::Nodelet);
+
+}

rtabmap_odom/src/nodelets/stereo_odometry.cpp

@@ -0,0 +1,847 @@
+/*
+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_odom/OdometryROS.h"
+#include "pluginlib/class_list_macros.hpp"
+#include "nodelet/nodelet.h"
+
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+
+#include <image_geometry/stereo_camera_model.h>
+
+#include <cv_bridge/cv_bridge.h>
+
+#include "rtabmap_conversions/MsgConversion.h"
+#include <rtabmap_msgs/RGBDImages.h>
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Odometry.h>
+
+using namespace rtabmap;
+
+namespace rtabmap_odom
+{
+
+class StereoOdometry : public OdometryROS
+{
+public:
+	StereoOdometry() :
+		OdometryROS(true, true, false),
+		approxSync_(0),
+		exactSync_(0),
+		approxSync2_(0),
+		exactSync2_(0),
+		approxSync3_(0),
+		exactSync3_(0),
+		approxSync4_(0),
+		exactSync4_(0),
+		queueSize_(5),
+		keepColor_(false)
+	{
+	}
+
+	virtual ~StereoOdometry()
+	{
+		if(approxSync_)
+		{
+			delete approxSync_;
+		}
+		if(exactSync_)
+		{
+			delete exactSync_;
+		}
+	}
+
+private:
+	virtual void onOdomInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		bool approxSync = false;
+		bool subscribeRGBD = false;
+		double approxSyncMaxInterval = 0.0;
+		int rgbdCameras = 1;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
+		pnh.param("queue_size", queueSize_, queueSize_);
+		pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
+		pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
+		pnh.param("keep_color", keepColor_, keepColor_);
+
+		NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false");
+		if(approxSync)
+			NODELET_INFO("StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
+		NODELET_INFO("StereoOdometry: queue_size = %d", queueSize_);
+		NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
+		NODELET_INFO("StereoOdometry: keep_color = %s", keepColor_?"true":"false");
+
+		std::string subscribedTopicsMsg;
+		if(subscribeRGBD)
+		{
+			if(rgbdCameras >= 2)
+			{
+				rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1);
+				rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1);
+				if(rgbdCameras >= 3)
+				{
+					rgbd_image3_sub_.subscribe(nh, "rgbd_image2", 1);
+				}
+				if(rgbdCameras >= 4)
+				{
+					rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1);
+				}
+
+				if(rgbdCameras == 2)
+				{
+					if(approxSync)
+					{
+						approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
+								MyApproxSync2Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_);
+						if(approxSyncMaxInterval > 0.0)
+							approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+						approxSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
+					}
+					else
+					{
+						exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
+								MyExactSync2Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_);
+						exactSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
+					}
+					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s",
+							getName().c_str(),
+							approxSync?"approx":"exact",
+							approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+							rgbd_image1_sub_.getTopic().c_str(),
+							rgbd_image2_sub_.getTopic().c_str());
+				}
+				else if(rgbdCameras == 3)
+				{
+					if(approxSync)
+					{
+						approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
+								MyApproxSync3Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_,
+								rgbd_image3_sub_);
+						if(approxSyncMaxInterval > 0.0)
+							approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+						approxSync3_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+					}
+					else
+					{
+						exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
+								MyExactSync3Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_,
+								rgbd_image3_sub_);
+						exactSync3_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+					}
+					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():"",
+							rgbd_image1_sub_.getTopic().c_str(),
+							rgbd_image2_sub_.getTopic().c_str(),
+							rgbd_image3_sub_.getTopic().c_str());
+				}
+				else if(rgbdCameras == 4)
+				{
+					if(approxSync)
+					{
+						approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
+								MyApproxSync4Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_,
+								rgbd_image3_sub_,
+								rgbd_image4_sub_);
+						if(approxSyncMaxInterval > 0.0)
+							approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+						approxSync4_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+					}
+					else
+					{
+						exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
+								MyExactSync4Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_,
+								rgbd_image3_sub_,
+								rgbd_image4_sub_);
+						exactSync4_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+					}
+					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():"",
+							rgbd_image1_sub_.getTopic().c_str(),
+							rgbd_image2_sub_.getTopic().c_str(),
+							rgbd_image3_sub_.getTopic().c_str(),
+							rgbd_image4_sub_.getTopic().c_str());
+				}
+				else
+				{
+					ROS_FATAL("%s doesn't support more than 4 cameras (rgbd_cameras=%d) with internal synchronization interface, set rgbd_cameras=0 and use rgbd_images input topic instead for more cameras.", getName().c_str(), rgbdCameras);
+				}
+
+			}
+			else if(rgbdCameras == 0)
+			{
+				rgbdxSub_ = nh.subscribe("rgbd_images", 1, &StereoOdometry::callbackRGBDX, this);
+
+				subscribedTopicsMsg =
+						uFormat("\n%s subscribed to:\n   %s",
+						getName().c_str(),
+						rgbdxSub_.getTopic().c_str());
+			}
+			else
+			{
+				rgbdSub_ = nh.subscribe("rgbd_image", 1, &StereoOdometry::callbackRGBD, this);
+
+				subscribedTopicsMsg =
+						uFormat("\n%s subscribed to:\n   %s",
+						getName().c_str(),
+						rgbdSub_.getTopic().c_str());
+			}
+		}
+		else
+		{
+			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"), 1, hintsLeft);
+			imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
+			cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
+			cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
+
+			if(approxSync)
+			{
+				approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+				if(approxSyncMaxInterval>0.0)
+					approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+				approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+			}
+			else
+			{
+				exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+				exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+			}
+
+
+			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():"",
+					imageRectLeft_.getTopic().c_str(),
+					imageRectRight_.getTopic().c_str(),
+					cameraInfoLeft_.getTopic().c_str(),
+					cameraInfoRight_.getTopic().c_str());
+		}
+
+		this->startWarningThread(subscribedTopicsMsg, approxSync);
+	}
+
+	virtual void updateParameters(ParametersMap & parameters)
+	{
+		//make sure we are using Reg/Strategy=0
+		ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
+		if(iter != parameters.end() && iter->second.compare("0") != 0)
+		{
+			ROS_WARN("Stereo odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
+		}
+		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
+	}
+
+	void commonCallback(
+			const std::vector<cv_bridge::CvImageConstPtr> & leftImages,
+			const std::vector<cv_bridge::CvImageConstPtr> & rightImages,
+			const std::vector<sensor_msgs::CameraInfo>& leftCameraInfos,
+			const std::vector<sensor_msgs::CameraInfo>& rightCameraInfos)
+	{
+		UASSERT(leftImages.size() > 0 &&
+				leftImages.size() == rightImages.size() &&
+				leftImages.size() == leftCameraInfos.size() &&
+				rightImages.size() == rightCameraInfos.size());
+		ros::Time higherStamp;
+		int leftWidth = leftImages[0]->image.cols;
+		int leftHeight = leftImages[0]->image.rows;
+		int rightWidth = rightImages[0]->image.cols;
+		int rightHeight = rightImages[0]->image.rows;
+
+		UASSERT_MSG(
+				leftWidth == rightWidth && leftHeight == rightHeight,
+			uFormat("left=%dx%d right=%dx%d", leftWidth, leftHeight, rightWidth, rightHeight).c_str());
+
+		int cameraCount = leftImages.size();
+		cv::Mat left;
+		cv::Mat right;
+		std::vector<rtabmap::StereoCameraModel> cameraModels;
+		for(unsigned int i=0; i<leftImages.size(); ++i)
+		{
+			if(!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+				 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+				 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+				 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+				 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
+				 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
+				 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
+				!(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+				  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+				  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+				  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+				  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
+				  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
+				  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
+			{
+				NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
+						leftImages[i]->encoding.c_str(), rightImages[i]->encoding.c_str());
+				return;
+			}
+
+			ros::Time stamp = leftImages[i]->header.stamp>rightImages[i]->header.stamp?leftImages[i]->header.stamp:rightImages[i]->header.stamp;
+
+			if(i == 0)
+			{
+				higherStamp = stamp;
+			}
+			else if(stamp > higherStamp)
+			{
+				higherStamp = stamp;
+			}
+
+			Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), leftImages[i]->header.frame_id, stamp, this->tfListener(), this->waitForTransformDuration());
+			if(localTransform.isNull())
+			{
+				return;
+			}
+
+			if(i>0)
+			{
+				double stampDiff = fabs(leftImages[i]->header.stamp.toSec() - leftImages[i-1]->header.stamp.toSec());
+				if(stampDiff > 1.0/60.0)
+				{
+					static bool warningShown = false;
+					if(!warningShown)
+					{
+						NODELET_WARN("The time difference between cameras %d and %d is "
+								"high (diff=%fs, cam%d=%fs, cam%d=%fs). You may want "
+								"to set approx_sync_max_interval to reject bad synchronizations or use "
+								"approx_sync=false if streams have all the exact same timestamp. This "
+								"message is only printed once.",
+								i-1, i,
+								stampDiff,
+								i-1, leftImages[i-1]->header.stamp.toSec(),
+								i, leftImages[i]->header.stamp.toSec());
+						warningShown = true;
+					}
+				}
+			}
+
+			int quality = -1;
+			if(!leftImages[i]->image.empty() && !rightImages[i]->image.empty())
+			{
+				bool alreadyRectified = true;
+				Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
+				rtabmap::Transform stereoTransform;
+				if(!alreadyRectified)
+				{
+					if(rightCameraInfos[i].header.frame_id.empty() || leftCameraInfos[i].header.frame_id.empty())
+					{
+						if(rightCameraInfos[i].P[3] == 0.0 && leftCameraInfos[i].P[3] == 0)
+						{
+							NODELET_ERROR("Parameter %s is false but the frame_id in one of the camera_info "
+									"topic is empty, so TF between the cameras cannot be computed!",
+									Parameters::kRtabmapImagesAlreadyRectified().c_str());
+							return;
+						}
+						else
+						{
+							static bool warned = false;
+							if(!warned)
+							{
+								NODELET_WARN("Parameter %s is false but the frame_id in one of the "
+										"camera_info topic is empty, so TF between the cameras cannot be "
+										"computed! However, the baseline can be computed from the calibration, "
+										"we will use this one instead of TF. This message is only printed once...",
+										Parameters::kRtabmapImagesAlreadyRectified().c_str());
+								warned = true;
+							}
+						}
+					}
+					else
+					{
+						stereoTransform = rtabmap_conversions::getTransform(
+								rightCameraInfos[i].header.frame_id,
+								leftCameraInfos[i].header.frame_id,
+								leftCameraInfos[i].header.stamp,
+								this->tfListener(),
+								this->waitForTransformDuration());
+						if(stereoTransform.isNull())
+						{
+							NODELET_ERROR("Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)",
+									Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+									rightCameraInfos[i].header.frame_id.c_str(),
+									leftCameraInfos[i].header.frame_id.c_str());
+							return;
+						}
+						else if(stereoTransform.isIdentity())
+						{
+							NODELET_ERROR("Parameter %s is false but we cannot get a valid TF between the two cameras! "
+									"Identity transform returned between left and right cameras. Verify that if TF between "
+									"the cameras is valid: \"rosrun tf tf_echo %s %s\".",
+									Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+									rightCameraInfos[i].header.frame_id.c_str(),
+									leftCameraInfos[i].header.frame_id.c_str());
+							return;
+						}
+					}
+				}
+
+				rtabmap::StereoCameraModel stereoModel = rtabmap_conversions::stereoCameraModelFromROS(leftCameraInfos[i], rightCameraInfos[i], localTransform, stereoTransform);
+
+				if( stereoModel.baseline() == 0 &&
+					alreadyRectified &&
+					!rightCameraInfos[i].header.frame_id.empty() &&
+					!leftCameraInfos[i].header.frame_id.empty())
+				{
+					stereoTransform = rtabmap_conversions::getTransform(
+							leftCameraInfos[i].header.frame_id,
+							rightCameraInfos[i].header.frame_id,
+							leftCameraInfos[i].header.stamp,
+							this->tfListener(),
+							this->waitForTransformDuration());
+
+					if(!stereoTransform.isNull() && stereoTransform.x()>0)
+					{
+						static bool warned = false;
+						if(!warned)
+						{
+							ROS_WARN("Right camera info doesn't have Tx set but we are assuming that stereo images are already rectified (see %s parameter). While not "
+									"recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed "
+									"a valid right camera info if stereo images are already rectified. This message is only printed once...",
+									rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+									rightCameraInfos[i].header.frame_id.c_str(), leftCameraInfos[i].header.frame_id.c_str(), stereoTransform.x());
+							warned = true;
+						}
+						stereoModel = rtabmap::StereoCameraModel(
+								stereoModel.left().fx(),
+								stereoModel.left().fy(),
+								stereoModel.left().cx(),
+								stereoModel.left().cy(),
+								stereoTransform.x(),
+								stereoModel.localTransform(),
+								stereoModel.left().imageSize());
+					}
+				}
+
+				if(alreadyRectified && stereoModel.baseline() <= 0)
+				{
+					NODELET_ERROR("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
+							  "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
+					return;
+				}
+
+				if(stereoModel.baseline() > 10.0)
+				{
+					static bool shown = false;
+					if(!shown)
+					{
+						NODELET_WARN("Detected baseline (%f m) is quite large! Is your "
+								 "right camera_info P(0,3) correctly set? Note that "
+								 "baseline=-P(0,3)/P(0,0). This warning is printed only once.",
+								 stereoModel.baseline());
+						shown = true;
+					}
+				}
+
+				cv_bridge::CvImageConstPtr ptrLeft = leftImages[i];
+				if(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+				   leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+				{
+					if(keepColor_ && leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+					{
+						ptrLeft = cv_bridge::cvtColor(leftImages[i], "bgr8");
+					}
+					else
+					{
+						ptrLeft = cv_bridge::cvtColor(leftImages[i], "mono8");
+					}
+				}
+
+				cv_bridge::CvImageConstPtr ptrRight = rightImages[i];
+				if(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+				   rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+				{
+					ptrRight = cv_bridge::cvtColor(rightImages[i], "mono8");
+				}
+
+				// initialize
+				if(left.empty())
+				{
+					left = cv::Mat(leftHeight, leftWidth*cameraCount, ptrLeft->image.type());
+				}
+				if(right.empty())
+				{
+					right = cv::Mat(rightHeight, rightWidth*cameraCount, ptrRight->image.type());
+				}
+
+				if(ptrLeft->image.type() == left.type())
+				{
+					ptrLeft->image.copyTo(cv::Mat(left, cv::Rect(i*leftWidth, 0, leftWidth, leftHeight)));
+				}
+				else
+				{
+					NODELET_ERROR("Some left images are not the same type! %d vs %d", ptrLeft->image.type(), left.type());
+					return;
+				}
+
+				if(ptrRight->image.type() == right.type())
+				{
+					ptrRight->image.copyTo(cv::Mat(right, cv::Rect(i*rightWidth, 0, rightWidth, rightHeight)));
+				}
+				else
+				{
+					NODELET_ERROR("Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type());
+					return;
+				}
+
+				cameraModels.push_back(stereoModel);
+			}
+			else
+			{
+				NODELET_ERROR("Odom: input images empty?!?");
+				return;
+			}
+		}
+
+		//
+		rtabmap::SensorData data(
+				left,
+				right,
+				cameraModels,
+				0,
+				rtabmap_conversions::timestampFromROS(higherStamp));
+
+		std_msgs::Header header;
+		header.stamp = higherStamp;
+		header.frame_id = leftImages.size()==1?leftImages[0]->header.frame_id:"";
+		this->processData(data, header);
+	}
+
+	void callback(
+				const sensor_msgs::ImageConstPtr& imageLeft,
+				const sensor_msgs::ImageConstPtr& imageRight,
+				const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
+				const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
+			std::vector<cv_bridge::CvImageConstPtr> rightMsgs(1);
+			std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
+			std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
+			leftMsgs[0] = cv_bridge::toCvShare(imageLeft);
+			rightMsgs[0] = cv_bridge::toCvShare(imageRight);
+			leftInfoMsgs.push_back(*cameraInfoLeft);
+			rightInfoMsgs.push_back(*cameraInfoRight);
+
+			double stampDiff = fabs(imageLeft->header.stamp.toSec() - imageRight->header.stamp.toSec());
+			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,
+						imageLeft->header.stamp.toSec(),
+						imageRight->header.stamp.toSec());
+			}
+
+			this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		}
+	}
+
+	void callbackRGBD(
+			const rtabmap_msgs::RGBDImageConstPtr& image)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
+			std::vector<cv_bridge::CvImageConstPtr> rightMsgs(1);
+			std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
+			std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
+			rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
+			leftInfoMsgs.push_back(image->rgb_camera_info);
+			rightInfoMsgs.push_back(image->depth_camera_info);
+
+			this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		}
+	}
+
+	void callbackRGBDX(
+			const rtabmap_msgs::RGBDImagesConstPtr& images)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			if(images->rgbd_images.empty())
+			{
+				NODELET_ERROR("Input topic \"%s\" doesn't contain any image(s)!", rgbdxSub_.getTopic().c_str());
+				return;
+			}
+			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(images->rgbd_images.size());
+			std::vector<cv_bridge::CvImageConstPtr> rightMsgs(images->rgbd_images.size());
+			std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
+			std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
+			for(size_t i=0; i<images->rgbd_images.size(); ++i)
+			{
+				rtabmap_conversions::toCvShare(images->rgbd_images[i], images, leftMsgs[i], rightMsgs[i]);
+				leftInfoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
+				rightInfoMsgs.push_back(images->rgbd_images[i].depth_camera_info);
+			}
+
+			this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		}
+	}
+
+	void callbackRGBD2(
+			const rtabmap_msgs::RGBDImageConstPtr& image,
+			const rtabmap_msgs::RGBDImageConstPtr& image2)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(2);
+			std::vector<cv_bridge::CvImageConstPtr> rightMsgs(2);
+			std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
+			std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
+			rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
+			rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
+			leftInfoMsgs.push_back(image->rgb_camera_info);
+			leftInfoMsgs.push_back(image2->rgb_camera_info);
+			rightInfoMsgs.push_back(image->depth_camera_info);
+			rightInfoMsgs.push_back(image2->depth_camera_info);
+
+			this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		}
+	}
+
+	void callbackRGBD3(
+			const rtabmap_msgs::RGBDImageConstPtr& image,
+			const rtabmap_msgs::RGBDImageConstPtr& image2,
+			const rtabmap_msgs::RGBDImageConstPtr& image3)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(3);
+			std::vector<cv_bridge::CvImageConstPtr> rightMsgs(3);
+			std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
+			std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
+			rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
+			rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
+			rtabmap_conversions::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
+			leftInfoMsgs.push_back(image->rgb_camera_info);
+			leftInfoMsgs.push_back(image2->rgb_camera_info);
+			leftInfoMsgs.push_back(image3->rgb_camera_info);
+			rightInfoMsgs.push_back(image->depth_camera_info);
+			rightInfoMsgs.push_back(image2->depth_camera_info);
+			rightInfoMsgs.push_back(image3->depth_camera_info);
+
+			this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		}
+	}
+
+	void callbackRGBD4(
+			const rtabmap_msgs::RGBDImageConstPtr& image,
+			const rtabmap_msgs::RGBDImageConstPtr& image2,
+			const rtabmap_msgs::RGBDImageConstPtr& image3,
+			const rtabmap_msgs::RGBDImageConstPtr& image4)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(4);
+			std::vector<cv_bridge::CvImageConstPtr> rightMsgs(4);
+			std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
+			std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
+			rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
+			rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
+			rtabmap_conversions::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
+			rtabmap_conversions::toCvShare(image4, leftMsgs[3], rightMsgs[3]);
+			leftInfoMsgs.push_back(image->rgb_camera_info);
+			leftInfoMsgs.push_back(image2->rgb_camera_info);
+			leftInfoMsgs.push_back(image3->rgb_camera_info);
+			leftInfoMsgs.push_back(image4->rgb_camera_info);
+			rightInfoMsgs.push_back(image->depth_camera_info);
+			rightInfoMsgs.push_back(image2->depth_camera_info);
+			rightInfoMsgs.push_back(image3->depth_camera_info);
+			rightInfoMsgs.push_back(image4->depth_camera_info);
+
+			this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		}
+	}
+
+protected:
+	virtual void flushCallbacks()
+	{
+		//flush callbacks
+		if(approxSync_)
+		{
+			delete approxSync_;
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+		}
+		if(exactSync_)
+		{
+			delete exactSync_;
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+		}
+		if(approxSync2_)
+		{
+			delete approxSync2_;
+			approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
+					MyApproxSync2Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_);
+			approxSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
+		}
+		if(exactSync2_)
+		{
+			delete exactSync2_;
+			exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
+					MyExactSync2Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_);
+			exactSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
+		}
+		if(approxSync3_)
+		{
+			delete approxSync3_;
+			approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
+					MyApproxSync3Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_,
+					rgbd_image3_sub_);
+			approxSync3_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+		}
+		if(exactSync3_)
+		{
+			delete exactSync3_;
+			exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
+					MyExactSync3Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_,
+					rgbd_image3_sub_);
+			exactSync3_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+		}
+		if(approxSync4_)
+		{
+			delete approxSync4_;
+			approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
+					MyApproxSync4Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_,
+					rgbd_image3_sub_,
+					rgbd_image4_sub_);
+			approxSync4_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+		}
+		if(exactSync4_)
+		{
+			delete exactSync4_;
+			exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
+					MyExactSync4Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_,
+					rgbd_image3_sub_,
+					rgbd_image4_sub_);
+			exactSync4_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+		}
+	}
+
+private:
+	image_transport::SubscriberFilter imageRectLeft_;
+	image_transport::SubscriberFilter imageRectRight_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
+
+	ros::Subscriber rgbdSub_;
+	ros::Subscriber rgbdxSub_;
+	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image1_sub_;
+	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image2_sub_;
+	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image3_sub_;
+	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image4_sub_;
+	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image5_sub_;
+
+	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_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync2Policy;
+	message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync2Policy;
+	message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync3Policy;
+	message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync3Policy;
+	message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync4Policy;
+	message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync4Policy;
+	message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
+
+	int queueSize_;
+	bool keepColor_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_odom::StereoOdometry, nodelet::Nodelet);
+
+}
+