cleanup ros-pkg

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1269 f169173b-cf89-36c8-b27e-44dbe73f0c83

src/CameraNode.cpp

@@ -0,0 +1,285 @@
+/*
+ * CameraNode.cpp
+ *
+ *  Created on: 1 févr. 2010
+ *      Author: labm2414
+ */
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <cv_bridge/cv_bridge.h>
+#include <std_msgs/Empty.h>
+#include <image_transport/image_transport.h>
+#include <std_srvs/Empty.h>
+
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/CameraThread.h>
+#include <rtabmap/core/CameraEvent.h>
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UEventsHandler.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+
+#include <dynamic_reconfigure/server.h>
+#include <rtabmap/CameraConfig.h>
+
+class CameraWrapper : public UEventsHandler
+{
+public:
+	// Usb device like a Webcam
+	CameraWrapper(int usbDevice = 0,
+			      float imageRate = 0,
+				  unsigned int imageWidth = 0,
+				  unsigned int imageHeight = 0) :
+		camera_(0),
+		frameId_("camera")
+	{
+		ros::NodeHandle nh;
+		ros::NodeHandle pnh("~");
+		pnh.param("frame_id", frameId_, frameId_);
+		image_transport::ImageTransport it(nh);
+		rosPublisher_ = it.advertise("image", 1);
+		startSrv_ = nh.advertiseService("start_camera", &CameraWrapper::startSrv, this);
+		stopSrv_ = nh.advertiseService("stop_camera", &CameraWrapper::stopSrv, this);
+		UEventsManager::addHandler(this);
+	}
+
+	virtual ~CameraWrapper()
+	{
+		if(camera_)
+		{
+			camera_->join(true);
+			delete camera_;
+		}
+	}
+
+	bool init()
+	{
+		if(camera_)
+		{
+			return camera_->init();
+		}
+		return false;
+	}
+
+	void start()
+	{
+		if(camera_)
+		{
+			return camera_->start();
+		}
+	}
+
+	bool startSrv(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+	{
+		ROS_INFO("Camera started...");
+		if(camera_)
+		{
+			camera_->start();
+		}
+		return true;
+	}
+
+	bool stopSrv(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+	{
+		ROS_INFO("Camera stopped...");
+		if(camera_)
+		{
+			camera_->kill();
+		}
+		return true;
+	}
+
+	void setParameters(int deviceId, double frameRate, int width, int height, const std::string & path, bool autoRestart, bool pause)
+	{
+		ROS_INFO("Parameters changed: deviceId=%d, path=%s frameRate=%f w/h=%d/%d autoRestart=%s pause=%s",
+				deviceId, path.c_str(), frameRate, width, height, autoRestart?"true":"false", pause?"true":"false");
+		if(camera_)
+		{
+			rtabmap::CameraVideo * videoCam = dynamic_cast<rtabmap::CameraVideo *>(camera_->getCamera());
+			rtabmap::CameraImages * imagesCam = dynamic_cast<rtabmap::CameraImages *>(camera_->getCamera());
+
+			if(imagesCam)
+			{
+				// images
+				if(!path.empty() && UDirectory::getDir(path+"/").compare(UDirectory::getDir(imagesCam->getPath())) == 0)
+				{
+					imagesCam->setImageRate(frameRate);
+					imagesCam->setImageSize(width, height);
+					camera_->setAutoRestart(autoRestart);
+					if(pause && !camera_->isPaused())
+					{
+						camera_->join(true);
+					}
+					else if(!pause && camera_->isPaused())
+					{
+						camera_->start();
+					}
+				}
+				else
+				{
+					delete camera_;
+					camera_ = 0;
+				}
+			}
+			else if(videoCam)
+			{
+				if(!path.empty() && path.compare(videoCam->getFilePath()) == 0)
+				{
+					// video
+					videoCam->setImageRate(frameRate);
+					videoCam->setImageSize(width, height);
+					camera_->setAutoRestart(autoRestart);
+					if(pause && !camera_->isPaused())
+					{
+						camera_->join(true);
+					}
+					else if(!pause && camera_->isPaused())
+					{
+						camera_->start();
+					}
+				}
+				else if(path.empty() &&
+						videoCam->getFilePath().empty() &&
+						videoCam->getUsbDevice() == deviceId)
+				{
+					// usb device
+					unsigned int w;
+					unsigned int h;
+					videoCam->getImageSize(w, h);
+					if((int)w == width && (int)h == height)
+					{
+						videoCam->setImageRate(frameRate);
+						camera_->setAutoRestart(autoRestart);
+						if(pause && !camera_->isPaused())
+						{
+							camera_->join(true);
+						}
+						else if(!pause && camera_->isPaused())
+						{
+							camera_->start();
+						}
+					}
+					else
+					{
+						delete camera_;
+						camera_ = 0;
+					}
+				}
+				else
+				{
+					delete camera_;
+					camera_ = 0;
+				}
+			}
+			else
+			{
+				ROS_ERROR("Wrong camera type ?!?");
+				delete camera_;
+				camera_ = 0;
+			}
+		}
+
+		if(!camera_)
+		{
+			if(!path.empty() && UDirectory::exists(path))
+			{
+				//images
+				camera_ = new rtabmap::CameraThread(new rtabmap::CameraImages(path, 1, false, frameRate, width, height), autoRestart);
+			}
+			else if(!path.empty() && UFile::exists(path))
+			{
+				//video
+				camera_ = new rtabmap::CameraThread(new rtabmap::CameraVideo(path, frameRate, width, height), autoRestart);
+			}
+			else
+			{
+				if(!path.empty() && !UDirectory::exists(path) && !UFile::exists(path))
+				{
+					ROS_ERROR("Path \"%s\" does not exist (or you don't have the permissions to read)... falling back to usb device...", path.c_str());
+				}
+				//usb device
+				camera_ = new rtabmap::CameraThread(new rtabmap::CameraVideo(deviceId, frameRate, width, height), autoRestart);
+			}
+			init();
+			if(!pause)
+			{
+				start();
+			}
+		}
+	}
+
+protected:
+	virtual void handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("CameraEvent") == 0)
+		{
+			rtabmap::CameraEvent * e = (rtabmap::CameraEvent*)event;
+			const cv::Mat & image = e->image().image();
+			if(!image.empty() && image.depth() == CV_8U)
+			{
+				cv_bridge::CvImage img;
+				if(image.channels() == 1)
+				{
+					img.encoding = sensor_msgs::image_encodings::MONO8;
+				}
+				else
+				{
+					img.encoding = sensor_msgs::image_encodings::BGR8;
+				}
+				img.image = image;
+				sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
+				rosMsg->header.frame_id = frameId_;
+				rosMsg->header.stamp = ros::Time::now();
+				rosPublisher_.publish(rosMsg);
+			}
+		}
+	}
+
+private:
+	image_transport::Publisher rosPublisher_;
+	rtabmap::CameraThread * camera_;
+	ros::ServiceServer startSrv_;
+	ros::ServiceServer stopSrv_;
+	std::string frameId_;
+};
+
+CameraWrapper * camera = 0;
+void callback(rtabmap::CameraConfig &config, uint32_t level)
+{
+	if(camera)
+	{
+		camera->setParameters(config.device_id, config.frame_rate, config.width, config.height, config.video_or_images_path, config.auto_restart, config.pause);
+	}
+}
+
+int main(int argc, char** argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	//ULogger::setLevel(ULogger::kDebug);
+	ULogger::setEventLevel(ULogger::kWarning);
+
+	ros::init(argc, argv, "camera");
+
+	ros::NodeHandle nh("~");
+
+	camera = new CameraWrapper(); // webcam device 0
+
+	dynamic_reconfigure::Server<rtabmap::CameraConfig> server;
+	dynamic_reconfigure::Server<rtabmap::CameraConfig>::CallbackType f;
+	f = boost::bind(&callback, _1, _2);
+	server.setCallback(f);
+
+	ros::spin();
+
+	//cleanup
+	if(camera)
+	{
+		delete camera;
+	}
+
+	return 0;
+}

src/CoreNode.cpp

