Added icp_odometry and rgbdicp_odometry nodes

CMakeLists.txt

@@ -18,7 +18,7 @@ find_package(rviz)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.11.8 REQUIRED)
+find_package(RTABMap 0.11.9 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 
@@ -151,6 +151,8 @@ SET(Libraries
 SET(rtabmap_ros_lib_src
    src/nodelets/rgbd_odometry.cpp
    src/nodelets/stereo_odometry.cpp
+   src/nodelets/rgbdicp_odometry.cpp
+   src/nodelets/icp_odometry.cpp
    src/nodelets/data_throttle.cpp
    src/nodelets/stereo_throttle.cpp
    src/nodelets/data_odom_sync.cpp
@@ -268,6 +270,12 @@ target_link_libraries(rgbd_odometry ${Libraries})
 add_executable(stereo_odometry src/StereoOdometryNode.cpp)
 target_link_libraries(stereo_odometry ${Libraries})
 
+add_executable(rgbdicp_odometry src/RGBDICPOdometryNode.cpp)
+target_link_libraries(rgbdicp_odometry ${Libraries})
+
+add_executable(icp_odometry src/ICPOdometryNode.cpp)
+target_link_libraries(icp_odometry ${Libraries})
+
 add_executable(map_optimizer src/MapOptimizerNode.cpp)
 target_link_libraries(map_optimizer rtabmap_ros ${Libraries})
 

include/rtabmap_ros/OdometryROS.h

@@ -51,7 +51,7 @@ class OdometryROS : public nodelet::Nodelet
 {
 
 public:
-	OdometryROS(bool stereo);
+	OdometryROS(bool stereoParams, bool visParams, bool icpParams);
 	virtual ~OdometryROS();
 
 	void processData(const rtabmap::SensorData & data, const ros::Time & stamp);
@@ -68,16 +68,17 @@ public:
 	const std::string & frameId() const {return frameId_;}
 	const std::string & odomFrameId() const {return odomFrameId_;}
 	const rtabmap::ParametersMap & parameters() const {return parameters_;}
-	const tf::TransformListener & tfListener() const {return tfListener_;}
 	bool isPaused() const {return paused_;}
 	rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
 
 protected:
 	virtual void flushCallbacks() = 0;
+	tf::TransformListener & tfListener() {return tfListener_;}
 
 private:
 	virtual void onInit();
 	virtual void onOdomInit() = 0;
+	virtual void updateParameters(rtabmap::ParametersMap & parameters) {}
 
 private:
 	rtabmap::Odometry * odometry_;
@@ -86,6 +87,7 @@ private:
 	std::string frameId_;
 	std::string odomFrameId_;
 	std::string groundTruthFrameId_;
+	std::string guessFrameId_;
 	bool publishTf_;
 	bool waitForTransform_;
 	double waitForTransformDuration_;
@@ -111,7 +113,9 @@ private:
 	bool paused_;
 	int resetCountdown_;
 	int resetCurrentCount_;
-	bool stereo_;
+	bool stereoParams_;
+	bool visParams_;
+	bool icpParams_;
 };
 
 }

launch/tests/test_icp_odometry.launch

@@ -0,0 +1,53 @@
+
+<launch>
+
+  <!-- We test here ICP odometry using a guess from visual odometry -->
+  
+  <arg name="rgbd"  default="false"/>
+   
+  <include file="$(find freenect_launch)/launch/freenect.launch" >
+    <arg name="depth_registration" value="true"/>
+    <arg name="data_skip" value="3"/>
+  </include>
+  
+  <group ns="camera"> 
+    <node pkg="nodelet" type="nodelet" name="points_xyz" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
+      <remap from="depth/image"       to="depth_registered/image_raw"/>
+      <remap from="depth/camera_info" to="depth_registered/camera_info"/>
+      <remap from="cloud"             to="/voxel_cloud" />
+
+      <param name="voxel_size" type="double" value="0.05"/>
+      <param name="decimation" type="int" value="8"/>
+      
+      <param name="Odom/AlignWithGround"  type="string" value="true"/>
+    </node>
+
+    <node if="$(arg rgbd)" pkg="nodelet" type="nodelet" name="rgbdicp_odometry" args="load rtabmap_ros/rgbdicp_odometry camera_nodelet_manager">
+      <remap from="scan_cloud"        to="/voxel_cloud"/>
+      <remap from="depth/image"       to="depth_registered/image_raw"/>
+      <remap from="rgb/camera_info"   to="rgb/camera_info"/>
+      <remap from="rgb/image"         to="rgb/image_rect_mono"/>
+	  
+      <param name="frame_id"        type="string" value="camera_link"/>   
+      <param name="scan_cloud_normal_k" type="int" value="10"/>
+      <param name="subscribe_scan_cloud" type="bool" value="true"/>   
+     
+      <param name="Icp/PointToPlane"  type="string" value="true"/>
+      <param name="Icp/VoxelSize"     type="string" value="0"/>
+    </node>
+    <node unless="$(arg rgbd)" pkg="nodelet" type="nodelet" name="icp_odometry" args="load rtabmap_ros/icp_odometry camera_nodelet_manager">
+      <remap from="scan_cloud"      to="/voxel_cloud"/>
+	  
+      <param name="frame_id"        type="string" value="camera_link"/>   
+      <param name="scan_cloud_normal_k" type="int" value="10"/>   
+     
+      <param name="Icp/PointToPlane"  type="string" value="true"/>
+      <param name="Icp/VoxelSize"     type="string" value="0"/>
+      <param name="Odom/GuessMotion"  type="string" value="true"/>
+    </node>
+
+  </group>
+  
+  <!-- Visualization RVIZ -->
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
+</launch>

nodelet_plugins.xml

@@ -15,6 +15,22 @@
       This is my nodelet.
     </description>
   </class>
+  
+  <class name="rtabmap_ros/rgbdicp_odometry" 
+         type="rtabmap_ros::RGBDICPOdometry" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
+  
+  <class name="rtabmap_ros/icp_odometry" 
+         type="rtabmap_ros::ICPOdometry" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
 
   <class name="rtabmap_ros/data_throttle" 
          type="rtabmap_ros::DataThrottleNodelet" 

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap_ros</name>
-  <version>0.11.8</version>
+  <version>0.11.9</version>
   <description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

src/CoreWrapper.cpp

@@ -790,10 +790,11 @@ Transform CoreWrapper::getTransform(const std::string & fromFrameId, const std::
 		if(waitForTransform_ && !stamp.isZero() && waitForTransformDuration_>0.0)
 		{
 			//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
-			if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_)))
+			std::string errorMsg;
+			if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_), ros::Duration(0.01), &errorMsg))
 			{
-				ROS_WARN("rtabmap: Could not get transform from %s to %s after %f seconds (for stamp=%f)!",
-						fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_, stamp.toSec());
+				ROS_WARN("rtabmap: Could not get transform from %s to %s after %f seconds (for stamp=%f)! Error=\"%s\"",
+						fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_, stamp.toSec(), errorMsg.c_str());
 				return transform;
 			}
 		}
