Updated for rtabmap 0.8.12. Updated synchronization of messages (laser scan and images) with odometry pose stamp. Removed data_recorder node, use data_recorder.launch instead.

src/CoreWrapper.cpp

@@ -185,6 +185,10 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	pnh.param("map_cache_cleanup", mapCacheCleanup_, mapCacheCleanup_);
 
 	ROS_INFO("rtabmap: frame_id = %s", frameId_.c_str());
+	if(!odomFrameId_.empty())
+	{
+		ROS_INFO("rtabmap: odom_frame_id = %s", odomFrameId_.c_str());
+	}
 	ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str());
 	ROS_INFO("rtabmap: queue_size = %d", queueSize);
 	ROS_INFO("rtabmap: tf_delay = %f", tfDelay);
@@ -305,6 +309,12 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 						Parameters::kMemNotLinkedNodesKept().c_str());
 				parameters_.at(Parameters::kMemNotLinkedNodesKept())= vStr;
 			}
+			else if(iter->compare("RGBD/LocalLoopDetectionMaxDiffID") == 0)
+			{
+				ROS_WARN("Parameter name changed: RGBD/LocalLoopDetectionMaxDiffID -> %s. Please update your launch file accordingly.",
+						Parameters::kRGBDLocalLoopDetectionMaxGraphDepth().c_str());
+				parameters_.at(Parameters::kRGBDLocalLoopDetectionMaxGraphDepth())= vStr;
+			}
 		}
 	}
 
@@ -331,17 +341,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 					  "detection on images-only.");
 		}
 	}
-	else
-	{
-		// loop closure detection (images-only)
-		if(subscribeDepth || subscribeLaserScan || subscribeStereo)
-		{
-			ROS_WARN("ROS param subscribe_depth, subscribe_laserScan or subscribe_stereo is true, but RTAB-Map "
-					  "parameter \"RGBD/Enabled\" is false! Please set subscribe_depth, subscribe_laserScan and subscribe_stereo "
-					  "to false to use rtabmap node for loop closure detection on images-only, or set \"RGBD/Enabled\" to true "
-					  "for RGB-D SLAM.");
-		}
-	}
+
 	if(paused_)
 	{
 		ROS_WARN("Node paused... don't forget to call service \"resume\" to start rtabmap.");
@@ -577,6 +577,7 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
 		}
 
 		lastPose_ = odom;
+		lastPoseStamp_ = odomMsg->header.stamp;
 		float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
 		float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
 		if(uIsFinite(rotVariance) && rotVariance > rotVariance_)
@@ -638,6 +639,7 @@ bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp)
 		}
 
 		lastPose_ = odom;
+		lastPoseStamp_ = stamp;
 		// Throttle
 		if(rate_>0.0f)
 		{
@@ -652,7 +654,7 @@ bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp)
 	return false;
 }
 
-Transform CoreWrapper::getLocalTransform(const std::string & sensorFrameId, const ros::Time & stamp) const
+Transform CoreWrapper::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const
 {
 	// TF ready?
 	Transform localTransform;
@@ -660,15 +662,15 @@ Transform CoreWrapper::getLocalTransform(const std::string & sensorFrameId, cons
 	{
 		if(waitForTransform_)
 		{
-			if(!tfListener_.waitForTransform(frameId_, sensorFrameId, stamp, ros::Duration(1)))
+			if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
 			{
-				ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), sensorFrameId.c_str());
+				ROS_WARN("Could not get transform from %s to %s after 1 second!", fromFrameId.c_str(), toFrameId.c_str());
 				return localTransform;
 			}
 		}
 
 		tf::StampedTransform tmp;
-		tfListener_.lookupTransform(frameId_, sensorFrameId, stamp, tmp);
+		tfListener_.lookupTransform(fromFrameId, toFrameId, stamp, tmp);
 		localTransform = rtabmap_ros::transformFromTF(tmp);
 	}
 	catch(tf::TransformException & ex)
@@ -697,17 +699,38 @@ void CoreWrapper::commonDepthCallback(
 		return;
 	}
 