@@ -0,0 +1,88 @@
+/*
+ * CoreNode.cpp
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "CoreWrapper.h"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UStl.h>
+
+int main(int argc, char** argv)
+{
+	ROS_INFO("Starting node...");
+
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	ros::init(argc, argv, "rtabmap");
+
+	bool deleteDbOnStart = false;
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--delete_db_on_start") == 0)
+		{
+			deleteDbOnStart = true;
+		}
+		else if(strcmp(argv[i], "--udebug") == 0)
+		{
+			ULogger::setLevel(ULogger::kDebug);
+		}
+		else if(strcmp(argv[i], "--params") == 0 || strcmp(argv[i], "--params-all") == 0)
+		{
+			rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+			uInsert(parameters,
+					std::make_pair(rtabmap::Parameters::kRtabmapWorkingDirectory(),
+					UDirectory::homeDir()+"/.ros")); // change default to ~/.ros
+			uInsert(parameters,
+					std::make_pair(rtabmap::Parameters::kRtabmapDatabasePath(),
+					UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName())); // change default to ~/.ros
+
+			if(strcmp(argv[i], "--params") == 0)
+			{
+				// hide specific parameters
+				for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
+				{
+					if(uSplit(iter->first, '/').front().compare("Bayes") == 0 ||
+					   uSplit(iter->first, '/').front().compare("VhEp") == 0 ||
+					   uSplit(iter->first, '/').front().compare("Odom") == 0 ||
+					   uSplit(iter->first, '/').front().compare("OdomBin") == 0 ||
+					   uSplit(iter->first, '/').front().compare("OdomICP") == 0)
+					{
+						parameters.erase(iter++);
+					}
+					else
+					{
+						++iter;
+					}
+				}
+			}
+
+			for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.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 RTAB-Map parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
+	}
+
+	CoreWrapper * rtabmap = new CoreWrapper(deleteDbOnStart);
+
+	ROS_INFO("rtabmap started...");
+	ros::spin();
+
+	delete rtabmap;
+
+	return 0;
+}

src/CoreWrapper.h

@@ -0,0 +1,155 @@
+/*
+ * CoreWrapper.h
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#ifndef COREWRAPPER_H_
+#define COREWRAPPER_H_
+
+
+#include <ros/ros.h>
+
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+
+#include <std_srvs/Empty.h>
+
+#include <cv_bridge/cv_bridge.h>
+
+#include <std_msgs/Empty.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/LaserScan.h>
+#include <nav_msgs/Odometry.h>
+
+#include <rtabmap/core/Statistics.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Rtabmap.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+class CoreWrapper
+{
+public:
+	CoreWrapper(bool deleteDbOnStart = false);
+	virtual ~CoreWrapper();
+
+private:
+	void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, int queueSize);
+	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
+	void depthCallback(const sensor_msgs::ImageConstPtr& imageMsg,
+					   const nav_msgs::OdometryConstPtr & odomMsg,
+					   const sensor_msgs::ImageConstPtr& imageDepthMsg,
+					   const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
+	void scanCallback(const sensor_msgs::ImageConstPtr& imageMsg,
+					  const nav_msgs::OdometryConstPtr & odomMsg,
+					  const sensor_msgs::LaserScanConstPtr& scanMsg);
+	void depthScanCallback(const sensor_msgs::ImageConstPtr& imageMsg,
+						   const nav_msgs::OdometryConstPtr & odomMsg,
+						   const sensor_msgs::ImageConstPtr& imageDepthMsg,
+						   const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
+						   const sensor_msgs::LaserScanConstPtr& scanMsg);
+
+	void process(
+			int id,
+			const cv::Mat & image,
+			const rtabmap::Transform & odom = rtabmap::Transform(),
+			const std::string & odomFrameId = "",
+			const cv::Mat & depth = cv::Mat(),
+			float depthConstant = 0.0f,
+			const rtabmap::Transform & localTransform = rtabmap::Transform(),
+			const cv::Mat & scan = cv::Mat());
+
+	bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool publishGlobalMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool publishLocalMapDataCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool publishGlobalGraphCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool publishLocalGraphCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+
+	void publishMapData(bool global);
+	void publishGraph(bool global);
+
+	rtabmap::ParametersMap loadParameters(const std::string & configFile);
+	void saveParameters(const std::string & configFile);
+
+	void publishLoop(double tfDelay);
+
+	void publishStats(const rtabmap::Statistics & stats);
+
+private:
+	rtabmap::Rtabmap rtabmap_;
+	bool paused_;
+
+	std::string frameId_;
+	std::string mapFrameId_;
+	std::string odomFrameId_;
+	std::string configPath_;
+
+	tf::Transform mapToOdom_;
+	boost::mutex mapToOdomMutex_;
+
+	ros::Publisher infoPub_;
+	ros::Publisher infoPubEx_;
+	ros::Publisher mapData_;
+
+	image_transport::Subscriber defaultSub_;
+	image_transport::SubscriberFilter imageSub_;
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+	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> 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_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::LaserScan> MyScanSyncPolicy;
+	message_filters::Synchronizer<MyScanSyncPolicy> * scanSync_;
+
+	tf::TransformBroadcaster tfBroadcaster_;
+	tf::TransformListener tfListener_;
+
+	ros::ServiceServer updateSrv_;
+	ros::ServiceServer resetSrv_;
+	ros::ServiceServer pauseSrv_;
+	ros::ServiceServer resumeSrv_;
+	ros::ServiceServer triggerNewMapSrv_;
+	ros::ServiceServer publishGlobalMapDataSrv_;
+	ros::ServiceServer publishLocalMapDataSrv_;
+	ros::ServiceServer publishGlobalGraphSrv_;
+	ros::ServiceServer publishLocalGraphSrv_;
+
+	boost::thread* transformThread_;
+
+	float rate_;
+	ros::Time time_;
+};
+
+#endif /* COREWRAPPER_H_ */

src/DataRecorderNode.cpp

@@ -0,0 +1,456 @@
+
+#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 <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include "rtabmap/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;
+
+
+class DataRecorderWrapper
+{
+public:
+	DataRecorderWrapper() :
+		fileName_("output.db"),
+		frameId_("base_link"),
+		depthScanSync_(0),
+		depthSync_(0),
+		scanSync_(0),
+		depthImageSync_(0)
+	{
+		ros::NodeHandle pnh("~");
+
+		bool subscribeOdometry = false;
+		bool subscribeLaserScan = false;
+		bool subscribeDepth = false;
+		int queueSize = 10;
+		pnh.param("subscribe_odometry", subscribeOdometry, subscribeOdometry);
+		pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
+		pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("output_file_name", fileName_, fileName_);
+		pnh.param("frame_id", frameId_, frameId_);
+
+		setupCallbacks(subscribeOdometry, subscribeDepth, subscribeLaserScan, queueSize);
+	}
+	bool init()
+	{
+		return recorder_.init(fileName_.c_str());
+	}
+
+	virtual ~DataRecorderWrapper()
+	{
+		if(depthScanSync_)
+			delete depthScanSync_;
+		if(depthSync_)
+			delete depthSync_;
+		if(scanSync_)
+			delete scanSync_;
+		if(depthImageSync_)
+			delete depthImageSync_;
+	}
+
+private:
+	void setupCallbacks(
+			bool subscribeOdom,
+			bool subscribeDepth,
+			bool subscribeLaserScan,
+			int queueSize)
+	{
+		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);
+			odomSub_.subscribe(nh, "odom", 1);
+			scanSub_.subscribe(nh, "scan", 1);
+			depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+			depthScanSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
+		}
+		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_->registerCallback(boost::bind(&DataRecorderWrapper::depthCallback, this, _1, _2, _3, _4));
+		}
+		else if(subscribeOdom && !subscribeDepth && subscribeLaserScan)
+		{
+			ROS_INFO("Registering LaserScan callback...");
+			imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+			odomSub_.subscribe(nh, "odom", 1);
+			scanSub_.subscribe(nh, "scan", 1);
+			scanSync_ = new message_filters::Synchronizer<MyScanSyncPolicy>(MyScanSyncPolicy(queueSize), imageSub_, odomSub_, scanSub_);
+			scanSync_->registerCallback(boost::bind(&DataRecorderWrapper::scanCallback, this, _1, _2, _3));
+		}
+		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_->registerCallback(boost::bind(&DataRecorderWrapper::depthImageCallback, this, _1, _2, _3));
+		}
+		else
+		{
+			ROS_INFO("Registering default callback...");
+			defaultSub_ = rgb_it.subscribe("image", 1, &DataRecorderWrapper::defaultCallback, this);
+		}
+	}
+
+	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
+	{
+		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+		rtabmap::Image image(
+			ptrImage->image.clone(),
+			cv::Mat(),
+			cv::Mat(),
+			0.0f,
+			Transform(),
+			Transform());
+		recorder_.addData(image);
+	}
+
+	void depthImageCallback(
+			const sensor_msgs::ImageConstPtr& imageMsg,
+			const sensor_msgs::ImageConstPtr& depthMsg,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+	{
+		// TF ready?
+		Transform localTransform;
+		try
+		{
+			tf::StampedTransform tmp;
+			tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
+			localTransform = transformFromTF(tmp);
+		}
+		catch(tf::TransformException & ex)
+		{
+			ROS_WARN("%s",ex.what());
+			return;
+		}
+
+		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
+
+		float depthConstant = 1.0f/cameraInfoMsg->K[4];
+
+		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;
+		}
+
+		rtabmap::Image image(
+			ptrImage->image.clone(),
+			depth16,
+			cv::Mat(),
+			depthConstant,
+			Transform(),
+			localTransform);
+		recorder_.addData(image);
+	}
+
+	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
+		{
+			tf::StampedTransform tmp;
+			tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
+			localTransform = transformFromTF(tmp);
+		}
+		catch(tf::TransformException & ex)
+		{
+			ROS_WARN("%s",ex.what());
+			return;
+		}
+
+		Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
+
+		float depthConstant = 1.0f/cameraInfoMsg->K[4];
+
+		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;
+		}
+
+		rtabmap::Image image(
+			ptrImage->image.clone(),
+			depth16,
+			cv::Mat(),
+			depthConstant,
+			odom,
+			localTransform);
+		recorder_.addData(image);
+	}
+
+	void scanCallback(
+			const sensor_msgs::ImageConstPtr& imageMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::LaserScanConstPtr& scanMsg)
+	{
+		// TF ready?
+		try
+		{
+			tf::StampedTransform tmp;
+			tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, 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::depth2DFromPointCloud(pclScan);
+
+		Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+
+		rtabmap::Image image(
+			ptrImage->image.clone(),
+			cv::Mat(),
+			scan,
+			0.0f,
+			odom,
+			Transform());
+		recorder_.addData(image);
+
+	}
+
+	void depthScanCallback(
+			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
+		{
+			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 = 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::depth2DFromPointCloud(pclScan);
+
+		Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
+
+		float depthConstant = 1.0f/cameraInfoMsg->K[4];
+
+		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;
+		}
+
+		rtabmap::Image image(
+			ptrImage->image.clone(),
+			depth16,
+			scan,
+			depthConstant,
+			odom,
+			localTransform);
+		recorder_.addData(image);
+	}
+
+private:
+	DataRecorder recorder_;
+	std::string fileName_;
+	std::string frameId_;
+
+	image_transport::Subscriber defaultSub_;
+	image_transport::SubscriberFilter imageSub_;
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+	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> 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_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::LaserScan> MyScanSyncPolicy;
+	message_filters::Synchronizer<MyScanSyncPolicy> * scanSync_;
+
+	//without odom
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthImageSyncPolicy;
+	message_filters::Synchronizer<MyDepthImageSyncPolicy> * depthImageSync_;
+
+	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!");
+	}
+}

src/GridMapAssemblerNode.cpp