@@ -1040,10 +1041,41 @@ void CoreWrapper::commonDepthCallback(
 				break;
 			}
 		}
+
+		// sync with odometry stamp
+		Transform localScanTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp);
+		if(localScanTransform.isNull())
+		{
+			ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec());
+			return;
+		}
+		Transform laserOdomT = localScanTransform;
+		if(lastPoseStamp_ != scan3dMsg->header.stamp)
+		{
+			if(!odomT.isNull())
+			{
+				Transform sensorT = getTransform(odomFrameId, frameId_, scan3dMsg->header.stamp);
+				if(sensorT.isNull())
+				{
+					ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry "
+							"stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan3dMsg->header.stamp.toSec(), lastPoseStamp_.toSec());
+				}
+				else
+				{
+					laserOdomT = odomT.inverse() * sensorT * localScanTransform;
+				}
+
+			}
+		}
+
 		if(containNormals)
 		{
 			pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
 			pcl::fromROSMsg(*scan3dMsg, *pclScan);
+			if(!laserOdomT.isIdentity())
+			{
+				pclScan = util3d::transformPointCloud(pclScan, laserOdomT);
+			}
 			scan = util3d::laserScanFromPointCloud(*pclScan);
 		}
 		else
@@ -1051,6 +1083,11 @@ void CoreWrapper::commonDepthCallback(
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 			pcl::fromROSMsg(*scan3dMsg, *pclScan);
 
+			if(!laserOdomT.isIdentity())
+			{
+				pclScan = util3d::transformPointCloud(pclScan, laserOdomT);
+			}
+
 			if(scanCloudNormalK_ > 0)
 			{
 				//compute normals
@@ -1205,10 +1242,43 @@ void CoreWrapper::commonStereoCallback(
 				break;
 			}
 		}
+
+		// sync with odometry stamp
+		Transform localScanTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp);
+		if(localScanTransform.isNull())
+		{
+			ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec());
+			return;
+		}
+		Transform laserOdomT = localScanTransform;
+		if(lastPoseStamp_ != scan3dMsg->header.stamp)
+		{
+			if(!odomT.isNull())
+			{
+				Transform sensorT = getTransform(odomFrameId, frameId_, scan3dMsg->header.stamp);
+				if(sensorT.isNull())
+				{
+					ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry "
+							"stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan3dMsg->header.stamp.toSec(), lastPoseStamp_.toSec());
+				}
+				else
+				{
+					laserOdomT = odomT.inverse() * sensorT * localScanTransform;
+				}
+
+			}
+		}
+
 		if(containNormals)
 		{
 			pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
 			pcl::fromROSMsg(*scan3dMsg, *pclScan);
+
+			if(!laserOdomT.isIdentity())
+			{
+				pclScan = util3d::transformPointCloud(pclScan, laserOdomT);
+			}
+
 			scan = util3d::laserScanFromPointCloud(*pclScan);
 		}
 		else