-	Transform localTransform = getLocalTransform(depthMsg->header.frame_id, depthMsg->header.stamp);
+	//for sync transform
+	Transform odomT = getTransform(odomFrameId, frameId_, lastPoseStamp_);
+	if(odomT.isNull() && !odomFrameId_.empty())
+	{
+		ROS_WARN("Could not get TF transform from %s to %s, sensors will not be synchronized with odometry pose.",
+				odomFrameId.c_str(), frameId_.c_str());
+	}
+
+	Transform localTransform = getTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp);
 	if(localTransform.isNull())
 	{
 		return;
 	}
+	// sync with odometry stamp
+	if(lastPoseStamp_ != depthMsg->header.stamp)
+	{
+		if(!odomT.isNull())
+		{
+			Transform sensorT = getTransform(odomFrameId, frameId_, depthMsg->header.stamp);
+			if(sensorT.isNull())
+			{
+				return;
+			}
+			localTransform = odomT.inverse() * sensorT * localTransform;
+		}
+	}
 
 	cv::Mat scan;
 	if(scanMsg.get() != 0)
 	{
 		// make sure the frame of the laser is updated too
-		if(getLocalTransform(scanMsg->header.frame_id, scanMsg->header.stamp).isNull())
+		if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull())
 		{
 			return;
 		}
@@ -716,9 +739,25 @@ void CoreWrapper::commonDepthCallback(
 		sensor_msgs::PointCloud2 scanOut;
 		laser_geometry::LaserProjection projection;
 		projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
-		pcl::PointCloud<pcl::PointXYZ> pclScan;
-		pcl::fromROSMsg(scanOut, pclScan);
-		scan = util3d::laserScanFromPointCloud(pclScan);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::fromROSMsg(scanOut, *pclScan);
+
+		// sync with odometry stamp
+		if(lastPoseStamp_ != scanMsg->header.stamp)
+		{
+			if(!odomT.isNull())
+			{
+				Transform sensorT = getTransform(odomFrameId, frameId_, scanMsg->header.stamp);
+				if(sensorT.isNull())
+				{
+					return;
+				}
+				Transform t = odomT.inverse() * sensorT;
+				pclScan = util3d::transformPointCloud<pcl::PointXYZ>(pclScan, t);
+
+			}
+		}
+		scan = util3d::laserScanFromPointCloud(*pclScan);
 	}
 
 	cv_bridge::CvImageConstPtr ptrImage;
@@ -741,7 +780,7 @@ void CoreWrapper::commonDepthCallback(
 	float cy = model.cy();
 
 	process(ptrImage->header.seq,
-			scanMsg.get() != 0?scanMsg->header.stamp:ptrImage->header.stamp,
+			scanMsg.get() != 0?scanMsg->header.stamp:ptrDepth->header.stamp,
 			ptrImage->image,
 			lastPose_,
 			odomFrameId,
@@ -754,7 +793,7 @@ void CoreWrapper::commonDepthCallback(
 			cy,
 			localTransform,
 			scan,
-			(int)scanMsg->ranges.size());
+			scanMsg.get() != 0?(int)scanMsg->ranges.size():0);
 	rotVariance_ = 0;
 	transVariance_ = 0;
 }
@@ -780,17 +819,39 @@ void CoreWrapper::commonStereoCallback(
 		return;
 	}
 
-	Transform localTransform = getLocalTransform(leftImageMsg->header.frame_id, leftImageMsg->header.stamp);
+	//for sync transform
+	Transform odomT = getTransform(odomFrameId, frameId_, lastPoseStamp_);
+	if(odomT.isNull() && !odomFrameId_.empty())
+	{
+		ROS_WARN("Could not get TF transform from %s to %s, sensors will not be synchronized with odometry pose.",
+				odomFrameId.c_str(), frameId_.c_str());
+	}
+
+	Transform localTransform = getTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp);
 	if(localTransform.isNull())
 	{
 		return;
 	}
 
+	// sync with odometry stamp
+	if(lastPoseStamp_ != leftImageMsg->header.stamp)
+	{
+		if(!odomT.isNull())
+		{
+			Transform sensorT = getTransform(odomFrameId, frameId_, leftImageMsg->header.stamp);
+			if(sensorT.isNull())
+			{
+				return;
+			}
+			localTransform = odomT.inverse() * sensorT * localTransform;
+		}
+	}
+
 	cv::Mat scan;
 	if(scanMsg.get() != 0)
 	{
 		// make sure the frame of the laser is updated too
-		if(getLocalTransform(scanMsg->header.frame_id, scanMsg->header.stamp).isNull())
+		if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull())
 		{
 			return;
 		}