@@ -0,0 +1,169 @@
+/*
+ * GuiWrapper.cpp
+ *
+ *  Created on: 4 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "GuiWrapper.h"
+#include <QtGui/QApplication>
+#include <QtCore/QDir>
+
+#include <cv_bridge/cv_bridge.h>
+#include <std_srvs/Empty.h>
+#include <std_msgs/Empty.h>
+#include <nav_msgs/GetMap.h>
+
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+
+#include <opencv2/highgui/highgui.hpp>
+
+#include <rtabmap/gui/MainWindow.h>
+#include <rtabmap/core/RtabmapEvent.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/ParamEvent.h>
+#include <rtabmap/core/OdometryEvent.h>
+#include <rtabmap/core/util3d.h>
+
+#include "rtabmap/MsgConversion.h"
+
+#include "PreferencesDialogROS.h"
+
+#include <pcl_ros/transforms.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/common/common.h>
+#include <laser_geometry/laser_geometry.h>
+
+using namespace rtabmap;
+
+class GridMapAssembler
+{
+
+public:
+	GridMapAssembler() :
+		scanVoxelSize_(0.01)
+	{
+		ros::NodeHandle pnh("~");
+		pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
+
+		ros::NodeHandle nh;
+		infoExTopic_ = nh.subscribe("infoEx", 1, &GridMapAssembler::infoExReceivedCallback, this);
+		mapDataTopic_ = nh.subscribe("mapData", 1, &GridMapAssembler::mapDataReceivedCallback, this);
+
+		gridMap_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1);
+		getMapService_ = nh.advertiseService("get_map", &GridMapAssembler::getMapCallback, this);
+	}
+
+	~GridMapAssembler()
+	{
+	}
+
+	void infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
+	{
+		for(unsigned int i=0; i<msg->data.depth2DIDs.size() && i<msg->data.depth2Ds.size(); ++i)
+		{
+			if(!uContains(scans_, msg->data.depth2DIDs[i]))
+			{
+				cv::Mat depth2d = util3d::uncompressData(msg->data.depth2Ds[i].bytes);
+				scans_.insert(std::make_pair(msg->data.depth2DIDs[i], util3d::depth2DToPointCloud(depth2d)));
+			}
+		}
+
+		std::map<int, Transform> poses;
+		for(unsigned int i=0; i<msg->data.poseIDs.size() && i<msg->data.poses.size(); ++i)
+		{
+			poses.insert(std::make_pair(msg->data.poseIDs[i], transformFromPoseMsg(msg->data.poses[i])));
+		}
+
+		if(gridMap_.getNumSubscribers())
+		{
+			float delta = 0.05; //m
+
+			// create the map
+			float xMin=0.0f, yMin=0.0f;
+			cv::Mat pixels = util3d::create2DMap(poses, scans_, delta, xMin, yMin);
+
+			if(!pixels.empty())
+			{
+				//init
+				map_.info.resolution = delta;
+				map_.info.origin.position.x = 0.0;
+				map_.info.origin.position.y = 0.0;
+				map_.info.origin.position.z = 0.0;
+				map_.info.origin.orientation.x = 0.0;
+				map_.info.origin.orientation.y = 0.0;
+				map_.info.origin.orientation.z = 0.0;
+				map_.info.origin.orientation.w = 1.0;
+
+				map_.info.width = pixels.cols;
+				map_.info.height = pixels.rows;
+				map_.info.origin.position.x = xMin;
+				map_.info.origin.position.y = yMin;
+				map_.data.resize(map_.info.width * map_.info.height);
+
+				memcpy(map_.data.data(), pixels.data, map_.info.width * map_.info.height);
+
+				map_.header.frame_id = msg->header.frame_id;
+				map_.header.stamp = ros::Time::now();
+
+				gridMap_.publish(map_);
+			}
+		}
+	}
+
+	void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
+	{
+		std::map<int, std::vector<unsigned char> > depths2d;
+
+		if(msg->depth2DIDs.size() != msg->depth2Ds.size())
+		{
+			ROS_WARN("grid_map_assembler: receiving map... depths2D and depth2DIDs are not the same size (%d vs %d)!",
+					(int)msg->depth2Ds.size(), (int)msg->depth2DIDs.size());
+		}
+
+		// fill maps
+		for(unsigned int i=0; i<msg->depth2DIDs.size() && i < msg->depth2Ds.size(); ++i)
+		{
+			if(!uContains(scans_, msg->depth2DIDs[i]))
+			{
+				cv::Mat depth2d = util3d::uncompressData(msg->depth2Ds[i].bytes);
+				scans_.insert(std::make_pair(msg->depth2DIDs[i], util3d::depth2DToPointCloud(depth2d)));
+			}
+		}
+	}
+
+	bool getMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res)
+	{
+		if(map_.data.size())
+		{
+			res.map = map_;
+			return true;
+		}
+		return false;
+	}
+
+private:
+	double scanVoxelSize_;
+
+	ros::Subscriber infoExTopic_;
+	ros::Subscriber mapDataTopic_;
+
+	ros::Publisher gridMap_;
+
+	ros::ServiceServer getMapService_;
+
+	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
+
+	nav_msgs::OccupancyGrid map_;
+};
+
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "grid_map_assembler");
+	GridMapAssembler assembler;
+	ros::spin();
+	return 0;
+}

src/GuiNode.cpp

@@ -0,0 +1,51 @@
+/*
+ * CoreNode.cpp
+ *
+ *  Created on: 2 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "GuiWrapper.h"
+#include "rtabmap/utilite/ULogger.h"
+
+#include <QApplication>
+#include <rtabmap/gui/MainWindow.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <signal.h>
+
+void my_handler(int s){
+	QApplication::closeAllWindows();
+}
+
+int main(int argc, char** argv)
+{
+	ROS_INFO("Starting node...");
+
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	ros::init(argc, argv, "rtabmapviz");
+
+	GuiWrapper gui(argc, argv);
+
+	// Catch ctrl-c to close the gui
+	// (Place this after QApplication's constructor)
+	struct sigaction sigIntHandler;
+	sigIntHandler.sa_handler = my_handler;
+	sigemptyset(&sigIntHandler.sa_mask);
+	sigIntHandler.sa_flags = 0;
+	sigaction(SIGINT, &sigIntHandler, NULL);
+
+	// Here start the ROS events loop
+	ros::AsyncSpinner spinner(4); // Use 4 threads
+	spinner.start();
+
+	ROS_INFO("rtabmapviz started.");
+	// Now wait for application to finish
+	int r = gui.exec();// MUST be called by the Main Thread
+
+	spinner.stop();
+
+	ROS_INFO("rtabmapviz: All done! Closing...");
+	return r;
+}

src/GuiWrapper.cpp

@@ -0,0 +1,683 @@
+/*
+ * GuiWrapper.cpp
+ *
+ *  Created on: 4 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "GuiWrapper.h"
+#include <QtGui/QApplication>
+#include <QtCore/QDir>
+
+#include <cv_bridge/cv_bridge.h>
+#include <std_srvs/Empty.h>
+#include <std_msgs/Empty.h>
+
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UConversion.h>
+
+#include <opencv2/highgui/highgui.hpp>
+
+#include <rtabmap/gui/MainWindow.h>
+#include <rtabmap/core/RtabmapEvent.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/ParamEvent.h>
+#include <rtabmap/core/OdometryEvent.h>
+#include <rtabmap/core/util3d.h>
+
+#include "rtabmap/MsgConversion.h"
+
+#include "PreferencesDialogROS.h"
+
+#include <pcl_ros/transforms.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <laser_geometry/laser_geometry.h>
+
+using namespace rtabmap;
+
+GuiWrapper::GuiWrapper(int & argc, char** argv) :
+		app_(0),
+		mainWindow_(0),
+		frameId_("base_link"),
+		cameraNodeName_("")
+{
+	ros::NodeHandle nh;
+	app_ = new QApplication(argc, argv);
+
+	QString configFile = QDir::homePath()+"/.ros/rtabmapGUI.ini";
+	for(int i=1; i<argc; ++i)
+	{
+		if(strcmp(argv[i], "-d") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				configFile = argv[i];
+			}
+			break;
+		}
+	}
+
+	configFile.replace('~', QDir::homePath());
+
+	ROS_INFO("rtabmapviz: Using configuration from \"%s\"", configFile.toStdString().c_str());
+	uSleep(500);
+	mainWindow_ = new MainWindow(new PreferencesDialogROS(configFile));
+	mainWindow_->setWindowTitle(mainWindow_->windowTitle()+" [ROS]");
+	mainWindow_->show();
+	bool paused = false;
+	nh.param("is_rtabmap_paused", paused, paused);
+	mainWindow_->setMonitoringState(paused);
+	app_->connect( app_, SIGNAL( lastWindowClosed() ), app_, SLOT( quit() ) );
+
+	ros::NodeHandle pnh("~");
+
+	// To receive odometry events
+	bool subscribeLaserScan = false;
+	bool subscribeDepth = false;
+	int queueSize = 10;
+	pnh.param("frame_id", frameId_, frameId_);
+	pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
+	pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
+	pnh.param("queue_size", queueSize, queueSize);
+	pnh.param("camera_node_name", cameraNodeName_, cameraNodeName_); // used to pause the rtabmap/camera when pausing the process
+	this->setupCallbacks(subscribeDepth, subscribeLaserScan, queueSize);
+
+	UEventsManager::addHandler(this);
+	UEventsManager::addHandler(mainWindow_);
+
+	infoExTopic_ = nh.subscribe("infoEx", 1, &GuiWrapper::infoExReceivedCallback, this);
+	mapDataTopic_ = nh.subscribe("mapData", 1, &GuiWrapper::mapDataReceivedCallback, this);
+}
+
+GuiWrapper::~GuiWrapper()
+{
+	delete mainWindow_;
+	delete app_;
+}
+
+int GuiWrapper::exec()
+{
+	return app_->exec();
+}
+
+void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
+{
+	//ROS_INFO("rtabmapviz: RTAB-Map info ex received!");
+
+	// Map from ROS struct to rtabmap struct
+	rtabmap::Statistics stat;
+
+	stat.setExtended(true); // Extended
+
+	stat.setRefImageId(msg->refId);
+	stat.setLoopClosureId(msg->loopClosureId);
+	stat.setLocalLoopClosureId(msg->localLoopClosureId);
+
+	//Posterior, likelihood, childCount
+	std::map<int, float> mapIntFloat;
+	for(unsigned int i=0; i<msg->posteriorKeys.size() && i<msg->posteriorValues.size(); ++i)
+	{
+		mapIntFloat.insert(std::pair<int, float>(msg->posteriorKeys.at(i), msg->posteriorValues.at(i)));
+	}
+	stat.setPosterior(mapIntFloat);
+	mapIntFloat.clear();
+	for(unsigned int i=0; i<msg->likelihoodKeys.size() && i<msg->likelihoodValues.size(); ++i)
+	{
+		mapIntFloat.insert(std::pair<int, float>(msg->likelihoodKeys.at(i), msg->likelihoodValues.at(i)));
+	}
+	stat.setLikelihood(mapIntFloat);
+	mapIntFloat.clear();
+	for(unsigned int i=0; i<msg->rawLikelihoodKeys.size() && i<msg->rawLikelihoodValues.size(); ++i)
+	{
+		mapIntFloat.insert(std::pair<int, float>(msg->rawLikelihoodKeys.at(i), msg->rawLikelihoodValues.at(i)));
+	}
+	stat.setRawLikelihood(mapIntFloat);
+	std::map<int, int> mapIntInt;
+	for(unsigned int i=0; i<msg->weightsKeys.size() && i<msg->weightsValues.size(); ++i)
+	{
+		mapIntInt.insert(std::pair<int, int>(msg->weightsKeys.at(i), msg->weightsValues.at(i)));
+	}
+	stat.setWeights(mapIntInt);
+
+	//SURF stuff...
+	std::multimap<int, cv::KeyPoint> mapIntKeypoint;
+	for(unsigned int i=0; i<msg->refWordsKeys.size() && i<msg->refWordsValues.size(); ++i)
+	{
+		cv::KeyPoint pt;
+		pt.angle = msg->refWordsValues.at(i).angle;
+		pt.response = msg->refWordsValues.at(i).response;
+		pt.pt.x = msg->refWordsValues.at(i).ptx;
+		pt.pt.y = msg->refWordsValues.at(i).pty;
+		pt.size = msg->refWordsValues.at(i).size;
+		mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->refWordsKeys.at(i), pt));
+	}
+	stat.setRefWords(mapIntKeypoint);
+	mapIntKeypoint.clear();
+	for(unsigned int i=0; i<msg->loopWordsKeys.size() && i<msg->loopWordsValues.size(); ++i)
+	{
+		cv::KeyPoint pt;
+		pt.angle = msg->loopWordsValues.at(i).angle;
+		pt.response = msg->loopWordsValues.at(i).response;
+		pt.pt.x = msg->loopWordsValues.at(i).ptx;
+		pt.pt.y = msg->loopWordsValues.at(i).pty;
+		pt.size = msg->loopWordsValues.at(i).size;
+		mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->loopWordsKeys.at(i), pt));
+	}
+	stat.setLoopWords(mapIntKeypoint);
+
+	// Statistics data
+	for(unsigned int i=0; i<msg->statsKeys.size() && i<msg->statsValues.size(); i++)
+	{
+		stat.addStatistic(msg->statsKeys.at(i), msg->statsValues.at(i));
+	}
+
+	//RGB-D SLAM data
+	stat.setMapCorrection(transformFromGeometryMsg(msg->mapCorrection));
+	stat.setLoopClosureTransform(transformFromGeometryMsg(msg->loopClosureTransform));
+	stat.setCurrentPose(transformFromPoseMsg(msg->currentPose));
+
+	std::map<int, std::vector<unsigned char> > images;
+	for(unsigned int i=0; i<msg->data.imageIDs.size() && i<msg->data.images.size(); ++i)
+	{
+		images.insert(std::make_pair(msg->data.imageIDs[i], msg->data.images[i].bytes));
+	}
+	stat.setImages(images);
+
+	std::map<int, std::vector<unsigned char> > depths;
+	for(unsigned int i=0; i<msg->data.depthIDs.size() && i<msg->data.depths.size(); ++i)
+	{
+		depths.insert(std::make_pair(msg->data.depthIDs[i], msg->data.depths[i].bytes));
+	}
+	stat.setDepths(depths);
+
+	std::map<int, std::vector<unsigned char> > depth2ds;
+	for(unsigned int i=0; i<msg->data.depth2DIDs.size() && i<msg->data.depth2Ds.size(); ++i)
+	{
+		depth2ds.insert(std::make_pair(msg->data.depth2DIDs[i], msg->data.depth2Ds[i].bytes));
+	}
+	stat.setDepth2ds(depth2ds);
+
+	std::map<int, float> depthConstants;
+	for(unsigned int i=0; i<msg->data.depthConstantIDs.size() && i<msg->data.depthConstants.size(); ++i)
+	{
+		depthConstants.insert(std::make_pair(msg->data.depthConstantIDs[i], msg->data.depthConstants[i]));
+	}
+	stat.setDepthConstants(depthConstants);
+
+	std::map<int, Transform> localTransforms;
+	for(unsigned int i=0; i<msg->data.localTransformIDs.size() && i<msg->data.localTransforms.size(); ++i)
+	{
+		localTransforms.insert(std::make_pair(msg->data.localTransformIDs[i], transformFromGeometryMsg(msg->data.localTransforms[i])));
+	}
+	stat.setLocalTransforms(localTransforms);
+
+	std::map<int, Transform> poses;
+	for(unsigned int i=0; i<msg->data.poseIDs.size() && i<msg->data.poses.size(); ++i)
+	{
+		poses.insert(std::make_pair(msg->data.poseIDs[i], transformFromPoseMsg(msg->data.poses[i])));
+	}
+	stat.setPoses(poses);
+
+	std::multimap<int, Link> constraints;
+	for(unsigned int i=0; i<msg->data.constraintFromIDs.size() && i<msg->data.constraintToIDs.size() && i<msg->data.constraintTypes.size() && i < msg->data.constraints.size(); ++i)
+	{
+		Transform t = transformFromGeometryMsg(msg->data.constraints[i]);
+		constraints.insert(std::make_pair(msg->data.constraintFromIDs[i], Link(msg->data.constraintFromIDs[i], msg->data.constraintToIDs[i], t, (Link::Type)msg->data.constraintTypes[i])));
+	}
+	stat.setConstraints(constraints);
+
+	std::map<int, int> mapIds;
+	for(unsigned int i=0; i<msg->data.mapIDs.size() && i<msg->data.maps.size(); ++i)
+	{
+		mapIds.insert(std::make_pair(msg->data.mapIDs[i], msg->data.maps[i]));
+	}
+	stat.setMapIds(mapIds);
+
+	this->post(new RtabmapEvent(stat));
+}
+
+void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
+{
+	std::map<int, std::vector<unsigned char> > images;
+	std::map<int, std::vector<unsigned char> > depths;
+	std::map<int, std::vector<unsigned char> > depths2d;
+	std::map<int, float> depthConstants;
+	std::map<int, Transform> localTransforms;
+	std::map<int, Transform> poses;
+	std::multimap<int, Link> constraints;
+
+	if(msg->imageIDs.size() != msg->images.size())
+	{
+		ROS_WARN("rtabmapviz: receiving map... images and IDs are not the same size (%d vs %d)!",
+				(int)msg->images.size(), (int)msg->imageIDs.size());
+	}
+
+	if(msg->depthIDs.size() != msg->depths.size())
+	{
+		ROS_WARN("rtabmapviz: receiving map... depths and IDs are not the same size (%d vs %d)!",
+				(int)msg->depths.size(), (int)msg->depthIDs.size());
+	}
+
+	if(msg->depth2DIDs.size() != msg->depth2Ds.size())
+	{
+		ROS_WARN("rtabmapviz: receiving map... depths2D and IDs are not the same size (%d vs %d)!",
+				(int)msg->depth2Ds.size(), (int)msg->depth2DIDs.size());
+	}
+
+	if(msg->depthConstantIDs.size() != msg->depthConstants.size())
+	{
+		ROS_WARN("rtabmapviz: receiving map... depthConstants and IDs are not the same size (%d vs %d)!",
+				(int)msg->depthConstants.size(), (int)msg->depthConstantIDs.size());
+	}
+
+	if(msg->poseIDs.size() != msg->poses.size())
+	{
+		ROS_WARN("rtabmapviz: receiving map... poses and IDs are not the same size (%d vs %d)!",
+				(int)msg->poses.size(), (int)msg->poseIDs.size());
+	}
+
+	if(msg->constraintFromIDs.size() != msg->constraints.size() ||
+	   msg->constraintToIDs.size() != msg->constraints.size() ||
+	   msg->constraintTypes.size() != msg->constraints.size())
+	{
+		ROS_WARN("rtabmapviz: receiving map... constraints and IDs are not the same size (%d vs %d vs %d vs %d)!",
+				(int)msg->constraints.size(), (int)msg->constraintFromIDs.size(), (int)msg->constraintToIDs.size(), (int)msg->constraintTypes.size());
+	}
+
+	for(unsigned int i=0; i<msg->imageIDs.size() && i < msg->images.size(); ++i)
+	{
+		images.insert(std::make_pair(msg->imageIDs[i], msg->images[i].bytes));
+	}
+
+	for(unsigned int i=0; i<msg->depthIDs.size() && i < msg->depths.size(); ++i)
+	{
+		depths.insert(std::make_pair(msg->depthIDs[i], msg->depths[i].bytes));
+	}
+
+	for(unsigned int i=0; i<msg->depth2DIDs.size() && i < msg->depth2Ds.size(); ++i)
+	{
+		depths2d.insert(std::make_pair(msg->depth2DIDs[i], msg->depth2Ds[i].bytes));
+	}
+
+	for(unsigned int i=0; i<msg->depthConstantIDs.size() && i < msg->depthConstants.size(); ++i)
+	{
+		depthConstants.insert(std::make_pair(msg->depthConstantIDs[i], msg->depthConstants[i]));
+	}
+
+	for(unsigned int i=0; i<msg->localTransformIDs.size() && i < msg->localTransforms.size(); ++i)
+	{
+		Transform t = transformFromGeometryMsg(msg->localTransforms[i]);
+		localTransforms.insert(std::make_pair(msg->localTransformIDs[i], t));
+	}
+
+	for(unsigned int i=0; i<msg->poseIDs.size() && i < msg->poses.size(); ++i)
+	{
+		Transform t = transformFromPoseMsg(msg->poses[i]);
+		poses.insert(std::make_pair(msg->poseIDs[i], t));
+	}
+
+	for(unsigned int i=0; i<msg->constraintFromIDs.size() && i<msg->constraintToIDs.size() && i<msg->constraintTypes.size() && i < msg->constraints.size(); ++i)
+	{
+		Transform t = transformFromGeometryMsg(msg->constraints[i]);
+		constraints.insert(std::make_pair(msg->constraintFromIDs[i], Link(msg->constraintFromIDs[i], msg->constraintToIDs[i], t, (Link::Type)msg->constraintTypes[i])));
+	}
+
+	this->post(new RtabmapEvent3DMap(images,
+			depths,
+			depths2d,
+			depthConstants,
+			localTransforms,
+			poses,
+			constraints));
+}
+
+void GuiWrapper::handleEvent(UEvent * anEvent)
+{
+	if(anEvent->getClassName().compare("ParamEvent") == 0)
+	{
+		const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters();
+		rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters();
+		bool modified = false;
+		ros::NodeHandle nh;
+		for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
+		{
+			//save only parameters with valid names
+			if(defaultParameters.find((*i).first) != defaultParameters.end())
+			{
+				nh.setParam((*i).first, (*i).second);
+				modified = true;
+			}
+			else if((*i).first.find('/') != (*i).first.npos)
+			{
+				ROS_WARN("Parameter %s is not used by the rtabmap node.", (*i).first.c_str());
+			}
+		}
+		if(modified)
+		{
+			ROS_INFO("Parameters updated");
+			std_srvs::Empty srv;
+			if(!ros::service::call("update_parameters", srv))
+			{
+				ROS_ERROR("Can't call \"update_parameters\" service");
+			}
+		}
+	}
+	else if(anEvent->getClassName().compare("RtabmapEventCmd") == 0)
+	{
+		std_srvs::Empty srv;
+		rtabmap::RtabmapEventCmd * cmdEvent = (rtabmap::RtabmapEventCmd *)anEvent;
+		rtabmap::RtabmapEventCmd::Cmd cmd = cmdEvent->getCmd();
+		if(cmd == rtabmap::RtabmapEventCmd::kCmdDeleteMemory)
+		{
+			if(!ros::service::call("reset", srv))
+			{
+				ROS_ERROR("Can't call \"reset\" service");
+			}
+		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdPause)
+		{
+			if(cmdEvent->getInt())
+			{
+				// Pause the camera if the rtabmap/camera node is used
+				if(!cameraNodeName_.empty())
+				{
+					std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause true", cameraNodeName_.c_str());
+					system(str.c_str());
+				}
+
+				// Pause rtabmap
+				if(!ros::service::call("pause", srv))
+				{
+					ROS_ERROR("Can't call \"pause\" service");
+				}
+			}
+			else
+			{
+				// Resume rtabmap
+				if(!ros::service::call("resume", srv))
+				{
+					ROS_ERROR("Can't call \"resume\" service");
+				}
+
+				// Resume the camera if the rtabmap/camera node is used
+				if(!cameraNodeName_.empty())
+				{
+					std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause false", cameraNodeName_.c_str());
+					system(str.c_str());
+				}
+			}
+		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdTriggerNewMap)
+		{
+			if(!ros::service::call("trigger_new_map", srv))
+			{
+				ROS_ERROR("Can't call \"trigger_new_map\" service");
+			}
+		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapLocal)
+		{
+			if(mapDataTopic_.getNumPublishers())
+			{
+				if(!ros::service::call("publish_local_map_data", srv))
+				{
+					ROS_WARN("Can't call \"publish_local_map_data\" service");
+					this->post(new RtabmapEvent3DMap(1)); // service error
+				}
+			}
+			else
+			{
+				ROS_WARN("No publisher subscribed for topic \"%s\", map cannot be downloaded!", mapDataTopic_.getTopic().c_str());
+				this->post(new RtabmapEvent3DMap(2)); // topic error
+			}
+		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMapGlobal)
+		{
+			if(mapDataTopic_.getNumPublishers())
+			{
+				if(!ros::service::call("publish_global_map_data", srv))
+				{
+					ROS_WARN("Can't call \"publish_global_map_data\" service");
+					this->post(new RtabmapEvent3DMap(1)); // service error
+				}
+			}
+			else
+			{
+				ROS_WARN("No publisher subscribed for topic \"%s\", map cannot be downloaded!", mapDataTopic_.getTopic().c_str());
+				this->post(new RtabmapEvent3DMap(2)); // topic error
+			}
+		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublishTOROGraphLocal)
+		{
+			if(mapDataTopic_.getNumPublishers())
+			{
+				if(!ros::service::call("publish_local_graph", srv))
+				{
+					ROS_WARN("Can't call \"publish_local_graph\" service");
+					this->post(new RtabmapEvent3DMap(1)); // service error
+				}
+			}
+			else
+			{
+				ROS_WARN("No publisher subscribed for topic \"%s\", map cannot be downloaded!", mapDataTopic_.getTopic().c_str());
+				this->post(new RtabmapEvent3DMap(2)); // topic error
+			}
+		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublishTOROGraphGlobal)
+		{
+			if(mapDataTopic_.getNumPublishers())
+			{
+				if(!ros::service::call("publish_global_graph", srv))
+				{
+					ROS_WARN("Can't call \"publish_global_graph\" service");
+					this->post(new RtabmapEvent3DMap(1)); // service error
+				}
+			}
+			else
+			{
+				ROS_WARN("No publisher subscribed for topic \"%s\", map cannot be downloaded!", mapDataTopic_.getTopic().c_str());
+				this->post(new RtabmapEvent3DMap(2)); // topic error
+			}
+		}
+		else
+		{
+			ROS_WARN("Not handled command (%d)...", cmd);
+		}
+	}
+	else if(anEvent->getClassName().compare("OdometryResetEvent") == 0)
+	{
+		std_srvs::Empty srv;
+		if(!ros::service::call("reset_odom", srv))
+		{
+			ROS_ERROR("Can't call \"reset_odom\" service, (will only work with rtabmap/visual_odometry node.)");
+		}
+	}
+}
+
+void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
+{
+	Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+	rtabmap::Image image(
+			cv::Mat(),
+			cv::Mat(),
+			0.0f,
+			odom,
+			Transform());
+	this->post(new OdometryEvent(image));
+}
+
+void GuiWrapper::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
+	{
+		tf::StampedTransform tmp;
+		tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
+		localTransform = transformFromTF(tmp);
+	}
+	catch(tf::TransformException & ex)
+	{
+		ROS_WARN("%s",ex.what());
+		return;
+	}
+
+	Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+	cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+	cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
+
+	float depthConstant = 1.0f/cameraInfoMsg->K[4];
+
+	rtabmap::Image image(
+			ptrImage->image.clone(),
+			ptrDepth->image.clone(),
+			depthConstant,
+			odom,
+			localTransform);
+	this->post(new OdometryEvent(image));
+}
+
+void GuiWrapper::scanCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	// TF ready?
+	try
+	{
+		tf::StampedTransform tmp;
+		tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, 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::depth2DFromPointCloud(pclScan);
+
+	Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+	cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+
+	rtabmap::Image image(
+			ptrImage->image.clone(),
+			cv::Mat(),
+			scan,
+			0.0f,
+			odom,
+			Transform());
+	this->post(new OdometryEvent(image));
+}
+
+void GuiWrapper::depthScanCallback(
+		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
+	{
+		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 = 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::depth2DFromPointCloud(pclScan);
+
+	Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+	cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg);
+	cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
+
+	float depthConstant = 1.0f/cameraInfoMsg->K[4];
+
+	rtabmap::Image image(
+			ptrImage->image.clone(),
+			ptrDepth->image.clone(),
+			scan,
+			depthConstant,
+			odom,
+			localTransform);
+	this->post(new OdometryEvent(image));
+}
+
+void GuiWrapper::setupCallbacks(
+		bool subscribeDepth,
+		bool subscribeLaserScan,
+		int queueSize)
+{
+	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(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);
+		odomSub_.subscribe(nh, "odom", 1);
+		scanSub_.subscribe(nh, "scan", 1);
+		depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+		depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
+	}
+	else if(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_->registerCallback(boost::bind(&GuiWrapper::depthCallback, this, _1, _2, _3, _4));
+	}
+	else if(!subscribeDepth && subscribeLaserScan)
+	{
+		ROS_INFO("Registering LaserScan callback...");
+		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		odomSub_.subscribe(nh, "odom", 1);
+		scanSub_.subscribe(nh, "scan", 1);
+		scanSync_ = new message_filters::Synchronizer<MyScanSyncPolicy>(MyScanSyncPolicy(queueSize), imageSub_, odomSub_, scanSub_);
+		scanSync_->registerCallback(boost::bind(&GuiWrapper::scanCallback, this, _1, _2, _3));
+	}
+	else // default odom only
+	{
+		ROS_INFO("Registering default callback...");
+		defaultSub_ = nh.subscribe("odom", 1, &GuiWrapper::defaultCallback, this);
+	}
+}
+

src/GuiWrapper.h

@@ -0,0 +1,109 @@
+/*
+ * GuiWrapper.h
+ *
+ *  Created on: 4 févr. 2010
+ *      Author: labm2414
+ */
+
+#ifndef GUIWRAPPER_H_
+#define GUIWRAPPER_H_
+
+#include <ros/ros.h>
+#include "rtabmap/InfoEx.h"
+#include "rtabmap/MapData.h"
+#include "rtabmap/utilite/UEventsHandler.h"
+
+#include <tf/transform_listener.h>
+
+#include <geometry_msgs/TwistStamped.h>
+#include <sensor_msgs/PointCloud2.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 <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+namespace rtabmap
+{
+	class MainWindow;
+}
+
+class QApplication;
+
+class GuiWrapper : public UEventsHandler
+{
+public:
+	GuiWrapper(int & argc, char** argv);
+	virtual ~GuiWrapper();
+
+	int exec();
+
+protected:
+	virtual void handleEvent(UEvent * anEvent);
+
+private:
+	void infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg);
+	void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg);
+
+	void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, int queueSize);
+	void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg); // odom
+	void depthCallback(const sensor_msgs::ImageConstPtr& imageMsg,
+					   const nav_msgs::OdometryConstPtr & odomMsg,
+					   const sensor_msgs::ImageConstPtr& imageDepthMsg,
+					   const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
+	void scanCallback(const sensor_msgs::ImageConstPtr& imageMsg,
+					  const nav_msgs::OdometryConstPtr & odomMsg,
+					  const sensor_msgs::LaserScanConstPtr& scanMsg);
+	void depthScanCallback(const sensor_msgs::ImageConstPtr& imageMsg,
+						   const nav_msgs::OdometryConstPtr & odomMsg,
+						   const sensor_msgs::ImageConstPtr& imageDepthMsg,
+						   const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
+						   const sensor_msgs::LaserScanConstPtr& scanMsg);
+
+private:
+	ros::Subscriber infoExTopic_;
+	ros::Subscriber mapDataTopic_;
+	QApplication * app_;
+	rtabmap::MainWindow * mainWindow_;
+	std::string cameraNodeName_;
+
+	// odometry subscription stuffs
+	std::string frameId_;
+	tf::TransformListener tfListener_;
+
+	ros::Subscriber defaultSub_; // odometry only
+	image_transport::SubscriberFilter imageSub_;
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+	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> 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_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			sensor_msgs::LaserScan> MyScanSyncPolicy;
+	message_filters::Synchronizer<MyScanSyncPolicy> * scanSync_;
+};
+
+#endif /* GUIWRAPPER_H_ */