@@ -1216,6 +1286,11 @@ void CoreWrapper::commonStereoCallback(
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 			pcl::fromROSMsg(*scan3dMsg, *pclScan);
 
+			if(!laserOdomT.isIdentity())
+			{
+				pclScan = util3d::transformPointCloud(pclScan, laserOdomT);
+			}
+
 			if(scanCloudNormalK_ > 0)
 			{
 				//compute normals

src/GuiWrapper.cpp

@@ -482,10 +482,11 @@ Transform GuiWrapper::getTransform(const std::string & fromFrameId, const std::s
 		if(waitForTransform_ && !stamp.isZero() && waitForTransformDuration_ > 0.0)
 		{
 			//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
-			if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_)))
+			std::string errorMsg;
+			if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_), ros::Duration(0.01), &errorMsg))
 			{
-				ROS_WARN("rtabmapviz: Could not get transform from %s to %s after %f seconds (for stamp=%f)!",
-						fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_, stamp.toSec());
+				ROS_WARN("rtabmapviz: Could not get transform from %s to %s after %f seconds (for stamp=%f)! Error=\"%s\".",
+						fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_, stamp.toSec(), errorMsg.c_str());
 				return transform;
 			}
 		}

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_ros/icp_odometry", remap, nargv);
+	ros::spin();
+	return 0;
+}

src/OdometryROS.cpp