@@ -798,9 +859,25 @@ void CoreWrapper::commonStereoCallback(
 		sensor_msgs::PointCloud2 scanOut;
 		laser_geometry::LaserProjection projection;
 		projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
-		pcl::PointCloud<pcl::PointXYZ> pclScan;
-		pcl::fromROSMsg(scanOut, pclScan);
-		scan = util3d::laserScanFromPointCloud(pclScan);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::fromROSMsg(scanOut, *pclScan);
+
+		// sync with odometry stamp
+		if(lastPoseStamp_ != scanMsg->header.stamp)
+		{
+			if(!odomT.isNull())
+			{
+				Transform sensorT = getTransform(odomFrameId, frameId_, scanMsg->header.stamp);
+				if(sensorT.isNull())
+				{
+					return;
+				}
+				Transform t = odomT.inverse() * sensorT;
+				pclScan = util3d::transformPointCloud<pcl::PointXYZ>(pclScan, t);
+
+			}
+		}
+		scan = util3d::laserScanFromPointCloud(*pclScan);
 	}
 
 	cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
@@ -824,7 +901,7 @@ void CoreWrapper::commonStereoCallback(
 	float baseline = model.baseline();
 
 	process(leftImageMsg->header.seq,
-			leftImageMsg->header.stamp,
+			scanMsg.get() != 0?scanMsg->header.stamp:leftImageMsg->header.stamp,
 			ptrLeftImage->image,
 			lastPose_,
 			odomFrameId,
@@ -837,7 +914,7 @@ void CoreWrapper::commonStereoCallback(
 			cy,
 			localTransform,
 			scan,
-			(int)scanMsg->ranges.size());
+			scanMsg.get() != 0?(int)scanMsg->ranges.size():0);
 	rotVariance_ = 0;
 	transVariance_ = 0;
 }
@@ -1121,12 +1198,13 @@ void CoreWrapper::process(
 		{
 			timeRtabmap = timer.ticks();
 		}
-		ROS_INFO("rtabmap: Update rate=%fs, Limit=%fs, RTAB-Map=%fs, Publish=%fs (%d local nodes)",
-					1.0f/rate_,
-					rtabmap_.getTimeThreshold()/1000.0f,
-					timeRtabmap,
-					timer.ticks(),
-					rtabmap_.getWMSize()+rtabmap_.getSTMSize());
+		ROS_INFO("rtabmap: Rate=%.2fs, Limit=%.3fs, RTAB-Map=%.4fs, Pub=%.4fs (local map=%d, WM=%d)",
+				rate_>0?1.0f/rate_:0,
+				rtabmap_.getTimeThreshold()/1000.0f,
+				timeRtabmap,
+				timer.ticks(),
+				(int)rtabmap_.getLocalOptimizedPoses().size(),
+				rtabmap_.getWMSize()+rtabmap_.getSTMSize());
 	}
 	else if(!rtabmap_.isIDsGenerated())
 	{

src/CoreWrapper.h

@@ -94,7 +94,7 @@ private:
 
 	bool commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg);
 	bool commonOdomTFUpdate(const ros::Time & stamp); // TF odom