src/MapAssemblerNode.cpp

@@ -0,0 +1,324 @@
+/*
+ * GuiWrapper.cpp
+ *
+ *  Created on: 4 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "GuiWrapper.h"
+#include <QtGui/QApplication>
+#include <QtCore/QDir>
+
+#include <cv_bridge/cv_bridge.h>
+#include <std_srvs/Empty.h>
+#include <std_msgs/Empty.h>
+
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+
+#include <opencv2/highgui/highgui.hpp>
+
+#include <rtabmap/gui/MainWindow.h>
+#include <rtabmap/core/RtabmapEvent.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/ParamEvent.h>
+#include <rtabmap/core/OdometryEvent.h>
+#include <rtabmap/core/util3d.h>
+
+#include "rtabmap/MsgConversion.h"
+
+#include "PreferencesDialogROS.h"
+
+#include <pcl_ros/transforms.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <laser_geometry/laser_geometry.h>
+
+using namespace rtabmap;
+
+class MapAssembler
+{
+
+public:
+	MapAssembler() :
+		cloudDecimation_(4),
+		cloudMaxDepth_(4.0),
+		cloudVoxelSize_(0.02),
+		scanVoxelSize_(0.01)
+	{
+		ros::NodeHandle pnh("~");
+		pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_);
+		pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_);
+		pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_);
+		pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
+
+		ros::NodeHandle nh;
+		infoExTopic_ = nh.subscribe("infoEx", 1, &MapAssembler::infoExReceivedCallback, this);
+		mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);
+
+		assembledMapClouds_ = nh.advertise<sensor_msgs::PointCloud2>("assembled_clouds", 1);
+		assembledMapScans_ = nh.advertise<sensor_msgs::PointCloud2>("assembled_scans", 1);
+	}
+
+	~MapAssembler()
+	{
+	}
+
+	void infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
+	{
+			for(unsigned int i=0; i<msg->data.localTransformIDs.size() && i<msg->data.localTransforms.size(); ++i)
+			{
+				int id = msg->data.localTransformIDs[i];
+				if(!uContains(rgbClouds_, id))
+				{
+					rtabmap::Transform localTransform = transformFromGeometryMsg(msg->data.localTransforms[i]);
+					if(!localTransform.isNull())
+					{
+						cv::Mat image, depth;
+						float depthConstant = 0.0f;
+
+						for(unsigned int i=0; i<msg->data.imageIDs.size() && i<msg->data.images.size(); ++i)
+						{
+							if(msg->data.imageIDs[i] == id)
+							{
+								image = util3d::uncompressImage(msg->data.images[i].bytes);
+								break;
+							}
+						}
+						for(unsigned int i=0; i<msg->data.depthIDs.size() && i<msg->data.depths.size(); ++i)
+						{
+							if(msg->data.depthIDs[i] == id)
+							{
+								depth = util3d::uncompressImage(msg->data.depths[i].bytes);
+								break;
+							}
+						}
+						for(unsigned int i=0; i<msg->data.depthConstantIDs.size() && i<msg->data.depthConstants.size(); ++i)
+						{
+							if(msg->data.depthConstantIDs[i] == id)
+							{
+								depthConstant = msg->data.depthConstants[i];
+								break;
+							}
+						}
+
+						if(!image.empty() && !depth.empty() && depthConstant > 0.0f)
+						{
+							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, depthConstant, cloudDecimation_);
+
+							if(cloudMaxDepth_ > 0)
+							{
+								cloud = util3d::passThrough(cloud, "z", 0, cloudMaxDepth_);
+							}
+							if(cloudVoxelSize_ > 0)
+							{
+								cloud = util3d::voxelize(cloud, cloudVoxelSize_);
+							}
+
+							cloud = util3d::transformPointCloud(cloud, localTransform);
+
+							rgbClouds_.insert(std::make_pair(id, cloud));
+						}
+					}
+				}
+			}
+
+
+		for(unsigned int i=0; i<msg->data.depth2DIDs.size() && i<msg->data.depth2Ds.size(); ++i)
+		{
+			if(!uContains(scans_, msg->data.depth2DIDs[i]))
+			{
+				cv::Mat depth2d = util3d::uncompressData(msg->data.depth2Ds[i].bytes);
+				if(!depth2d.empty())
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::depth2DToPointCloud(depth2d);
+					if(scanVoxelSize_ > 0)
+					{
+						cloud = util3d::voxelize(cloud, scanVoxelSize_);
+					}
+
+					scans_.insert(std::make_pair(msg->data.depth2DIDs[i], cloud));
+				}
+			}
+		}
+
+
+		if(assembledMapClouds_.getNumSubscribers())
+		{
+			// generate the assembled cloud!
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+
+			for(unsigned int i=0; i<msg->data.poseIDs.size() && i<msg->data.poses.size(); ++i)
+			{
+				Transform pose = transformFromPoseMsg(msg->data.poses[i]);
+
+				std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter = rgbClouds_.find(msg->data.poseIDs[i]);
+				if(iter != rgbClouds_.end())
+				{
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(iter->second, pose);
+					*assembledCloud+=*transformed;
+				}
+			}
+
+			if(assembledCloud->size())
+			{
+				if(cloudVoxelSize_ > 0)
+				{
+					assembledCloud = util3d::voxelize(assembledCloud,cloudVoxelSize_);
+				}
+
+				sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
+				pcl::toROSMsg(*assembledCloud, *cloudMsg);
+				cloudMsg->header.stamp = ros::Time::now();
+				cloudMsg->header.frame_id = msg->header.frame_id;
+				assembledMapClouds_.publish(cloudMsg);
+			}
+		}
+
+		if(assembledMapScans_.getNumSubscribers())
+		{
+			// generate the assembled scan!
+			pcl::PointCloud<pcl::PointXYZ>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+			for(unsigned int i=0; i<msg->data.poseIDs.size() && i<msg->data.poses.size(); ++i)
+			{
+				Transform pose = transformFromPoseMsg(msg->data.poses[i]);
+
+				std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = scans_.find(msg->data.poseIDs[i]);
+				if(iter != scans_.end())
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = util3d::transformPointCloud(iter->second, pose);
+					*assembledCloud+=*transformed;
+				}
+			}
+
+			if(assembledCloud->size())
+			{
+				if(scanVoxelSize_ > 0)
+				{
+					assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_);
+				}
+
+				sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
+				pcl::toROSMsg(*assembledCloud, *cloudMsg);
+				cloudMsg->header.stamp = ros::Time::now();
+				cloudMsg->header.frame_id = msg->header.frame_id;
+				assembledMapScans_.publish(cloudMsg);
+			}
+		}
+	}
+
+	void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
+	{
+		std::map<int, std::vector<unsigned char> > images;
+		std::map<int, std::vector<unsigned char> > depths;
+		std::map<int, std::vector<unsigned char> > depths2d;
+		std::map<int, float> depthConstants;
+		std::map<int, Transform> localTransforms;
+		std::map<int, Transform> poses;
+
+		if(msg->imageIDs.size() != msg->images.size())
+		{
+			ROS_WARN("rtabmapviz: receiving map... images and IDs are not the same size (%d vs %d)!",
+					(int)msg->images.size(), (int)msg->imageIDs.size());
+		}
+
+		if(msg->depthIDs.size() != msg->depths.size())
+		{
+			ROS_WARN("rtabmapviz: receiving map... depths and IDs are not the same size (%d vs %d)!",
+					(int)msg->depths.size(), (int)msg->depthIDs.size());
+		}
+
+		if(msg->depthConstantIDs.size() != msg->depthConstants.size())
+		{
+			ROS_WARN("rtabmapviz: receiving map... depthConstants and IDs are not the same size (%d vs %d)!",
+					(int)msg->depthConstants.size(), (int)msg->depthConstantIDs.size());
+		}
+
+		if(msg->depth2DIDs.size() != msg->depth2Ds.size())
+		{
+			ROS_WARN("rtabmapviz: receiving map... depths2D and depth2DIDs are not the same size (%d vs %d)!",
+					(int)msg->depth2Ds.size(), (int)msg->depth2DIDs.size());
+		}
+
+		// fill maps
+		for(unsigned int i=0; i<msg->imageIDs.size() && i < msg->images.size(); ++i)
+		{
+			if(!uContains(rgbClouds_, msg->imageIDs[i]))
+			{
+				images.insert(std::make_pair(msg->imageIDs[i], msg->images[i].bytes));
+			}
+		}
+
+		for(unsigned int i=0; i<msg->depthIDs.size() && i < msg->depths.size(); ++i)
+		{
+			if(!uContains(rgbClouds_, msg->depthIDs[i]))
+			{
+				depths.insert(std::make_pair(msg->depthIDs[i], msg->depths[i].bytes));
+			}
+		}
+
+		for(unsigned int i=0; i<msg->depthConstantIDs.size() && i < msg->depthConstants.size(); ++i)
+		{
+			if(!uContains(rgbClouds_, msg->depthConstantIDs[i]))
+			{
+				depthConstants.insert(std::make_pair(msg->depthConstantIDs[i], msg->depthConstants[i]));
+			}
+		}
+
+		for(unsigned int i=0; i<msg->localTransformIDs.size() && i < msg->localTransforms.size(); ++i)
+		{
+			if(!uContains(rgbClouds_, msg->localTransformIDs[i]))
+			{
+				Transform t = transformFromGeometryMsg(msg->localTransforms[i]);
+				localTransforms.insert(std::make_pair(msg->localTransformIDs[i], t));
+			}
+		}
+
+		for(unsigned int i=0; i<msg->depth2DIDs.size() && i < msg->depth2Ds.size(); ++i)
+		{
+			if(!uContains(scans_, msg->depth2DIDs[i]))
+			{
+				cv::Mat depth2d = util3d::uncompressData(msg->depth2Ds[i].bytes);
+				scans_.insert(std::make_pair(msg->depth2DIDs[i], util3d::depth2DToPointCloud(depth2d)));
+			}
+		}
+
+		// create clouds
+		for(std::map<int, std::vector<unsigned char> >::iterator iter = images.begin(); iter!=images.end(); ++iter)
+		{
+			if(uContains(depths, iter->first) && uContains(depthConstants, iter->first) && uContains(localTransforms, iter->first))
+			{
+				cv::Mat image = util3d::uncompressImage(iter->second);
+				cv::Mat depth = util3d::uncompressImage(depths.at(iter->first));
+				float depthConstant = depthConstants.at(iter->first);
+				rtabmap::Transform localTransform = localTransforms.at(iter->first);
+				rgbClouds_.insert(std::make_pair(iter->first, util3d::cloudFromDepthRGB(image, depth, depthConstant, cloudDecimation_)));
+			}
+		}
+	}
+
+private:
+	int cloudDecimation_;
+	double cloudMaxDepth_;
+	double cloudVoxelSize_;
+	double scanVoxelSize_;
+
+	ros::Subscriber infoExTopic_;
+	ros::Subscriber mapDataTopic_;
+
+	ros::Publisher assembledMapClouds_;
+	ros::Publisher assembledMapScans_;
+
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > rgbClouds_;
+	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
+};
+
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "map_assembler");
+	MapAssembler assembler;
+	ros::spin();
+	return 0;
+}

src/MsgConversion.cpp

@@ -0,0 +1,74 @@
+
+#include "rtabmap/MsgConversion.h"
+
+#include <opencv2/highgui/highgui.hpp>
+#include <zlib.h>
+#include <ros/ros.h>
+#include <rtabmap/core/util3d.h>
+#include <tf_conversions/tf_eigen.h>
+
+namespace rtabmap {
+
+void transformToTF(const rtabmap::Transform & transform, tf::Transform & tfTransform)
+{
+	if(!transform.isNull())
+	{
+		tf::transformEigenToTF(util3d::transformToEigen3d(transform), tfTransform);
+	}
+	else
+	{
+		tfTransform = tf::Transform();
+	}
+}
+
+rtabmap::Transform transformFromTF(const tf::Transform & transform)
+{
+	Eigen::Affine3d eigenTf;
+	tf::transformTFToEigen(transform, eigenTf);
+	return util3d::transformFromEigen3d(eigenTf);
+}
+
+void transformToGeometryMsg(const rtabmap::Transform & transform, geometry_msgs::Transform & msg)
+{
+	if(!transform.isNull())
+	{
+		tf::Transform tfTransform;
+		transformToTF(transform, tfTransform);
+		tf::transformTFToMsg(tfTransform, msg);
+	}
+	else
+	{
+		msg = geometry_msgs::Transform();
+	}
+}
+
+
+rtabmap::Transform transformFromGeometryMsg(const geometry_msgs::Transform & msg)
+{
+	tf::Transform tfTransform;
+	tf::transformMsgToTF(msg, tfTransform);
+	return transformFromTF(tfTransform);
+}
+
+void transformToPoseMsg(const rtabmap::Transform & transform, geometry_msgs::Pose & msg)
+{
+	if(!transform.isNull())
+	{
+		tf::Transform tfTransform;
+		transformToTF(transform, tfTransform);
+		tf::poseTFToMsg(tfTransform, msg);
+	}
+	else
+	{
+		msg = geometry_msgs::Pose();
+	}
+}
+
+rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg)
+{
+	tf::Pose tfTransform;
+	tf::poseMsgToTF(msg, tfTransform);
+	return transformFromTF(tfTransform);
+}
+
+}

src/PreferencesDialogROS.cpp

@@ -0,0 +1,120 @@
+/*
+ * PreferencesDialogROS.cpp
+ *
+ *  Created on: 4 févr. 2010
+ *      Author: labm2414
+ */
+
+#include "PreferencesDialogROS.h"
+#include <rtabmap/core/Parameters.h>
+#include <QtCore/QDir>
+#include <QtCore/QSettings>
+#include <QtGui/QHBoxLayout>
+#include <QtCore/QTimer>
+#include <QtGui/QLabel>
+#include <rtabmap/core/RtabmapEvent.h>
+#include <QtGui/QMessageBox>
+#include <ros/exceptions.h>
+
+using namespace rtabmap;
+
+PreferencesDialogROS::PreferencesDialogROS(const QString & configFile) :
+		configFile_(configFile)
+{
+
+}
+
+PreferencesDialogROS::~PreferencesDialogROS()
+{
+	ROS_INFO("rtabmapviz: GUI settings are saved to \"%s\"", configFile_.toStdString().c_str());
+}
+
+QString PreferencesDialogROS::getIniFilePath() const
+{
+	if(configFile_.isEmpty())
+	{
+		return PreferencesDialog::getIniFilePath();
+	}
+	return configFile_;
+}
+
+void PreferencesDialogROS::readCameraSettings(const QString & filePath)
+{
+	this->setInputRate(0);
+}
+
+QString PreferencesDialogROS::getParamMessage()
+{
+	return tr("Reading parameters from the ROS server...");
+}
+
+bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
+{
+	if(filePath.isEmpty())
+	{
+		ros::NodeHandle nh;
+		ROS_INFO("%s", this->getParamMessage().toStdString().c_str());
+		bool validParameters = true;
+		int readCount = 0;
+		rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+		for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
+		{
+			std::string value;
+			if(nh.getParam((*i).first,value))
+			{
+				PreferencesDialog::setParameter((*i).first, value);
+				++readCount;
+			}
+			else
+			{
+				validParameters = false;
+			}
+		}
+
+		ROS_INFO("Parameters read = %d", readCount);
+
+		if(validParameters)
+		{
+			ROS_INFO("Parameters successfully read.");
+		}
+		else
+		{
+			if(this->isVisible())
+			{
+				QString warning = tr("Failed to get some RTAB-Map parameters from ROS server, the rtabmap node may be not started or some parameters won't work...");
+				ROS_WARN("%s", warning.toStdString().c_str());
+				QMessageBox::warning(this, tr("Can't read parameters from ROS server."), warning);
+			}
+			return false;
+		}
+		return true;
+	}
+	else
+	{
+		return PreferencesDialog::readCoreSettings(filePath);
+	}
+
+}
+
+void PreferencesDialogROS::writeSettings(const QString & filePath)
+{
+	writeGuiSettings(filePath);
+
+	// This will tell the MainWindow that the
+	//parameters are updated. The MainWindow will send an Event that
+	// will be handled by the GuiWrapper where we will write
+	// parameters in ROS and the rtabmap_node will be notified.
+	if(_parameters.size())
+	{
+		emit settingsChanged(_parameters);
+	}
+
+	if(_obsoletePanels)
+	{
+		emit settingsChanged(_obsoletePanels);
+	}
+
+	_parameters = rtabmap::ParametersMap();
+	_obsoletePanels = kPanelDummy;
+}
+

