Added odometry nodelets (issue #96)

2026-10-06 09:47:46 +08:00 · 2016-07-06 20:08:23 -04:00
parent fb09c96ba1
commit 7a896f6e7b
10 changed files with 851 additions and 670 deletions
@@ -0,0 +1,625 @@
+/*
+Copyright (c) 2010-2014, 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 "OdometryROS.h"
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <nav_msgs/Odometry.h>
+
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <cv_bridge/cv_bridge.h>
+
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/OdometryF2M.h>
+#include <rtabmap/core/OdometryF2F.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/Memory.h>
+#include <rtabmap/core/Signature.h>
+#include "rtabmap_ros/MsgConversion.h"
+#include "rtabmap_ros/OdomInfo.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UFile.h"
+
+#define BAD_COVARIANCE 9999
+
+using namespace rtabmap;
+
+namespace rtabmap_ros {
+
+OdometryROS::OdometryROS(bool stereo) :
+	odometry_(0),
+	frameId_("base_link"),
+	odomFrameId_("odom"),
+	groundTruthFrameId_(""),
+	publishTf_(true),
+	waitForTransform_(true),
+	waitForTransformDuration_(0.1), // 100 ms
+	publishNullWhenLost_(true),
+	guessFromTf_(false),
+	paused_(false),
+	resetCountdown_(0),
+	resetCurrentCount_(0),
+	stereo_(stereo)
+{
+
+}
+
+OdometryROS::~OdometryROS()
+{
+	ros::NodeHandle & pnh = getPrivateNodeHandle();
+	if(pnh.ok())
+	{
+		for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
+		{
+			pnh.deleteParam(iter->first);
+		}
+	}
+
+	delete odometry_;
+}
+
+void OdometryROS::onInit()
+{
+	ros::NodeHandle & nh = getNodeHandle();
+	ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+	odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
+	odomInfoPub_ = nh.advertise<rtabmap_ros::OdomInfo>("odom_info", 1);
+	odomLocalMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
+	odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
+
+	Transform initialPose = Transform::getIdentity();
+	std::string initialPoseStr;
+	std::string tfPrefix;
+	std::string configPath;
+	pnh.param("frame_id", frameId_, frameId_);
+	pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
+	pnh.param("publish_tf", publishTf_, publishTf_);
+	pnh.param("tf_prefix", tfPrefix, tfPrefix);
+	pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
+	pnh.param("wait_for_transform_duration",  waitForTransformDuration_, waitForTransformDuration_);
+	pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw"
+	pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
+	pnh.param("config_path", configPath, configPath);
+	pnh.param("publish_null_when_lost", publishNullWhenLost_, publishNullWhenLost_);
+	pnh.param("guess_from_tf", guessFromTf_, guessFromTf_);
+
+	if(publishTf_ && guessFromTf_)
+	{
+		NODELET_WARN( "\"publish_tf\" and \"guess_from_tf\" cannot be used at the same time. \"guess_from_tf\" is disabled.");
+		guessFromTf_ = false;
+	}
+
+	configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
+	if(configPath.size() && configPath.at(0) != '/')
+	{
+		configPath = UDirectory::currentDir(true) + configPath;
+	}
+
+	if(!tfPrefix.empty())
+	{
+		if(!frameId_.empty())
+		{
+			frameId_ = tfPrefix + "/" + frameId_;
+		}
+		if(!odomFrameId_.empty())
+		{
+			odomFrameId_ = tfPrefix + "/" + odomFrameId_;
+		}
+		if(!groundTruthFrameId_.empty())
+		{
+			groundTruthFrameId_ = tfPrefix + "/" + groundTruthFrameId_;
+		}
+	}
+
+	if(initialPoseStr.size())
+	{
+		std::vector<std::string> values = uListToVector(uSplit(initialPoseStr, ' '));
+		if(values.size() == 6)
+		{
+			initialPose = Transform(
+					uStr2Float(values[0]), uStr2Float(values[1]), uStr2Float(values[2]),
+					uStr2Float(values[3]), uStr2Float(values[4]), uStr2Float(values[5]));
+		}
+		else
+		{
+			NODELET_ERROR( "Wrong initial_pose format: %s (should be \"x y z roll pitch yaw\" with angle in radians). "
+					  "Identity will be used...", initialPoseStr.c_str());
+		}
+	}
+
+
+	//parameters
+	parameters_ = Parameters::getDefaultOdometryParameters(stereo_);
+	if(!configPath.empty())
+	{
+		if(UFile::exists(configPath.c_str()))
+		{
+			NODELET_INFO( "Odometry: Loading parameters from %s", configPath.c_str());
+			rtabmap::ParametersMap allParameters;
+			Parameters::readINI(configPath.c_str(), allParameters);
+			// only update odometry parameters
+			for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
+			{
+				ParametersMap::iterator jter = allParameters.find(iter->first);
+				if(jter!=allParameters.end())
+				{
+					iter->second = jter->second;
+				}
+			}
+		}
+		else
+		{
+			NODELET_ERROR( "Config file \"%s\" not found!", configPath.c_str());
+		}
+	}
+	for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
+	{
+		std::string vStr;
+		bool vBool;
+		int vInt;
+		double vDouble;
+		if(pnh.getParam(iter->first, vStr))
+		{
+			NODELET_INFO( "Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
+			iter->second = vStr;
+		}
+		else if(pnh.getParam(iter->first, vBool))
+		{
+			NODELET_INFO( "Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
+			iter->second = uBool2Str(vBool);
+		}
+		else if(pnh.getParam(iter->first, vDouble))
+		{
+			NODELET_INFO( "Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
+			iter->second = uNumber2Str(vDouble);
+		}
+		else if(pnh.getParam(iter->first, vInt))
+		{
+			NODELET_INFO( "Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
+			iter->second = uNumber2Str(vInt);
+		}
+
+		if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8)
+		{
+			NODELET_WARN( "Parameter min_inliers must be >= 8, setting to 8...");
+			iter->second = uNumber2Str(8);
+		}
+	}
+
+	std::vector<std::string> argList = getMyArgv();
+	char * argv[argList.size()];
+	for(unsigned int i=0; i<argList.size(); ++i)
+	{
+		argv[i] = &argList[i].at(0);
+	}
+	rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argList.size(), argv);
+	for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		rtabmap::ParametersMap::iterator jter = parameters_.find(iter->first);
+		if(jter!=parameters_.end())
+		{
+			NODELET_INFO( "Update odometry parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str());
+			jter->second = iter->second;
+		}
+	}
+
+	// Backward compatibility
+	for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
+		iter!=Parameters::getRemovedParameters().end();
+		++iter)
+	{
+		std::string vStr;
+		if(pnh.getParam(iter->first, vStr))
+		{
+			if(iter->second.first)
+			{
+				// can be migrated
+				parameters_.at(iter->second.second)= vStr;
+				NODELET_WARN( "Odometry: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
+						iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
+			}
+			else
+			{
+				if(iter->second.second.empty())
+				{
+					NODELET_ERROR( "Odometry: Parameter \"%s\" doesn't exist anymore!",
+							iter->first.c_str());
+				}
+				else
+				{
+					NODELET_ERROR( "Odometry: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
+							iter->first.c_str(), iter->second.second.c_str());
+				}
+			}
+		}
+	}
+
+	Parameters::parse(parameters_, Parameters::kOdomResetCountdown(), resetCountdown_);
+	parameters_.at(Parameters::kOdomResetCountdown()) = "0"; // use modified reset countdown here
+	odometry_ = Odometry::create(parameters_);
+	if(!initialPose.isIdentity())
+	{
+		odometry_->reset(initialPose);
+	}
+
+	resetSrv_ = nh.advertiseService("reset_odom", &OdometryROS::reset, this);
+	resetToPoseSrv_ = nh.advertiseService("reset_odom_to_pose", &OdometryROS::resetToPose, this);
+	pauseSrv_ = nh.advertiseService("pause_odom", &OdometryROS::pause, this);
+	resumeSrv_ = nh.advertiseService("resume_odom", &OdometryROS::resume, this);
+
+	setLogDebugSrv_ = pnh.advertiseService("log_debug", &OdometryROS::setLogDebug, this);
+	setLogInfoSrv_ = pnh.advertiseService("log_info", &OdometryROS::setLogInfo, this);
+	setLogWarnSrv_ = pnh.advertiseService("log_warning", &OdometryROS::setLogWarn, this);
+	setLogErrorSrv_ = pnh.advertiseService("log_error", &OdometryROS::setLogError, this);
+
+	onOdomInit();
+}
+
+Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const
+{
+	// TF ready?
+	Transform transform;
+	try
+	{
+		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_)))
+			{
+				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_);
+				return transform;
+			}
+		}
+
+		tf::StampedTransform tmp;
+		tfListener_.lookupTransform(fromFrameId, toFrameId, stamp, tmp);
+		transform = rtabmap_ros::transformFromTF(tmp);
+	}
+	catch(tf::TransformException & ex)
+	{
+		NODELET_WARN( "%s",ex.what());
+	}
+	return transform;
+}
+
+void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
+{
+	if(odometry_->getPose().isIdentity() &&
+	   !groundTruthFrameId_.empty())
+	{
+		// sync with the first value of the ground truth
+		Transform initialPose = getTransform(groundTruthFrameId_, frameId_, stamp);
+		if(initialPose.isNull())
+		{
+			return;
+		}
+
+		NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
+				initialPose.prettyPrint().c_str(),
+				groundTruthFrameId_.c_str(),
+				frameId_.c_str());
+		odometry_->reset(initialPose);
+	}
+
+	Transform guess;
+	if(guessFromTf_)
+	{
+		Transform previousPose = this->getTransform(odomFrameId_, frameId_, ros::Time(odometry_->previousStamp()));
+		Transform pose = this->getTransform(odomFrameId_, frameId_, stamp);
+		if(!previousPose.isNull() && !pose.isNull())
+		{
+			guess = previousPose.inverse() * pose;
+
+			/*if(!odometry_->previousVelocityTransform().isNull())
+			{
+				float dt = rtabmap_ros::timestampFromROS(stamp) - odometry_->previousStamp();
+				float vx,vy,vz, vroll,vpitch,vyaw;
+				odometry_->previousVelocityTransform().getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
+				Transform motionGuess(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
+				NODELET_WARN( "P  Guess %s", motionGuess.prettyPrint().c_str());
+			}
+			NODELET_WARN( "TF Guess %s", guess.prettyPrint().c_str());*/
+		}
+	}
+
+	// process data
+	ros::WallTime time = ros::WallTime::now();
+	rtabmap::OdometryInfo info;
+	SensorData dataCpy = data;
+	rtabmap::Transform pose = odometry_->process(dataCpy, guess, &info);
+	if(!pose.isNull())
+	{
+		resetCurrentCount_ = resetCountdown_;
+
+		//*********************
+		// Update odometry
+		//*********************
+		geometry_msgs::TransformStamped poseMsg;
+		poseMsg.child_frame_id = frameId_;
+		poseMsg.header.frame_id = odomFrameId_;
+		poseMsg.header.stamp = stamp;
+		rtabmap_ros::transformToGeometryMsg(pose, poseMsg.transform);
+
+		if(publishTf_)
+		{
+			tfBroadcaster_.sendTransform(poseMsg);
+		}
+
+		if(odomPub_.getNumSubscribers())
+		{
+			//next, we'll publish the odometry message over ROS
+			nav_msgs::Odometry odom;
+			odom.header.stamp = stamp; // use corresponding time stamp to image
+			odom.header.frame_id = odomFrameId_;
+			odom.child_frame_id = frameId_;
+
+			//set the position
+			odom.pose.pose.position.x = poseMsg.transform.translation.x;
+			odom.pose.pose.position.y = poseMsg.transform.translation.y;
+			odom.pose.pose.position.z = poseMsg.transform.translation.z;
+			odom.pose.pose.orientation = poseMsg.transform.rotation;
+
+			//set covariance
+			// libviso2 uses approximately vel variance * 2
+			odom.pose.covariance.at(0) = info.variance*2;  // xx
+			odom.pose.covariance.at(7) = info.variance*2;  // yy
+			odom.pose.covariance.at(14) = info.variance*2; // zz
+			odom.pose.covariance.at(21) = info.variance*2; // rr
+			odom.pose.covariance.at(28) = info.variance*2; // pp
+			odom.pose.covariance.at(35) = info.variance*2; // yawyaw
+
+			//set velocity
+			bool setTwist = !odometry_->previousVelocityTransform().isNull();
+			if(setTwist)
+			{
+				float x,y,z,roll,pitch,yaw;
+				odometry_->previousVelocityTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+				odom.twist.twist.linear.x = x;
+				odom.twist.twist.linear.y = y;
+				odom.twist.twist.linear.z = z;
+				odom.twist.twist.angular.x = roll;
+				odom.twist.twist.angular.y = pitch;
+				odom.twist.twist.angular.z = yaw;
+			}
+
+			odom.twist.covariance.at(0) = setTwist?info.variance:BAD_COVARIANCE;  // xx
+			odom.twist.covariance.at(7) = setTwist?info.variance:BAD_COVARIANCE;  // yy
+			odom.twist.covariance.at(14) = setTwist?info.variance:BAD_COVARIANCE; // zz
+			odom.twist.covariance.at(21) = setTwist?info.variance:BAD_COVARIANCE; // rr
+			odom.twist.covariance.at(28) = setTwist?info.variance:BAD_COVARIANCE; // pp
+			odom.twist.covariance.at(35) = setTwist?info.variance:BAD_COVARIANCE; // yawyaw
+
+			//publish the message
+			odomPub_.publish(odom);
+		}
+
+		// local map / reference frame
+		if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryF2M*>(odometry_))
+		{
+			pcl::PointCloud<pcl::PointXYZ> cloud;
+			const std::multimap<int, cv::Point3f> & map = ((OdometryF2M*)odometry_)->getMap().getWords3();
+			for(std::multimap<int, cv::Point3f>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
+			{
+				cloud.push_back(pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z));
+			}
+			sensor_msgs::PointCloud2 cloudMsg;
+			pcl::toROSMsg(cloud, cloudMsg);
+			cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
+			cloudMsg.header.frame_id = odomFrameId_;
+			odomLocalMap_.publish(cloudMsg);
+		}
+
+		if(odomLastFrame_.getNumSubscribers())
+		{
+			if(dynamic_cast<OdometryF2M*>(odometry_))
+			{
+				const std::multimap<int, cv::Point3f> & words3  = ((OdometryF2M*)odometry_)->getLastFrame().getWords3();
+				if(words3.size())
+				{
+					pcl::PointCloud<pcl::PointXYZ> cloud;
+					for(std::multimap<int, cv::Point3f>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
+					{
+						// transform to odom frame
+						cv::Point3f pt = util3d::transformPoint(iter->second, pose);
+						cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
+					}
+
+					sensor_msgs::PointCloud2 cloudMsg;
+					pcl::toROSMsg(cloud, cloudMsg);
+					cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
+					cloudMsg.header.frame_id = odomFrameId_;
+					odomLastFrame_.publish(cloudMsg);
+				}
+			}
+			else
+			{
+				//Frame to Frame
+				const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame();
+				if(refFrame.getWords3().size())
+				{
+					pcl::PointCloud<pcl::PointXYZ> cloud;
+					for(std::multimap<int, cv::Point3f>::const_iterator iter=refFrame.getWords3().begin(); iter!=refFrame.getWords3().end(); ++iter)
+					{
+						// transform to odom frame
+						cv::Point3f pt = util3d::transformPoint(iter->second, pose);
+						cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
+					}
+					sensor_msgs::PointCloud2 cloudMsg;
+					pcl::toROSMsg(cloud, cloudMsg);
+					cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
+					cloudMsg.header.frame_id = odomFrameId_;
+					odomLastFrame_.publish(cloudMsg);
+				}
+			}
+		}
+	}
+	else if(publishNullWhenLost_)
+	{
+		//NODELET_WARN( "Odometry lost!");
+
+		//send null pose to notify that odometry is lost
+		nav_msgs::Odometry odom;
+		odom.header.stamp = stamp; // use corresponding time stamp to image
+		odom.header.frame_id = odomFrameId_;
+		odom.child_frame_id = frameId_;
+		odom.pose.covariance.at(0) = BAD_COVARIANCE;  // xx
+		odom.pose.covariance.at(7) = BAD_COVARIANCE;  // yy
+		odom.pose.covariance.at(14) = BAD_COVARIANCE; // zz
+		odom.pose.covariance.at(21) = BAD_COVARIANCE; // rr
+		odom.pose.covariance.at(28) = BAD_COVARIANCE; // pp
+		odom.pose.covariance.at(35) = BAD_COVARIANCE; // yawyaw
+		odom.twist.covariance.at(0) = BAD_COVARIANCE;  // xx
+		odom.twist.covariance.at(7) = BAD_COVARIANCE;  // yy
+		odom.twist.covariance.at(14) = BAD_COVARIANCE; // zz
+		odom.twist.covariance.at(21) = BAD_COVARIANCE; // rr
+		odom.twist.covariance.at(28) = BAD_COVARIANCE; // pp
+		odom.twist.covariance.at(35) = BAD_COVARIANCE; // yawyaw
+
+		//publish the message
+		odomPub_.publish(odom);
+	}
+
+	if(pose.isNull() && resetCurrentCount_ > 0)
+	{
+		NODELET_WARN( "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_);
+
+		--resetCurrentCount_;
+		if(resetCurrentCount_ == 0)
+		{
+			// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
+			Transform tfPose = this->getTransform(odomFrameId_, frameId_, stamp);
+			if(tfPose.isNull())
+			{
+				NODELET_WARN( "Odometry automatically reset to latest computed pose!");
+				odometry_->reset(odometry_->getPose());
+			}
+			else
+			{
+				NODELET_WARN( "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
+						odomFrameId_.c_str(), frameId_.c_str());
+				odometry_->reset(tfPose);
+			}
+
+		}
+	}
+
+	if(odomInfoPub_.getNumSubscribers())
+	{
+		rtabmap_ros::OdomInfo infoMsg;
+		odomInfoToROS(info, infoMsg);
+		infoMsg.header.stamp = stamp; // use corresponding time stamp to image
+		infoMsg.header.frame_id = odomFrameId_;
+		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());
+}
+
+bool OdometryROS::isOdometryF2M() const
+{
+	return dynamic_cast<OdometryF2M*>(odometry_) != 0;
+}
+
+bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	NODELET_INFO( "visual_odometry: reset odom!");
+	odometry_->reset();
+	this->flushCallbacks();
+	return true;
+}
+
+bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros::ResetPose::Response&)
+{
+	Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
+	NODELET_INFO( "visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
+	odometry_->reset(pose);
+	this->flushCallbacks();
+	return true;
+}
+
+bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	if(paused_)
+	{
+		NODELET_WARN( "visual_odometry: Already paused!");
+	}
+	else
+	{
+		paused_ = true;
+		NODELET_INFO( "visual_odometry: paused!");
+	}
+	return true;
+}
+
+bool OdometryROS::resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	if(!paused_)
+	{
+		NODELET_WARN( "visual_odometry: Already running!");
+	}
+	else
+	{
+		paused_ = false;
+		NODELET_INFO( "visual_odometry: resumed!");
+	}
+	return true;
+}
+
+bool OdometryROS::setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	NODELET_INFO( "visual_odometry: Set log level to Debug");
+	ULogger::setLevel(ULogger::kDebug);
+	return true;
+}
+bool OdometryROS::setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	NODELET_INFO( "visual_odometry: Set log level to Info");
+	ULogger::setLevel(ULogger::kInfo);
+	return true;
+}
+bool OdometryROS::setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	NODELET_INFO( "visual_odometry: Set log level to Warning");
+	ULogger::setLevel(ULogger::kWarning);
+	return true;
+}
+bool OdometryROS::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	NODELET_INFO( "visual_odometry: Set log level to Error");
+	ULogger::setLevel(ULogger::kError);
+	return true;
+}
+
+
+}
@@ -0,0 +1,120 @@
+/*
+Copyright (c) 2010-2014, 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 <rtabmap_ros/ResetPose.h>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Parameters.h>
+
+namespace rtabmap {
+class Odometry;
+}
+
+namespace rtabmap_ros {
+
+class OdometryROS : public nodelet::Nodelet
+{
+
+public:
+	OdometryROS(bool stereo);
+	virtual ~OdometryROS();
+
+	void processData(const rtabmap::SensorData & data, const ros::Time & stamp);
+
+	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool resetToPose(rtabmap_ros::ResetPose::Request&, rtabmap_ros::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 rtabmap::ParametersMap & parameters() const {return parameters_;}
+	const tf::TransformListener & tfListener() const {return tfListener_;}
+	bool isPaused() const {return paused_;}
+	bool isOdometryF2M() const;
+	rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
+
+protected:
+	virtual void flushCallbacks() = 0;
+
+private:
+	virtual void onInit();
+	virtual void onOdomInit() = 0;
+
+private:
+	rtabmap::Odometry * odometry_;
+
+	// parameters
+	std::string frameId_;
+	std::string odomFrameId_;
+	std::string groundTruthFrameId_;
+	bool publishTf_;
+	bool waitForTransform_;
+	double waitForTransformDuration_;
+	bool publishNullWhenLost_;
+	bool guessFromTf_;
+	rtabmap::ParametersMap parameters_;
+
+	ros::Publisher odomPub_;
+	ros::Publisher odomInfoPub_;
+	ros::Publisher odomLocalMap_;
+	ros::Publisher odomLastFrame_;
+	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_;
+
+	bool paused_;
+	int resetCountdown_;
+	int resetCurrentCount_;
+	bool stereo_;
+};
+
+}
+
+#endif
@@ -0,0 +1,419 @@
+/*
+Copyright (c) 2010-2014, 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 "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 <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include "rtabmap_ros/MsgConversion.h"
+
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+
+using namespace rtabmap;
+
+namespace rtabmap_ros
+{
+
+class RGBDOdometry : public rtabmap_ros::OdometryROS
+{
+public:
+	RGBDOdometry() :
+		OdometryROS(false),
+		approxSync_(0),
+		exactSync_(0),
+		sync2_(0),
+		queueSize_(5)
+	{
+	}
+
+	virtual ~RGBDOdometry()
+	{
+		if(approxSync_)
+		{
+			delete approxSync_;
+		}
+		if(exactSync_)
+		{
+			delete exactSync_;
+		}
+		if(sync2_)
+		{
+			delete sync2_;
+		}
+	}
+
+private:
+
+	virtual void onOdomInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int depthCameras = 1;
+		bool approxSync = true;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("queue_size", queueSize_, queueSize_);
+		pnh.param("depth_cameras", depthCameras, depthCameras);
+		if(depthCameras <= 0)
+		{
+			depthCameras = 1;
+		}
+		if(depthCameras > 2)
+		{
+			NODELET_FATAL("Only 2 cameras maximum supported yet.");
+		}
+
+		if(depthCameras == 2)
+		{
+			ros::NodeHandle rgb0_nh(nh, "rgb0");
+			ros::NodeHandle depth0_nh(nh, "depth0");
+			ros::NodeHandle rgb0_pnh(pnh, "rgb0");
+			ros::NodeHandle depth0_pnh(pnh, "depth0");
+			image_transport::ImageTransport rgb0_it(rgb0_nh);
+			image_transport::ImageTransport depth0_it(depth0_nh);
+			image_transport::TransportHints hintsRgb0("raw", ros::TransportHints(), rgb0_pnh);
+			image_transport::TransportHints hintsDepth0("raw", ros::TransportHints(), depth0_pnh);
+
+			image_mono_sub_.subscribe(rgb0_it, rgb0_nh.resolveName("image"), 1, hintsRgb0);
+			image_depth_sub_.subscribe(depth0_it, depth0_nh.resolveName("image"), 1, hintsDepth0);
+			info_sub_.subscribe(rgb0_nh, "camera_info", 1);
+
+			ros::NodeHandle rgb1_nh(nh, "rgb1");
+			ros::NodeHandle depth1_nh(nh, "depth1");
+			ros::NodeHandle rgb1_pnh(pnh, "rgb1");
+			ros::NodeHandle depth1_pnh(pnh, "depth1");
+			image_transport::ImageTransport rgb1_it(rgb1_nh);
+			image_transport::ImageTransport depth1_it(depth1_nh);
+			image_transport::TransportHints hintsRgb1("raw", ros::TransportHints(), rgb1_pnh);
+			image_transport::TransportHints hintsDepth1("raw", ros::TransportHints(), depth1_pnh);
+
+			image_mono2_sub_.subscribe(rgb1_it, rgb1_nh.resolveName("image"), 1, hintsRgb1);
+			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_,
+					image_depth_sub_,
+					info_sub_,
+					image_mono2_sub_,
+					image_depth2_sub_,
+					info2_sub_);
+			sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
+		}
+		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_);
+				approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _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, _1, _2, _3));
+			}
+
+			NODELET_INFO("\n%s subscribed to (%s sync):\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());
+		}
+	}
+
+	void callback(
+			const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& depth,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		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;
+			}
+
+			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);
+			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);
+
+				rtabmap::SensorData data(
+						ptrImage->image,
+						ptrDepth->image,
+						rtabmapModel,
+						0,
+						rtabmap_ros::timestampFromROS(stamp));
+
+				this->processData(data, stamp);
+			}
+		}
+	}
+
+	void callback2(
+			const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& depth,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo,
+			const sensor_msgs::ImageConstPtr& image2,
+			const sensor_msgs::ImageConstPtr& depth2,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo2)
+	{
+		if(!this->isPaused())
+		{
+			std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
+			std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
+			std::vector<sensor_msgs::CameraInfoConstPtr> infoMsgs;
+			imageMsgs.push_back(image);
+			imageMsgs.push_back(image2);
+			depthMsgs.push_back(depth);
+			depthMsgs.push_back(depth2);
+			infoMsgs.push_back(cameraInfo);
+			infoMsgs.push_back(cameraInfo2);
+
+			ros::Time higherStamp;
+			int imageWidth = imageMsgs[0]->width;
+			int imageHeight = imageMsgs[0]->height;
+			int cameraCount = imageMsgs.size();
+			cv::Mat rgb;
+			cv::Mat depth;
+			pcl::PointCloud<pcl::PointXYZ> scanCloud;
+			std::vector<CameraModel> cameraModels;
+			for(unsigned int i=0; i<imageMsgs.size(); ++i)
+			{
+				if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+					!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+					 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
+					 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
+				{
+					NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
+					return;
+				}
+				UASSERT_MSG(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight,
+						uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
+								imageWidth,
+								imageMsgs[i]->width,
+								imageHeight,
+								imageMsgs[i]->height).c_str());
+				UASSERT_MSG(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight,
+						uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
+								imageWidth,
+								depthMsgs[i]->width,
+								imageHeight,
+								depthMsgs[i]->height).c_str());
+
+				ros::Time stamp = imageMsgs[i]->header.stamp>depthMsgs[i]->header.stamp?imageMsgs[i]->header.stamp:depthMsgs[i]->header.stamp;
+
+				if(i == 0)
+				{
+					higherStamp = stamp;
+				}
+				else if(stamp > higherStamp)
+				{
+					higherStamp = stamp;
+				}
+
+				Transform localTransform = getTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp);
+				if(localTransform.isNull())
+				{
+					return;
+				}
+
+				cv_bridge::CvImageConstPtr ptrImage;
+				if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
+				{
+					ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
+				}
+				else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+				   imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+				{
+					ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
+				}
+				else
+				{
+					ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8");
+				}
+				cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
+				cv::Mat subDepth = ptrDepth->image;
+
+				// initialize
+				if(rgb.empty())
+				{
+					rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
+				}
+				if(depth.empty())
+				{
+					depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.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!");
+					return;
+				}
+
+				if(subDepth.type() == depth.type())
+				{
+					subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
+				}
+				else
+				{
+					NODELET_ERROR("Some Depth images are not the same type!");
+					return;
+				}
+
+				cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*infoMsgs[i], localTransform));
+			}
+
+			rtabmap::SensorData data(
+					rgb,
+					depth,
+					cameraModels,
+					0,
+					rtabmap_ros::timestampFromROS(higherStamp));
+
+			this->processData(data, higherStamp);
+		}
+	}
+
+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, _1, _2, _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, _1, _2, _3));
+		}
+		if(sync2_)
+		{
+			delete sync2_;
+			sync2_ = new message_filters::Synchronizer<MySync2Policy>(
+					MySync2Policy(queueSize_),
+					image_mono_sub_,
+					image_depth_sub_,
+					info_sub_,
+					image_mono2_sub_,
+					image_depth2_sub_,
+					info2_sub_);
+			sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
+		}
+	}
+
+private:
+	image_transport::SubscriberFilter image_mono_sub_;
+	image_transport::SubscriberFilter image_depth_sub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
+	image_transport::SubscriberFilter image_mono2_sub_;
+	image_transport::SubscriberFilter image_depth2_sub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> info2_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::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySync2Policy;
+	message_filters::Synchronizer<MySync2Policy> * sync2_;
+	int queueSize_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDOdometry, nodelet::Nodelet);
+
+}
@@ -0,0 +1,235 @@
+/*
+Copyright (c) 2010-2014, 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 "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 <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_ros/MsgConversion.h"
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
+
+using namespace rtabmap;
+
+namespace rtabmap_ros
+{
+
+class StereoOdometry : public rtabmap_ros::OdometryROS
+{
+public:
+	StereoOdometry() :
+		rtabmap_ros::OdometryROS(true),
+		approxSync_(0),
+		exactSync_(0),
+		queueSize_(5)
+	{
+	}
+
+	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;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("queue_size", queueSize_, queueSize_);
+		NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
+
+		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_);
+			approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
+		}
+		else
+		{
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, 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",
+				imageRectLeft_.getTopic().c_str(),
+				imageRectRight_.getTopic().c_str(),
+				cameraInfoLeft_.getTopic().c_str(),
+				cameraInfoRight_.getTopic().c_str());
+	}
+
+	void callback(
+			const sensor_msgs::ImageConstPtr& imageRectLeft,
+			const sensor_msgs::ImageConstPtr& imageRectRight,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
+	{
+		if(!this->isPaused())
+		{
+			if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+				 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+				 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+				 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+				!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+				  imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+				  imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+				  imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
+			{
+				NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended)");
+				return;
+			}
+
+			ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp;
+
+			Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp);
+			if(localTransform.isNull())
+			{
+				return;
+			}
+
+			ros::WallTime time = ros::WallTime::now();
+
+			int quality = -1;
+			if(imageRectLeft->data.size() && imageRectRight->data.size())
+			{
+				rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform);
+				if(stereoModel.baseline() <= 0)
+				{
+					NODELET_FATAL("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::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8");
+				cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8");
+
+				UTimer stepTimer;
+				//
+				UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
+				rtabmap::SensorData data(
+						ptrImageLeft->image,
+						ptrImageRight->image,
+						stereoModel,
+						0,
+						rtabmap_ros::timestampFromROS(stamp));
+
+				this->processData(data, stamp);
+			}
+			else
+			{
+				NODELET_WARN("Odom: input images empty?!?");
+			}
+		}
+	}
+
+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, _1, _2, _3, _4));
+		}
+		if(exactSync_)
+		{
+			delete exactSync_;
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
+		}
+	}
+
+private:
+	image_transport::SubscriberFilter imageRectLeft_;
+	image_transport::SubscriberFilter imageRectRight_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
+	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_;
+	int queueSize_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoOdometry, nodelet::Nodelet);
+
+}
+