@@ -55,11 +55,12 @@ using namespace rtabmap;
 
 namespace rtabmap_ros {
 
-OdometryROS::OdometryROS(bool stereo) :
+OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
 	odometry_(0),
 	frameId_("base_link"),
 	odomFrameId_("odom"),
 	groundTruthFrameId_(""),
+	guessFrameId_(""),
 	publishTf_(true),
 	waitForTransform_(true),
 	waitForTransformDuration_(0.1), // 100 ms
@@ -68,7 +69,9 @@ OdometryROS::OdometryROS(bool stereo) :
 	paused_(false),
 	resetCountdown_(0),
 	resetCurrentCount_(0),
-	stereo_(stereo)
+	stereoParams_(stereoParams),
+	visParams_(visParams),
+	icpParams_(icpParams)
 {
 
 }
@@ -112,10 +115,13 @@ void OdometryROS::onInit()
 	pnh.param("config_path", configPath, configPath);
 	pnh.param("publish_null_when_lost", publishNullWhenLost_, publishNullWhenLost_);
 	pnh.param("guess_from_tf", guessFromTf_, guessFromTf_);
+	pnh.param("guess_frame_id", guessFrameId_, frameId_);
 
-	if(publishTf_ && guessFromTf_)
+	if(publishTf_ && guessFromTf_ && guessFrameId_.compare(frameId_) == 0)
 	{
-		NODELET_WARN( "\"publish_tf\" and \"guess_from_tf\" cannot be used at the same time. \"guess_from_tf\" is disabled.");
+		NODELET_WARN( "\"publish_tf\" and \"guess_from_tf\" cannot be used "
+				"at the same time if \"guess_frame_id\" and \"frame_id\" "
+				"are the same frame (value=\"%s\"). \"guess_from_tf\" is disabled.", frameId_.c_str());
 		guessFromTf_ = false;
 	}
 
@@ -159,7 +165,7 @@ void OdometryROS::onInit()
 
 
 	//parameters
-	parameters_ = Parameters::getDefaultOdometryParameters(stereo_);
+	parameters_ = Parameters::getDefaultOdometryParameters(stereoParams_, visParams_, icpParams_);
 	if(!configPath.empty())
 	{
 		if(UFile::exists(configPath.c_str()))
@@ -267,6 +273,9 @@ void OdometryROS::onInit()
 
 	Parameters::parse(parameters_, Parameters::kOdomResetCountdown(), resetCountdown_);
 	parameters_.at(Parameters::kOdomResetCountdown()) = "0"; // use modified reset countdown here
+
+	this->updateParameters(parameters_);
+
 	odometry_ = Odometry::create(parameters_);
 	if(!initialPose.isIdentity())
 	{
@@ -295,10 +304,11 @@ Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::
 		if(waitForTransform_ && !stamp.isZero() && waitForTransformDuration_ > 0.0)
 		{
 			//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
-			if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_)))
+			std::string errorMsg;
+			if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_), ros::Duration(0.01), &errorMsg))
 			{
-				NODELET_WARN( "odometry: Could not get transform from %s to %s (stamp=%f) after %f seconds (\"wait_for_transform_duration\"=%f)!",
-						fromFrameId.c_str(), toFrameId.c_str(), stamp.toSec(), waitForTransformDuration_, waitForTransformDuration_);
+				NODELET_WARN( "odometry: Could not get transform from %s to %s (stamp=%f) after %f seconds (\"wait_for_transform_duration\"=%f)! Error=\"%s\"",
+						fromFrameId.c_str(), toFrameId.c_str(), stamp.toSec(), waitForTransformDuration_, waitForTransformDuration_, errorMsg.c_str());
 				return transform;
 			}
 		}
@@ -336,8 +346,9 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 	Transform guess;
 	if(guessFromTf_)
 	{
-		Transform previousPose = this->getTransform(odomFrameId_, frameId_, ros::Time(odometry_->previousStamp()));
-		Transform pose = this->getTransform(odomFrameId_, frameId_, stamp);
+		ROS_WARN("Time previous=%f new=%f", odometry_->previousStamp(), stamp.toSec());
+		Transform previousPose = this->getTransform(odomFrameId_, guessFrameId_, ros::Time(odometry_->previousStamp()));
+		Transform pose = this->getTransform(odomFrameId_, guessFrameId_, stamp);
 		if(!previousPose.isNull() && !pose.isNull())
 		{
 			guess = previousPose.inverse() * pose;
@@ -352,6 +363,11 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 			}
 			NODELET_WARN( "TF Guess %s", guess.prettyPrint().c_str());*/
 		}