src/PreferencesDialogROS.h

@@ -0,0 +1,35 @@
+/*
+ * PreferencesDialogROS.h
+ *
+ *  Created on: 4 févr. 2010
+ *      Author: labm2414
+ */
+
+#ifndef PREFERENCESDIALOGROS_H_
+#define PREFERENCESDIALOGROS_H_
+
+#include <ros/ros.h>
+#include <rtabmap/gui/PreferencesDialog.h>
+
+using namespace rtabmap;
+
+class PreferencesDialogROS : public PreferencesDialog
+{
+public:
+	PreferencesDialogROS(const QString & configFile);
+	virtual ~PreferencesDialogROS();
+
+	virtual QString getIniFilePath() const;
+
+protected:
+	virtual QString getParamMessage();
+
+	virtual void readCameraSettings(const QString & filePath);
+	virtual bool readCoreSettings(const QString & filePath);
+	virtual void writeSettings(const QString & filePath);
+
+private:
+	QString configFile_;
+};
+
+#endif /* PREFERENCESDIALOGROS_H_ */

src/VisualOdometryNode.cpp

@@ -0,0 +1,314 @@
+/*
+ * visualodometry_.cpp
+ *
+ *  Created on: 2013-01-16
+ *      Author: mathieu
+ */
+
+#include <ros/ros.h>
+
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+
+#include <std_srvs/Empty.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 <nav_msgs/Odometry.h>
+
+#include <cv_bridge/cv_bridge.h>
+
+#include <rtabmap/core/Odometry.h>
+#include <rtabmap/core/util3d.h>
+#include "rtabmap/MsgConversion.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UStl.h"
+
+using namespace rtabmap;
+
+class VisualOdometry
+{
+public:
+	VisualOdometry() :
+		odometry_(0),
+		frameId_("base_link"),
+		odomFrameId_("odom"),
+		sync_(0)
+	{
+		ros::NodeHandle nh;
+
+		odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
+
+		ros::NodeHandle pnh("~");
+
+		int queueSize = 5;
+		pnh.param("frame_id", frameId_, frameId_);
+		pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
+		pnh.param("queue_size", queueSize, queueSize);
+
+		//parameters
+		rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+		rtabmap::ParametersMap parametersOdom;
+		for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		{
+			if((uSplit(iter->first, '/').front().compare("Odom") == 0 ||
+				uSplit(iter->first, '/').front().compare("OdomBin") == 0 ||
+				uSplit(iter->first, '/').front().compare("Kp") == 0 ||
+				uSplit(iter->first, '/').front().compare("SURF") == 0 ||
+				uSplit(iter->first, '/').front().compare("SIFT") == 0)
+			   &&
+			   uSplit(iter->first, '/').front().compare("OdomICP") != 0)
+			{
+				parametersOdom.insert(*iter);
+			}
+		}
+
+		for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
+		{
+			std::string vStr;
+			bool vBool;
+			int vInt;
+			double vDouble;
+			if(pnh.getParam(iter->first, vStr))
+			{
+				ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
+				iter->second = vStr;
+			}
+			else if(pnh.getParam(iter->first, vBool))
+			{
+				ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
+				iter->second = uBool2Str(vBool);
+			}
+			else if(pnh.getParam(iter->first, vInt))
+			{
+				ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
+				iter->second = uNumber2Str(vInt);
+
+				if(iter->first.compare(Parameters::kOdomMinInliers()) == 0 && vInt < 8)
+				{
+					ROS_WARN("Parameter min_inliers must be >= 8, setting to 8...");
+					iter->second = uNumber2Str(8);
+				}
+			}
+			else if(pnh.getParam(iter->first, vDouble))
+			{
+				ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
+				iter->second = uNumber2Str(vDouble);
+			}
+		}
+
+		if(std::atoi(parametersOdom.at(Parameters::kOdomType()).c_str()) == 1)
+		{
+			odometry_ = new rtabmap::OdometryBinary(parametersOdom);
+		}
+		else
+		{
+			odometry_ = new rtabmap::OdometryBOW(parametersOdom);
+		}
+
+		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);
+
+		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_mono_sub_, image_depth_sub_, info_sub_);
+		sync_->registerCallback(boost::bind(&VisualOdometry::callback, this, _1, _2, _3));
+
+		resetSrv_ = nh.advertiseService("reset_odom", &VisualOdometry::reset, this);
+	}
+
+	~VisualOdometry()
+	{
+		delete sync_;
+		delete odometry_;
+	}
+
+	void callback(const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& depth,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==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))
+		{
+			ROS_ERROR("Input type must be image=mono8,rgb8,bgr8 and image_depth=16UC1");
+			return;
+		}
+		else if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0)
+		{
+			static bool warned = false;
+			if(!warned)
+			{
+				ROS_WARN("Input depth type is 32FC1, please use type 16UC1 for depth. The depth images "
+						 "will be processed anyway but with a conversion. This warning is only be printed once...");
+				warned = true;
+			}
+		}
+
+		tf::StampedTransform localTransform;
+		try
+		{
+			tfListener_.lookupTransform(frameId_, image->header.frame_id, image->header.stamp, localTransform);
+		}
+		catch(tf::TransformException & ex)
+		{
+			ROS_WARN("%s",ex.what());
+			return;
+		}
+
+		ros::WallTime time = ros::WallTime::now();
+
+		if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
+		{
+			float depthConstant = 1.0f/cameraInfo->K[4];
+			cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image);
+			cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);
+
+			rtabmap::Image data(ptrImage->image,
+					ptrDepth->image.type() == CV_32FC1?util3d::cvtDepthFromFloat(ptrDepth->image):ptrDepth->image,
+					depthConstant,
+					rtabmap::Transform(),
+					rtabmap::transformFromTF(localTransform));
+			rtabmap::Transform pose = odometry_->process(data);
+			if(!pose.isNull())
+			{
+				//*********************
+				// Update odometry
+				//*********************
+				tf::Transform poseTF;
+				rtabmap::transformToTF(pose, poseTF);
+
+				tfBroadcaster_.sendTransform( tf::StampedTransform (poseTF, image->header.stamp, odomFrameId_, frameId_));
+
+				if(odomPub_.getNumSubscribers())
+				{
+					//next, we'll publish the odometry message over ROS
+					nav_msgs::Odometry odom;
+					odom.header.stamp = image->header.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 = poseTF.getOrigin().x();
+					odom.pose.pose.position.y = poseTF.getOrigin().y();
+					odom.pose.pose.position.z = poseTF.getOrigin().z();
+					tf::quaternionTFToMsg(poseTF.getRotation().normalized(), odom.pose.pose.orientation);
+
+					//publish the message
+					odomPub_.publish(odom);
+				}
+			}
+			else
+			{
+				//ROS_WARN("Odometry lost!");
+
+				//send null pose to notify that odometry is lost
+				nav_msgs::Odometry odom;
+				odom.header.stamp = image->header.stamp; // use corresponding time stamp to image
+				odom.header.frame_id = odomFrameId_;
+				odom.child_frame_id = frameId_;
+
+				//publish the message
+				odomPub_.publish(odom);
+			}
+		}
+
+		ROS_INFO("Odom update time(%f s)", (ros::WallTime::now()-time).toSec());
+	}
+
+	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+	{
+		ROS_INFO("visual_odometry: reset odom!");
+		odometry_->reset();
+		return true;
+	}
+
+private:
+	rtabmap::Odometry * odometry_;
+
+	// parameters
+	std::string frameId_;
+	std::string odomFrameId_;
+
+	ros::Publisher odomPub_;
+	ros::ServiceServer resetSrv_;
+	tf::TransformBroadcaster tfBroadcaster_;
+	tf::TransformListener tfListener_;
+
+	image_transport::SubscriberFilter image_mono_sub_;
+	image_transport::SubscriberFilter image_depth_sub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
+	message_filters::Synchronizer<MySyncPolicy> * sync_;
+};
+
+int main(int argc, char *argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+	ros::init(argc, argv, "visual_odometry");
+
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--params") == 0)
+		{
+			rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+			rtabmap::ParametersMap parametersOdom;
+			if(strcmp(argv[i], "--params") == 0)
+			{
+				// show specific parameters
+				for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+				{
+					if((uSplit(iter->first, '/').front().compare("Odom") == 0 ||
+						uSplit(iter->first, '/').front().compare("OdomBin") == 0 ||
+						uSplit(iter->first, '/').front().compare("Kp") == 0 ||
+						uSplit(iter->first, '/').front().compare("SURF") == 0 ||
+						uSplit(iter->first, '/').front().compare("SIFT") == 0)
+					   &&
+					   uSplit(iter->first, '/').front().compare("OdomICP") != 0)
+					{
+						parametersOdom.insert(*iter);
+					}
+				}
+			}
+
+			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);
+		}
+	}
+
+	VisualOdometry vOdom;
+	ros::spin();
+	return 0;
+}