-	rtabmap::Transform getLocalTransform(const std::string & sensorFrameId, const ros::Time & stamp) const;
+	rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
 
 	void commonDepthCallback(
 				const std::string & odomFrameId,
@@ -223,6 +223,7 @@ private:
 	rtabmap::Rtabmap rtabmap_;
 	bool paused_;
 	rtabmap::Transform lastPose_;
+	ros::Time lastPoseStamp_;
 	float rotVariance_;
 	float transVariance_;
 	rtabmap::Transform currentMetricGoal_;

src/DataRecorderNode.cpp

@@ -1,813 +0,0 @@
-/*
-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 <ros/ros.h>
-
-#include "rtabmap/utilite/ULogger.h"
-
-#include <tf/tf.h>
-#include <tf/transform_listener.h>
-
-#include <cv_bridge/cv_bridge.h>
-
-#include <sensor_msgs/Image.h>
-#include <sensor_msgs/CameraInfo.h>
-#include <sensor_msgs/LaserScan.h>
-#include <nav_msgs/Odometry.h>
-
-#include <message_filters/subscriber.h>
-#include <message_filters/synchronizer.h>
-#include <message_filters/sync_policies/approximate_time.h>
-#include <message_filters/sync_policies/exact_time.h>
-
-#include <image_transport/image_transport.h>
-#include <image_transport/subscriber_filter.h>
-
-#include <image_geometry/stereo_camera_model.h>
-
-#include "rtabmap_ros/MsgConversion.h"
-
-#include <pcl_ros/transforms.h>
-#include <pcl_conversions/pcl_conversions.h>
-#include <laser_geometry/laser_geometry.h>
-
-#include <rtabmap/core/util3d.h>
-
-#include <QtGui/QApplication>
-
-#include "rtabmap/gui/DataRecorder.h"
-
-using namespace rtabmap;
-
-float max3( const float& a, const float& b, const float& c)
-{
-	float m=a>b?a:b;
-	return m>c?m:c;
-}
-
-class DataRecorderWrapper
-{
-public:
-	DataRecorderWrapper() :
-		fileName_("output.db"),
-		frameId_("base_link"),
-		odomFrameId_(""), // null = use topic, otherwise use referred TF.
-		waitForTransform_(false),
-		depthOdomScanSync_(0),
-		depthSync_(0),
-		depthImageSync_(0)
-	{
-		ros::NodeHandle pnh("~");
-
-		bool subscribeOdometry = false;
-		bool subscribeLaserScan = false;
-		bool subscribeDepth = false;
-		bool subscribeStereo = false;
-		int queueSize = 10;
-		double agePenalty = 0.1;
-		pnh.param("subscribe_odometry", subscribeOdometry, subscribeOdometry);
-		pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
-		pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
-		pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
-		pnh.param("queue_size", queueSize, queueSize);
-		pnh.param("age_penalty", agePenalty, agePenalty);
-		pnh.param("output_file_name", fileName_, fileName_);
-		pnh.param("frame_id", frameId_, frameId_);
-		pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
-		pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
-		if(!subscribeOdometry)
-		{
-			UWARN("odom_frame_id set without setting subscribe_odometry to true. Ignoring odometry from TF.");
-			odomFrameId_.clear();
-		}
-
-		setupCallbacks(subscribeOdometry, subscribeDepth, subscribeStereo, subscribeLaserScan, queueSize, agePenalty);
-	}
-	bool init()
-	{
-		return recorder_.init(fileName_.c_str());
-	}
-
-	virtual ~DataRecorderWrapper()
-	{
-		if(depthOdomScanSync_)
-			delete depthOdomScanSync_;
-		if(depthSync_)
-			delete depthSync_;
-		if(depthImageSync_)
-			delete depthImageSync_;
-	}
-
-private:
-	void setupCallbacks(
-			bool subscribeOdom,
-			bool subscribeDepth,
-			bool subscribeStereo,
-			bool subscribeLaserScan,
-			int queueSize,
-			double agePenalty)
-	{
-		if(subscribeStereo)
-		{
-			ros::NodeHandle nh; // public
-			ros::NodeHandle pnh("~"); // private
-			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);
-
-			imageSub_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
-			imageRightSub_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
-			cameraInfoSub_.subscribe(left_nh, "camera_info", 1);
-			cameraInfoRightSub_.subscribe(right_nh, "camera_info", 1);
-			if(subscribeOdom)
-			{
-				ROS_INFO("Registering Stereo+Odom callback...");
-				odomSub_.subscribe(nh, "odom", 1);
-				stereoOdomSync_ = new message_filters::Synchronizer<MyStereoOdomSyncPolicy>(MyStereoOdomSyncPolicy(queueSize), imageSub_, imageRightSub_, cameraInfoSub_, cameraInfoRightSub_, odomSub_);
-				stereoOdomSync_->setAgePenalty(agePenalty);
-				stereoOdomSync_->registerCallback(boost::bind(&DataRecorderWrapper::stereoOdomCallback, this, _1, _2, _3, _4, _5));
-			}
-			else
-			{
-				ROS_INFO("Registering Stereo callback...");
-				stereoSync_ = new message_filters::Synchronizer<MyStereoSyncPolicy>(MyStereoSyncPolicy(queueSize), imageSub_, imageRightSub_, cameraInfoSub_, cameraInfoRightSub_);
-				stereoSync_->registerCallback(boost::bind(&DataRecorderWrapper::stereoCallback, this, _1, _2, _3, _4));
-			}
-		}
-		else
-		{
-			ros::NodeHandle nh; // public
-			ros::NodeHandle pnh("~"); // private
-			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);
-
-			if(subscribeOdom && subscribeDepth && subscribeLaserScan)
-			{
-				ROS_INFO("Registering Depth+LaserScan callback...");
-				imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
-				imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
-				cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
-				scanSub_.subscribe(nh, "scan", 1);
-				if(odomFrameId_.empty())
-				{
-					odomSub_.subscribe(nh, "odom", 1);
-					depthOdomScanSync_ = new message_filters::Synchronizer<MyDepthOdomScanSyncPolicy>(MyDepthOdomScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
-					depthOdomScanSync_->setAgePenalty(agePenalty);
-					depthOdomScanSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthOdomScanCallback, this, _1, _2, _3, _4, _5));
-				}
-				else
-				{
-					depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
-					depthScanSync_->setAgePenalty(agePenalty);
-					depthScanSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthScanCallback, this, _1, _2, _3, _4));
-				}
-			}
-			else if(subscribeOdom && subscribeDepth && !subscribeLaserScan)
-			{
-				ROS_INFO("Registering Depth callback...");
-				imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
-				imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
-				cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
-				odomSub_.subscribe(nh, "odom", 1);
-				depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
-				depthSync_->setAgePenalty(agePenalty);
-				depthSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthCallback, this, _1, _2, _3, _4));
-			}
-			else if(!subscribeOdom && subscribeDepth)
-			{
-				ROS_INFO("Registering to depth without odometry callback...");
-				imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
-				imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
-				cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
-				depthImageSync_ = new message_filters::Synchronizer<MyDepthImageSyncPolicy>(MyDepthImageSyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
-				depthImageSync_->setAgePenalty(agePenalty);
-				depthImageSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthImageCallback, this, _1, _2, _3));
-			}
-			else
-			{
-				if(subscribeOdom && !subscribeDepth && subscribeLaserScan)
-				{
-					ROS_WARN("Cannot record only laser scan without depth images...");
-				}
-				ROS_INFO("Registering default callback (\"image\" only)...");
-				defaultSub_ = rgb_it.subscribe("image", 1, &DataRecorderWrapper::defaultCallback, this);
-			}
-		}
-	}
-
-	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
-	{
-		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
-		rtabmap::SensorData data(ptrImage->image.clone());
-		recorder_.addData(data);
-	}
-
-	void depthImageCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& depthMsg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
-	{
-		// TF ready?
-		Transform localTransform;
-		try
-		{
-			if(waitForTransform_)
-			{
-				if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
-				{
-					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
-					return;
-				}
-			}
-
-			tf::StampedTransform tmp;
-			tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
-			localTransform = rtabmap_ros::transformFromTF(tmp);
-		}
-		catch(tf::TransformException & ex)
-		{
-			ROS_WARN("%s",ex.what());
-			return;
-		}
-
-		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
-		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
-
-		image_geometry::PinholeCameraModel model;
-		model.fromCameraInfo(*cameraInfoMsg);
-		float fx = model.fx();
-		float fy = model.fy();
-		float cx = model.cx();
-		float cy = model.cy();
-
-		cv::Mat depth16;
-		if(ptrDepth->image.type() != CV_16UC1)
-		{
-			if(ptrDepth->image.type() == CV_32FC1)
-			{
-				//convert to 16 bits
-				depth16 = util3d::cvtDepthFromFloat(ptrDepth->image);
-				static bool shown = false;
-				if(!shown)
-				{
-					ROS_WARN("Use depth image with \"unsigned short\" type to "
-							 "avoid conversion. This message is only printed once...");
-					shown = true;
-				}
-			}
-			else
-			{
-				ROS_ERROR("Depth image must be of type \"unsigned short\"!");
-				return;
-			}
-		}
-		else
-		{
-			depth16 = ptrDepth->image.clone();
-		}
-
-		rtabmap::SensorData data(
-			ptrImage->image.clone(),
-			depth16,
-			fx,
-			fy,
-			cx,
-			cy,
-			localTransform,
-			Transform(),
-			1.0f,
-			1.0f,
-			0,
-			rtabmap_ros::timestampFromROS(imageMsg->header.stamp));
-		recorder_.addData(data);
-	}
-
-	void depthCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& depthMsg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
-	{
-		// TF ready?
-		Transform localTransform;
-		try
-		{
-			if(waitForTransform_)
-			{
-				if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
-				{
-					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
-					return;
-				}
-			}
-
-			tf::StampedTransform tmp;
-			tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
-			localTransform = rtabmap_ros::transformFromTF(tmp);
-		}
-		catch(tf::TransformException & ex)
-		{
-			ROS_WARN("%s",ex.what());
-			return;
-		}
-
-		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
-		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
-
-		image_geometry::PinholeCameraModel model;
-		model.fromCameraInfo(*cameraInfoMsg);
-		float fx = model.fx();
-		float fy = model.fy();
-		float cx = model.cx();
-		float cy = model.cy();
-
-		cv::Mat depth16;
-		if(ptrDepth->image.type() != CV_16UC1)
-		{
-			if(ptrDepth->image.type() == CV_32FC1)
-			{
-				//convert to 16 bits
-				depth16 = util3d::cvtDepthFromFloat(ptrDepth->image);
-				static bool shown = false;
-				if(!shown)
-				{
-					ROS_WARN("Use depth image with \"unsigned short\" type to "
-							 "avoid conversion. This message is only printed once...");
-					shown = true;
-				}
-			}
-			else
-			{
-				ROS_ERROR("Depth image must be of type \"unsigned short\"!");
-				return;
-			}
-		}
-		else
-		{
-			depth16 = ptrDepth->image.clone();
-		}
-
-		float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
-		float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
-
-		rtabmap::SensorData data(
-			ptrImage->image.clone(),
-			depth16,
-			fx,
-			fy,
-			cx,
-			cy,
-			localTransform,
-			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
-			uIsFinite(rotVariance) && rotVariance>0?rotVariance:1,
-			uIsFinite(transVariance) && transVariance>0?transVariance:1,
-			0,
-			rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
-		recorder_.addData(data);
-	}
-
-	void depthOdomScanCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& depthMsg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scanMsg)
-	{
-		// TF ready?
-		Transform localTransform;
-		try
-		{
-			if(waitForTransform_)
-			{
-				if(!tfListener_.waitForTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, ros::Duration(1)))
-				{
-					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), scanMsg->header.frame_id.c_str());
-					return;
-				}
-				if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
-				{
-					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
-					return;
-				}
-			}
-
-			tf::StampedTransform tmp;
-			tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
-			tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
-			localTransform = rtabmap_ros::transformFromTF(tmp);
-		}
-		catch(tf::TransformException & ex)
-		{
-			ROS_WARN("%s",ex.what());
-			return;
-		}
-
-		//transform in frameId_ frame
-		sensor_msgs::PointCloud2 scanOut;
-		laser_geometry::LaserProjection projection;
-		projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
-		pcl::PointCloud<pcl::PointXYZ> pclScan;
-		pcl::fromROSMsg(scanOut, pclScan);
-		cv::Mat scan = util3d::laserScanFromPointCloud(pclScan);
-
-		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
-		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
-
-		image_geometry::PinholeCameraModel model;
-		model.fromCameraInfo(*cameraInfoMsg);
-		float fx = model.fx();
-		float fy = model.fy();
-		float cx = model.cx();
-		float cy = model.cy();
-
-		cv::Mat depth16;
-		if(ptrDepth->image.type() != CV_16UC1)
-		{
-			if(ptrDepth->image.type() == CV_32FC1)
-			{
-				//convert to 16 bits
-				depth16 = util3d::cvtDepthFromFloat(ptrDepth->image);
-				static bool shown = false;
-				if(!shown)
-				{
-					ROS_WARN("Use depth image with \"unsigned short\" type to "
-							 "avoid conversion. This message is only printed once...");
-					shown = true;
-				}
-			}
-			else
-			{
-				ROS_ERROR("Depth image must be of type \"unsigned short\"!");
-				return;
-			}
-		}
-		else
-		{
-			depth16 = ptrDepth->image.clone();
-		}
-
-		float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
-		float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
-
-		rtabmap::SensorData data(
-			scan,
-			(int)scanMsg->ranges.size(),
-			ptrImage->image.clone(),
-			depth16,
-			fx,
-			fy,
-			cx,
-			cy,
-			localTransform,
-			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
-			uIsFinite(rotVariance) && rotVariance>0?rotVariance:1,
-			uIsFinite(transVariance) && transVariance>0?transVariance:1,
-			0,
-			rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
-		recorder_.addData(data);
-	}
-
-	void depthScanCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& depthMsg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scanMsg)
-	{
-		// TF ready?
-		Transform localTransform;
-		Transform odom;
-		try
-		{
-			if(waitForTransform_)
-			{
-				if(!tfListener_.waitForTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, ros::Duration(1)))
-				{
-					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), scanMsg->header.frame_id.c_str());
-					return;
-				}
-				if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
-				{
-					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
-					return;
-				}
-				if(!odomFrameId_.empty())
-				{
-					if(!tfListener_.waitForTransform(odomFrameId_, frameId_, scanMsg->header.stamp, ros::Duration(1)))
-					{
-						ROS_WARN("Could not get transform from %s to %s after 1 second!", odomFrameId_.c_str(), frameId_.c_str());
-						return;
-					}
-				}
-			}
-
-			tf::StampedTransform tmp;
-			tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
-			tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
-			localTransform = rtabmap_ros::transformFromTF(tmp);
-			if(!odomFrameId_.empty())
-			{
-				tfListener_.lookupTransform(odomFrameId_, frameId_, scanMsg->header.stamp, tmp);
-				odom = rtabmap_ros::transformFromTF(tmp);
-			}
-		}
-		catch(tf::TransformException & ex)
-		{
-			ROS_WARN("%s",ex.what());
-			return;
-		}
-
-		//transform in frameId_ frame
-		sensor_msgs::PointCloud2 scanOut;
-		laser_geometry::LaserProjection projection;
-		projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
-		pcl::PointCloud<pcl::PointXYZ> pclScan;
-		pcl::fromROSMsg(scanOut, pclScan);
-		cv::Mat scan = util3d::laserScanFromPointCloud(pclScan);
-
-		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
-		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
-
-		image_geometry::PinholeCameraModel model;
-		model.fromCameraInfo(*cameraInfoMsg);
-		float fx = model.fx();
-		float fy = model.fy();
-		float cx = model.cx();
-		float cy = model.cy();
-
-		cv::Mat depth16;
-		if(ptrDepth->image.type() != CV_16UC1)
-		{
-			if(ptrDepth->image.type() == CV_32FC1)
-			{
-				//convert to 16 bits
-				depth16 = util3d::cvtDepthFromFloat(ptrDepth->image);
-				static bool shown = false;
-				if(!shown)
-				{
-					ROS_WARN("Use depth image with \"unsigned short\" type to "
-							 "avoid conversion. This message is only printed once...");
-					shown = true;
-				}
-			}
-			else
-			{
-				ROS_ERROR("Depth image must be of type \"unsigned short\"!");
-				return;
-			}
-		}
-		else
-		{
-			depth16 = ptrDepth->image.clone();
-		}
-
-		rtabmap::SensorData data(
-			scan,
-			(int)scanMsg->ranges.size(),
-			ptrImage->image.clone(),
-			depth16,
-			fx,
-			fy,
-			cx,
-			cy,
-			localTransform,
-			odom,
-			1,
-			1,
-			0,
-			rtabmap_ros::timestampFromROS(scanMsg->header.stamp));
-		recorder_.addData(data);
-	}
-
-	void stereoOdomCallback(
-				const sensor_msgs::ImageConstPtr& leftImageMsg,
-				const sensor_msgs::ImageConstPtr& rightImageMsg,
-				const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-				const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg,
-				const nav_msgs::OdometryConstPtr & odomMsg)
-	{
-		// TF ready?
-		Transform localTransform;
-		try
-		{
-			if(waitForTransform_)
-			{
-				if(!tfListener_.waitForTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, ros::Duration(1)))
-				{
-					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), leftImageMsg->header.frame_id.c_str());
-					return;
-				}
-			}
-
-			tf::StampedTransform tmp;
-			tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
-			localTransform = rtabmap_ros::transformFromTF(tmp);
-		}
-		catch(tf::TransformException & ex)
-		{
-			ROS_WARN("%s",ex.what());
-			return;
-		}
-
-		cv_bridge::CvImageConstPtr ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8");
-		cv_bridge::CvImageConstPtr ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
-
-		image_geometry::StereoCameraModel model;
-		model.fromCameraInfo(*leftCameraInfoMsg, *rightCameraInfoMsg);
-		float fx = model.right().fx();
-		float baseline = model.baseline();
-		float cx = model.right().cx();
-		float cy = model.right().cy();
-
-		float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
-		float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
-		rtabmap::SensorData data(
-				ptrLeftImage->image.clone(),
-				ptrRightImage->image.clone(),
-				fx,
-				baseline,
-				cx,
-				cy,
-				localTransform,
-				rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
-				uIsFinite(rotVariance) && rotVariance>0?rotVariance:1,
-				uIsFinite(transVariance) && transVariance>0?transVariance:1,
-				0,
-				rtabmap_ros::timestampFromROS(odomMsg->header.stamp));
-		recorder_.addData(data);
-	}
-
-	void stereoCallback(
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
-	{
-		// TF ready?
-		Transform localTransform;
-		try
-		{
-			if(waitForTransform_)
-			{
-				if(!tfListener_.waitForTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, ros::Duration(1)))
-				{
-					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), leftImageMsg->header.frame_id.c_str());
-					return;
-				}
-			}
-
-			tf::StampedTransform tmp;
-			tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
-			localTransform = rtabmap_ros::transformFromTF(tmp);
-		}
-		catch(tf::TransformException & ex)
-		{
-			ROS_WARN("%s",ex.what());
-			return;
-		}
-
-		cv_bridge::CvImageConstPtr ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8");
-		cv_bridge::CvImageConstPtr ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
-
-		image_geometry::StereoCameraModel model;
-		model.fromCameraInfo(*leftCameraInfoMsg, *rightCameraInfoMsg);
-		float fx = model.right().fx();
-		float baseline = model.baseline();
-		float cx = model.right().cx();
-		float cy = model.right().cy();
-
-		rtabmap::SensorData data(
-				ptrLeftImage->image.clone(),
-				ptrRightImage->image.clone(),
-				fx,
-				baseline,
-				cx,
-				cy,
-				localTransform,
-				Transform(),
-				1.0f,
-				1.0f,
-				0,
-				rtabmap_ros::timestampFromROS(leftImageMsg->header.stamp));
-		recorder_.addData(data);
-	}
-
-private:
-	DataRecorder recorder_;
-	std::string fileName_;
-	std::string frameId_;
-	std::string odomFrameId_;
-	bool waitForTransform_;
-
-	image_transport::Subscriber defaultSub_;
-	image_transport::SubscriberFilter imageSub_;
-	image_transport::SubscriberFilter imageDepthSub_;
-	image_transport::SubscriberFilter imageRightSub_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRightSub_;
-	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
-	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::LaserScan> MyDepthOdomScanSyncPolicy;
-	message_filters::Synchronizer<MyDepthOdomScanSyncPolicy> * depthOdomScanSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-				sensor_msgs::Image,
-				sensor_msgs::Image,
-				sensor_msgs::CameraInfo,
-				sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
-		message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthSyncPolicy;
-	message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
-
-	//without odom
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthImageSyncPolicy;
-	message_filters::Synchronizer<MyDepthImageSyncPolicy> * depthImageSync_;
-
-	//stereo with odometry
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo,
-			nav_msgs::Odometry> MyStereoOdomSyncPolicy;
-	message_filters::Synchronizer<MyStereoOdomSyncPolicy> * stereoOdomSync_;
-
-	//stereo without odometry
-	typedef message_filters::sync_policies::ExactTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoSyncPolicy;
-	message_filters::Synchronizer<MyStereoSyncPolicy> * stereoSync_;
-
-	tf::TransformListener tfListener_;
-};
-
-int main(int argc, char** argv)
-{
-	ULogger::setType(ULogger::kTypeConsole);
-	ULogger::setLevel(ULogger::kInfo);
-
-	ros::init(argc, argv, "data_recorder");
-
-	QApplication app(argc, argv);
-
-	DataRecorderWrapper recorder;
-
-	if(recorder.init())
-	{
-		ros::spin();
-	}
-	else
-	{
-		ROS_ERROR("Cannot initialize the recorder! Make sure the parameter output_file_name is set!");
-	}
-}