+		else
+		{
+			ROS_ERROR("\"guess_from_tf\" is true, but guess cannot be computed between frames \"%s\" -> \"%s\". Aborting odometry update...", odomFrameId_.c_str(), guessFrameId_.c_str());
+			return;
+		}
 	}
 
 	// process data
@@ -547,7 +563,22 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 		odomInfoPub_.publish(infoMsg);
 	}
 
-	NODELET_INFO( "Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec());
+	if(visParams_)
+	{
+		if(icpParams_)
+		{
+			NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm, update time=%fs", info.inliers, info.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec());
+		}
+		else
+		{
+			NODELET_INFO( "Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec());
+		}
+	}
+	else
+	{
+		NODELET_INFO( "Odom: ratio=%f, std dev=%fm, update time=%fs", info.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec());
+	}
+
 }
 
 bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)

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_ros/rgbdicp_odometry", remap, nargv);
+	ros::spin();
+	return 0;
+}

src/nodelets/icp_odometry.cpp

@@ -0,0 +1,211 @@
+/*
+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_ros/OdometryROS.h>
+
+#include <pluginlib/class_list_macros.h>
+#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 "rtabmap_ros/MsgConversion.h"
+
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_surface.h>
+#include <rtabmap/core/util3d_transforms.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_ros
+{
+
+class ICPOdometry : public rtabmap_ros::OdometryROS
+{
+public:
+	ICPOdometry() :
+		OdometryROS(false, false, true),
+		scanCloudMaxPoints_(0),
+		scanCloudNormalK_(0)
+	{
+	}
+
+	virtual ~ICPOdometry()
+	{
+	}
+
+private:
+
+	virtual void onOdomInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		bool approxSync = true;
+		bool subscribeScanCloud = false;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
+		pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
+
+		scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
+		cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
+	}
+
+	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("ICP odometry works only with \"Reg/Strategy\"=1. Ignoring value %s.", iter->second.c_str());
+		}
+		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "1"));
+	}
+
+	void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg)
+	{
+		// make sure the frame of the laser is updated too
+		if(getTransform(this->frameId(),
+				scanMsg->header.frame_id,
+				scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)).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(this->frameId(), *scanMsg, scanOut, this->tfListener());
+		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::fromROSMsg(scanOut, *pclScan);
+
+		cv::Mat scan = util3d::laserScan2dFromPointCloud(*pclScan);
+
+		rtabmap::SensorData data(
+				scan,
+				(int)scanMsg->ranges.size(),
+				scanMsg->range_max,
+				cv::Mat(),
+				cv::Mat(),
+				CameraModel(),
+				0,
+				rtabmap_ros::timestampFromROS(scanMsg->header.stamp));
+
+		this->processData(data, scanMsg->header.stamp);
+	}
+
+	void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
+	{
+		cv::Mat scan;
+		bool containNormals = false;
+		for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
+		{
+			if(cloudMsg->fields[i].name.compare("normal_x") == 0)
+			{
+				containNormals = true;
+				break;
+			}
+		}
+
+		Transform localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp);
+		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(containNormals)
+		{
+			pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
+			pcl::fromROSMsg(*cloudMsg, *pclScan);
+			if(!localScanTransform.isIdentity())
+			{
+				pclScan = util3d::transformPointCloud(pclScan, localScanTransform);
+			}
+			scan = util3d::laserScanFromPointCloud(*pclScan);
+		}
+		else
+		{
+			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::fromROSMsg(*cloudMsg, *pclScan);
+
+			if(!localScanTransform.isIdentity())
+			{
+				pclScan = util3d::transformPointCloud(pclScan, localScanTransform);
+			}
+
+			if(scanCloudNormalK_ > 0)
+			{
+				//compute normals
+				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_);
+				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(
+				scan,
+				scanCloudMaxPoints_,
+				0,
+				cv::Mat(),
+				cv::Mat(),
+				CameraModel(),
+				0,
+				rtabmap_ros::timestampFromROS(cloudMsg->header.stamp));
+
+		this->processData(data, cloudMsg->header.stamp);
+	}
+
+protected:
+	virtual void flushCallbacks()
+	{
+		// flush callbacks
+	}
+
+private:
+	ros::Subscriber scan_sub_;
+	ros::Subscriber cloud_sub_;
+	int scanCloudMaxPoints_;
+	int scanCloudNormalK_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ICPOdometry, nodelet::Nodelet);
+
+}

src/nodelets/rgbd_odometry.cpp

@@ -49,6 +49,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
 
 using namespace rtabmap;
 
@@ -59,7 +60,7 @@ class RGBDOdometry : public rtabmap_ros::OdometryROS
 {
 public:
 	RGBDOdometry() :
-		OdometryROS(false),
+		OdometryROS(false, true, false),
 		approxSync_(0),
 		exactSync_(0),
 		sync2_(0),
@@ -132,15 +133,6 @@ private:
 			image_depth2_sub_.subscribe(depth1_it, depth1_nh.resolveName("image"), 1, hintsDepth1);
 			info2_sub_.subscribe(rgb1_nh, "camera_info", 1);
 
-			NODELET_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
-					ros::this_node::getName().c_str(),
-					image_mono_sub_.getTopic().c_str(),
-					image_depth_sub_.getTopic().c_str(),
-					info_sub_.getTopic().c_str(),
-					image_mono2_sub_.getTopic().c_str(),
-					image_depth2_sub_.getTopic().c_str(),
-					info2_sub_.getTopic().c_str());
-
 			sync2_ = new message_filters::Synchronizer<MySync2Policy>(
 					MySync2Policy(queueSize_),
 					image_mono_sub_,
@@ -150,6 +142,14 @@ private:
 					image_depth2_sub_,
 					info2_sub_);
 			sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
+			NODELET_INFO("\n%s subscribed to (approx sync):\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+					ros::this_node::getName().c_str(),
+					image_mono_sub_.getTopic().c_str(),
+					image_depth_sub_.getTopic().c_str(),
+					info_sub_.getTopic().c_str(),
+					image_mono2_sub_.getTopic().c_str(),
+					image_depth2_sub_.getTopic().c_str(),
+					info2_sub_.getTopic().c_str());
 		}
 		else
 		{
@@ -186,6 +186,17 @@ private:
 		}
 	}
 
+	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"));
+	}
+
 	void callback(
 			const sensor_msgs::ImageConstPtr& image,
 			const sensor_msgs::ImageConstPtr& depth,
@@ -208,6 +219,7 @@ private:
 				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;
 
 			Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp);

src/nodelets/rgbdicp_odometry.cpp

@@ -0,0 +1,408 @@
+/*
+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_ros/OdometryROS.h>
+
+#include <pluginlib/class_list_macros.h>
+#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_ros/MsgConversion.h"
+
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_surface.h>
+#include <rtabmap/core/util3d_transforms.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_ros
+{
+
+class RGBDICPOdometry : public rtabmap_ros::OdometryROS
+{
+public:
+	RGBDICPOdometry() :
+		OdometryROS(false, true, true),
+		approxScanSync_(0),
+		exactScanSync_(0),
+		approxCloudSync_(0),
+		exactCloudSync_(0),
+		queueSize_(5),
+		scanCloudMaxPoints_(0),
+		scanCloudNormalK_(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;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("queue_size", queueSize_, queueSize_);
+		pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud);
+		pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
+		pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
+
+		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(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_);
+				approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _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, _1, _2, _3, _4));
+			}
+
+			NODELET_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s, \n   %s",
+					ros::this_node::getName().c_str(),
+					approxSync?"approx":"exact",
+					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_);
+				approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _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, _1, _2, _3, _4));
+			}
+
+			NODELET_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s, \n   %s",
+					ros::this_node::getName().c_str(),
+					approxSync?"approx":"exact",
+					image_mono_sub_.getTopic().c_str(),
+					image_depth_sub_.getTopic().c_str(),
+					info_sub_.getTopic().c_str(),
+					scan_sub_.getTopic().c_str());
+		}
+	}
+
+	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 callback(
+			const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& depth,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		sensor_msgs::LaserScanConstPtr scanMsg;
+		sensor_msgs::PointCloud2ConstPtr cloudMsg;
+		callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg);
+	}
+
+	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)
+	{
+		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) ||
+			   !(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 (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 = getTransform(this->frameId(), image->header.frame_id, stamp);
+			if(localTransform.isNull())
+			{
+				return;
+			}
+
+			if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
+			{
+				rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
+				cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
+				cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
+
+				cv::Mat scan;
+				if(scanMsg.get() != 0)
+				{
+					// make sure the frame of the laser is updated too
+					if(getTransform(this->frameId(),
+							scanMsg->header.frame_id,
+							scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)).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(this->frameId(), *scanMsg, scanOut, this->tfListener());
+					pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+					pcl::fromROSMsg(scanOut, *pclScan);
+
+					scan = util3d::laserScan2dFromPointCloud(*pclScan);
+				}
+				else if(cloudMsg.get() != 0)
+				{
+					bool containNormals = false;
+					for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
+					{
+						if(cloudMsg->fields[i].name.compare("normal_x") == 0)
+						{
+							containNormals = true;
+							break;
+						}
+					}
+					Transform localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp);
+					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(containNormals)
+					{
+						pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::fromROSMsg(*cloudMsg, *pclScan);
+						if(!localScanTransform.isIdentity())
+						{
+							pclScan = util3d::transformPointCloud(pclScan, localScanTransform);
+						}
+						scan = util3d::laserScanFromPointCloud(*pclScan);
+					}
+					else
+					{
+						pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+						pcl::fromROSMsg(*cloudMsg, *pclScan);
+
+						if(!localScanTransform.isIdentity())
+						{
+							pclScan = util3d::transformPointCloud(pclScan, localScanTransform);
+						}
+
+						if(scanCloudNormalK_ > 0)
+						{
+							//compute normals
+							pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_);
+							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(
+						scan,
+						scanMsg.get() != 0?(int)scanMsg->ranges.size():cloudMsg.get() != 0?scanCloudMaxPoints_:0,
+						scanMsg.get() != 0?scanMsg->range_max:0,
+						ptrImage->image,
+						ptrDepth->image,
+						rtabmapModel,
+						0,
+						rtabmap_ros::timestampFromROS(stamp));
+
+				this->processData(data, stamp);
+			}
+		}
+	}
+
+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, _1, _2, _3, _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, _1, _2, _3, _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, _1, _2, _3, _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, _1, _2, _3, _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_;
+	int scanCloudMaxPoints_;
+	int scanCloudNormalK_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDICPOdometry, nodelet::Nodelet);
+
+}

src/nodelets/stereo_odometry.cpp

@@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UStl.h>
 
 using namespace rtabmap;
 
@@ -57,7 +58,7 @@ class StereoOdometry : public rtabmap_ros::OdometryROS
 {
 public:
 	StereoOdometry() :
-		rtabmap_ros::OdometryROS(true),
+		rtabmap_ros::OdometryROS(true, true, false),
 		approxSync_(0),
 		exactSync_(0),
 		queueSize_(5)
@@ -122,6 +123,17 @@ private:
 				cameraInfoRight_.getTopic().c_str());
 	}
 
+	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 callback(
 			const sensor_msgs::ImageConstPtr& imageRectLeft,
 			const sensor_msgs::ImageConstPtr& imageRectRight,