src/nodelets/data_odom_sync.cpp

@@ -0,0 +1,104 @@
+
+#include "ros/ros.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/CameraInfo.h>
+#include <nav_msgs/Odometry.h>
+
+namespace rtabmap
+{
+
+class DataOdomSyncNodelet : public nodelet::Nodelet
+{
+public:
+	//Constructor
+	DataOdomSyncNodelet():
+		sync_(0)
+	{
+	}
+
+	virtual ~DataOdomSyncNodelet()
+	{
+		delete sync_;
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle& nh = getNodeHandle();
+		ros::NodeHandle& private_nh = getPrivateNodeHandle();
+
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle rgb_pnh(private_nh, "rgb");
+		ros::NodeHandle depth_pnh(private_nh, "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);
+
+		int queueSize = 10;
+		private_nh.param("queue_size", queueSize, queueSize);
+
+		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_, odom_sub_);
+		sync_->registerCallback(boost::bind(&DataOdomSyncNodelet::callback, this, _1, _2, _3, _4));
+
+		image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb);
+		image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
+		info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
+		odom_sub_.subscribe(nh, "odom_in", 1);
+
+		imagePub_ = rgb_it.advertise("image_out", 10);
+		imageDepthPub_ = depth_it.advertise("image_out", 10);
+		infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 10);
+		odomPub_ = nh.advertise<nav_msgs::Odometry>("odom_out", 10);
+	};
+
+	void callback(const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& imageDepth,
+			const sensor_msgs::CameraInfoConstPtr& camInfo,
+			const nav_msgs::OdometryConstPtr & odom)
+	{
+		if(imagePub_.getNumSubscribers())
+		{
+			imagePub_.publish(image);
+		}
+		if(imageDepthPub_.getNumSubscribers())
+		{
+			imageDepthPub_.publish(imageDepth);
+		}
+		if(infoPub_.getNumSubscribers())
+		{
+			infoPub_.publish(camInfo);
+		}
+		if(odomPub_.getNumSubscribers())
+		{
+			odomPub_.publish(odom);
+		}
+	}
+
+	image_transport::Publisher imagePub_;
+	image_transport::Publisher imageDepthPub_;
+	ros::Publisher infoPub_;
+	ros::Publisher odomPub_;
+
+	image_transport::SubscriberFilter image_sub_;
+	image_transport::SubscriberFilter image_depth_sub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
+	message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, nav_msgs::Odometry> MySyncPolicy;
+	message_filters::Synchronizer<MySyncPolicy> * sync_;
+
+};
+
+
+PLUGINLIB_DECLARE_CLASS(rtabmap, data_odom_sync, rtabmap::DataOdomSyncNodelet, nodelet::Nodelet);
+}

src/nodelets/data_throttle.cpp

@@ -0,0 +1,111 @@
+#include "ros/ros.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/CameraInfo.h>
+
+namespace rtabmap
+{
+
+class DataThrottleNodelet : public nodelet::Nodelet
+{
+public:
+	//Constructor
+	DataThrottleNodelet():
+		max_update_rate_(0),
+		sync_(0)
+	{
+	}
+
+	virtual ~DataThrottleNodelet()
+	{
+		delete sync_;
+	}
+
+private:
+	ros::Time last_update_;
+	double max_update_rate_;
+	virtual void onInit()
+	{
+		ros::NodeHandle& nh = getNodeHandle();
+		ros::NodeHandle& private_nh = getPrivateNodeHandle();
+
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle rgb_pnh(private_nh, "rgb");
+		ros::NodeHandle depth_pnh(private_nh, "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);
+
+		int queueSize = 10;
+		private_nh.param("max_rate", max_update_rate_, max_update_rate_);
+		private_nh.param("queue_size", queueSize, queueSize);
+
+		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
+		sync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3));
+
+		image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb);
+		image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
+		info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
+
+		imagePub_ = rgb_it.advertise("image_out", 10);
+		imageDepthPub_ = depth_it.advertise("image_out", 10);
+		infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 10);
+	};
+
+	void callback(const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& imageDepth,
+			const sensor_msgs::CameraInfoConstPtr& camInfo)
+	{
+		if (max_update_rate_ > 0.0)
+		{
+			NODELET_DEBUG("update set to %f", max_update_rate_);
+			if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
+			{
+				NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
+				return;
+			}
+		}
+		else
+			NODELET_DEBUG("update_rate unset continuing");
+
+		last_update_ = ros::Time::now();
+
+		if(imagePub_.getNumSubscribers())
+		{
+			imagePub_.publish(image);
+		}
+		if(imageDepthPub_.getNumSubscribers())
+		{
+			imageDepthPub_.publish(imageDepth);
+		}
+		if(infoPub_.getNumSubscribers())
+		{
+			infoPub_.publish(camInfo);
+		}
+	}
+
+	image_transport::Publisher imagePub_;
+	image_transport::Publisher imageDepthPub_;
+	ros::Publisher infoPub_;
+
+	image_transport::SubscriberFilter image_sub_;
+	image_transport::SubscriberFilter image_depth_sub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
+	message_filters::Synchronizer<MySyncPolicy> * sync_;
+
+};
+
+
+PLUGINLIB_DECLARE_CLASS(rtabmap, data_throttle, rtabmap::DataThrottleNodelet, nodelet::Nodelet);
+}

src/nodelets/point_cloud_xyzrgb.cpp

@@ -0,0 +1,140 @@
+/*
+ * visual_odometry.cpp
+ *
+ *  Created on: 2013-01-16
+ *      Author: mathieu
+ */
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/subscriber.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "rtabmap/core/util3d.h"
+
+namespace rtabmap
+{
+
+class PointCloudXYZRGB : public nodelet::Nodelet
+{
+public:
+	PointCloudXYZRGB() : voxelSize_(0.0) {}
+
+	virtual ~PointCloudXYZRGB()
+	{
+		delete sync_;
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("voxel_size", voxelSize_, voxelSize_);
+
+		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+		sync_->registerCallback(boost::bind(&PointCloudXYZRGB::callback, this, _1, _2, _3));
+		cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
+
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle rgb_pnh(pnh, "rgb");
+		ros::NodeHandle depth_pnh(pnh, "depth");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::ImageTransport depth_it(depth_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+	}
+
+
+
+	void callback(
+			  const sensor_msgs::ImageConstPtr& image,
+			  const sensor_msgs::ImageConstPtr& imageDepth,
+			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) &&
+			(imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 ||
+			 imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0))
+		{
+			ROS_ERROR("Input type must be image=mono8,rgb8,bgr8 and image_depth=32FC1,16UC1");
+			return;
+		}
+
+
+		cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+		cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
+		pclCloud = rtabmap::util3d::cloudFromDepthRGB(
+				imagePtr->image,
+				imageDepthPtr->image,
+				cameraInfo->K[2],
+				cameraInfo->K[5],
+				cameraInfo->K[0],
+				cameraInfo->K[4]);
+
+		if(voxelSize_ > 0.0)
+		{
+			pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
+		}
+
+		//*********************
+		// Publish Map
+		//*********************
+		if(cloudPub_.getNumSubscribers())
+		{
+			sensor_msgs::PointCloud2 rosCloud;
+			pcl::toROSMsg(*pclCloud, rosCloud);
+			rosCloud.header.stamp = image->header.stamp;
+			rosCloud.header.frame_id = image->header.frame_id;
+
+			//publish the message
+			cloudPub_.publish(rosCloud);
+		}
+}
+
+private:
+
+	double voxelSize_;
+	ros::Publisher cloudPub_;
+
+	image_transport::SubscriberFilter imageSub_;
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+
+
+	// without odometry subscription (odometry is computed by this node)
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
+	message_filters::Synchronizer<MySyncPolicy> * sync_;
+};
+
+PLUGINLIB_DECLARE_CLASS(rtabmap, point_cloud_xyzrgb, rtabmap::PointCloudXYZRGB, nodelet::Nodelet);
+}
+