First version rtabmap_launch working

rtabmap_util/src/DbPlayerNode.cpp

@@ -0,0 +1,682 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <rosgraph_msgs/Clock.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <nav_msgs/Odometry.h>
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <std_srvs/Empty.h>
+#include <rtabmap_conversions/MsgConversion.h>
+#include <rtabmap_msgs/SetGoal.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/DBReader.h>
+#include <rtabmap/core/OdometryEvent.h>
+#include <cmath>
+
+#ifndef _WIN32
+#include <sys/ioctl.h>
+#include <termios.h>
+bool spacehit()
+{
+	bool charAvailable = true;
+	bool hit = false;
+	while(charAvailable)
+	{
+		termios term;
+		tcgetattr(0, &term);
+
+		termios term2 = term;
+		term2.c_lflag &= ~ICANON;
+		term2.c_lflag &= ~ECHO;
+		term2.c_lflag &= ~ISIG;
+		term2.c_cc[VMIN] = 0;
+		term2.c_cc[VTIME] = 0;
+		tcsetattr(0, TCSANOW, &term2);
+
+		int c = getchar();
+		if(c != EOF)
+		{
+			if(c == ' ')
+			{
+				hit = true;
+			}
+		}
+		else
+		{
+			charAvailable = false;
+		}
+
+		tcsetattr(0, TCSANOW, &term);
+	}
+
+    return hit;
+}
+#endif
+
+bool paused = false;
+bool pauseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	if(paused)
+	{
+		ROS_WARN("Already paused!");
+	}
+	else
+	{
+		paused = true;
+		ROS_INFO("paused!");
+	}
+	return true;
+}
+
+bool resumeCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	if(!paused)
+	{
+		ROS_WARN("Already running!");
+	}
+	else
+	{
+		paused = false;
+		ROS_INFO("resumed!");
+	}
+	return true;
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "data_player");
+
+	//ULogger::setType(ULogger::kTypeConsole);
+	//ULogger::setLevel(ULogger::kDebug);
+	//ULogger::setEventLevel(ULogger::kWarning);
+
+	bool publishClock = false;
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--clock") == 0)
+		{
+			publishClock = true;
+		}
+	}
+
+	ros::NodeHandle nh;
+	ros::NodeHandle pnh("~");
+
+	std::string frameId = "base_link";
+	std::string odomFrameId = "odom";
+	std::string cameraFrameId = "camera_optical_link";
+	std::string scanFrameId = "base_laser_link";
+	double rate = 1.0f;
+	std::string databasePath = "";
+	bool publishTf = true;
+	int startId = 0;
+	bool useDbStamps = true;
+
+	pnh.param("frame_id", frameId, frameId);
+	pnh.param("odom_frame_id", odomFrameId, odomFrameId);
+	pnh.param("camera_frame_id", cameraFrameId, cameraFrameId);
+	pnh.param("scan_frame_id", scanFrameId, scanFrameId);
+	pnh.param("rate", rate, rate); // Ratio of the database stamps
+	pnh.param("database", databasePath, databasePath);
+	pnh.param("publish_tf", publishTf, publishTf);
+	pnh.param("start_id", startId, startId);
+
+	// A general 360 lidar with 0.5 deg increment
+	double scanAngleMin, scanAngleMax, scanAngleIncrement, scanRangeMin, scanRangeMax;
+	pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI);
+	pnh.param<double>("scan_angle_max", scanAngleMax, M_PI);
+	pnh.param<double>("scan_angle_increment", scanAngleIncrement, M_PI / 720.0);
+	pnh.param<double>("scan_range_min", scanRangeMin, 0.0);
+	pnh.param<double>("scan_range_max", scanRangeMax, 60);
+
+	ROS_INFO("frame_id = %s", frameId.c_str());
+	ROS_INFO("odom_frame_id = %s", odomFrameId.c_str());
+	ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str());
+	ROS_INFO("scan_frame_id = %s", scanFrameId.c_str());
+	ROS_INFO("rate = %f", rate);
+	ROS_INFO("publish_tf = %s", publishTf?"true":"false");
+	ROS_INFO("start_id = %d", startId);
+	ROS_INFO("Publish clock (--clock): %s", publishClock?"true":"false");
+
+	if(databasePath.empty())
+	{
+		ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database).");
+		return -1;
+	}
+	databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
+	if(databasePath.size() && databasePath.at(0) != '/')
+	{
+		databasePath = UDirectory::currentDir(true) + databasePath;
+	}
+	ROS_INFO("database = %s", databasePath.c_str());
+
+	rtabmap::DBReader reader(databasePath, -rate, false, false, false, startId);
+	if(!reader.init())
+	{
+		ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str());
+		return -1;
+	}
+
+	ros::ServiceServer pauseSrv = pnh.advertiseService("pause", pauseCallback);
+	ros::ServiceServer resumeSrv = pnh.advertiseService("resume", resumeCallback);
+
+	image_transport::ImageTransport it(nh);
+	image_transport::Publisher imagePub;
+	image_transport::Publisher rgbPub;
+	image_transport::Publisher depthPub;
+	image_transport::Publisher leftPub;
+	image_transport::Publisher rightPub;
+	ros::Publisher rgbCamInfoPub;
+	ros::Publisher depthCamInfoPub;
+	ros::Publisher leftCamInfoPub;
+	ros::Publisher rightCamInfoPub;
+	ros::Publisher odometryPub;
+	ros::Publisher scanPub;
+	ros::Publisher scanCloudPub;
+	ros::Publisher globalPosePub;
+	ros::Publisher gpsFixPub;
+	ros::Publisher clockPub;
+	tf2_ros::TransformBroadcaster tfBroadcaster;
+
+	if(publishClock)
+	{
+		clockPub = nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
+	}
+
+	UTimer timer;
+	rtabmap::CameraInfo cameraInfo;
+	rtabmap::SensorData data = reader.takeImage(&cameraInfo);
+	rtabmap::OdometryInfo odomInfo;
+	odomInfo.reg.covariance = cameraInfo.odomCovariance;
+	rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo);
+	double acquisitionTime = timer.ticks();
+	while(ros::ok() && odom.data().id())
+	{
+		ROS_INFO("Reading sensor data %d...", odom.data().id());
+
+		ros::Time time(odom.data().stamp());
+
+		if(publishClock)
+		{
+			rosgraph_msgs::Clock msg;
+			msg.clock = time;
+			clockPub.publish(msg);
+		}
+
+		sensor_msgs::CameraInfo camInfoA; //rgb or left
+		sensor_msgs::CameraInfo camInfoB; //depth or right
+
+		camInfoA.K.assign(0);
+		camInfoA.K[0] = camInfoA.K[4] = camInfoA.K[8] = 1;
+		camInfoA.R.assign(0);
+		camInfoA.R[0] = camInfoA.R[4] = camInfoA.R[8] = 1;
+		camInfoA.P.assign(0);
+		camInfoA.P[10] = 1;
+
+		camInfoA.header.frame_id = cameraFrameId;
+		camInfoA.header.stamp = time;
+
+		camInfoB = camInfoA;
+
+		int type = -1;
+		if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1))
+		{
+			if(odom.data().cameraModels().size() > 1)
+			{
+				ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
+			}
+			else
+			{
+				//depth
+				if(odom.data().cameraModels().size())
+				{
+					camInfoA.D.resize(5,0);
+
+					camInfoA.P[0] = odom.data().cameraModels()[0].fx();
+					camInfoA.K[0] = odom.data().cameraModels()[0].fx();
+					camInfoA.P[5] = odom.data().cameraModels()[0].fy();
+					camInfoA.K[4] = odom.data().cameraModels()[0].fy();
+					camInfoA.P[2] = odom.data().cameraModels()[0].cx();
+					camInfoA.K[2] = odom.data().cameraModels()[0].cx();
+					camInfoA.P[6] = odom.data().cameraModels()[0].cy();
+					camInfoA.K[5] = odom.data().cameraModels()[0].cy();
+
+					camInfoB = camInfoA;
+				}
+
+				type=0;
+
+				if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1);
+				if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1);
+				if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
+				if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
+			}
+		}
+		else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
+		{
+			if(odom.data().stereoCameraModels().size() > 1)
+			{
+				ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
+			}
+			else
+			{
+				//stereo
+				if(odom.data().stereoCameraModels()[0].isValidForProjection())
+				{
+					camInfoA.D.resize(8,0);
+
+					camInfoA.P[0] = odom.data().stereoCameraModels()[0].left().fx();
+					camInfoA.K[0] = odom.data().stereoCameraModels()[0].left().fx();
+					camInfoA.P[5] = odom.data().stereoCameraModels()[0].left().fy();
+					camInfoA.K[4] = odom.data().stereoCameraModels()[0].left().fy();
+					camInfoA.P[2] = odom.data().stereoCameraModels()[0].left().cx();
+					camInfoA.K[2] = odom.data().stereoCameraModels()[0].left().cx();
+					camInfoA.P[6] = odom.data().stereoCameraModels()[0].left().cy();
+					camInfoA.K[5] = odom.data().stereoCameraModels()[0].left().cy();
+
+					camInfoB = camInfoA;
+					camInfoB.P[3] = odom.data().stereoCameraModels()[0].right().Tx(); // Right_Tx = -baseline*fx
+				}
+
+				type=1;
+
+				if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
+				if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
+				if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
+				if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
+			}
+
+		}
+		else
+		{
+			if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
+		}
+
+		camInfoA.height = odom.data().imageRaw().rows;
+		camInfoA.width = odom.data().imageRaw().cols;
+		camInfoB.height = odom.data().depthOrRightRaw().rows;
+		camInfoB.width = odom.data().depthOrRightRaw().cols;
+
+		if(!odom.data().laserScanRaw().isEmpty())
+		{
+			if(scanPub.getTopic().empty() && odom.data().laserScanRaw().is2d())
+			{
+				scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
+				if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
+				{
+					ROS_INFO("Scan will be published.");
+				}
+				else
+				{
+					ROS_INFO("Scan will be published with those parameters:");
+					ROS_INFO("  scan_angle_min=%f", scanAngleMin);
+					ROS_INFO("  scan_angle_max=%f", scanAngleMax);
+					ROS_INFO("  scan_angle_increment=%f", scanAngleIncrement);
+					ROS_INFO("  scan_range_min=%f", scanRangeMin);
+					ROS_INFO("  scan_range_max=%f", scanRangeMax);
+				}
+			}
+			else if(scanCloudPub.getTopic().empty())
+			{
+				scanCloudPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1);
+				ROS_INFO("Scan cloud will be published.");
+			}
+		}
+
+		if(!odom.data().globalPose().isNull() &&
+			odom.data().globalPoseCovariance().cols==6 &&
+			odom.data().globalPoseCovariance().rows==6)
+		{
+			if(globalPosePub.getTopic().empty())
+			{
+				globalPosePub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("global_pose", 1);
+				ROS_INFO("Global pose will be published.");
+			}
+		}
+
+		if(odom.data().gps().stamp() > 0.0)
+		{
+			if(gpsFixPub.getTopic().empty())
+			{
+				gpsFixPub = nh.advertise<sensor_msgs::NavSatFix>("gps/fix", 1);
+				ROS_INFO("GPS will be published.");
+			}
+		}
+
+		// publish transforms first
+		if(publishTf)
+		{
+			rtabmap::Transform localTransform;
+			if(odom.data().cameraModels().size() == 1)
+			{
+				localTransform = odom.data().cameraModels()[0].localTransform();
+			}
+			else if(odom.data().stereoCameraModels().size() == 1)
+			{
+				localTransform = odom.data().stereoCameraModels()[0].left().localTransform();
+			}
+			if(!localTransform.isNull())
+			{
+				geometry_msgs::TransformStamped baseToCamera;
+				baseToCamera.child_frame_id = cameraFrameId;
+				baseToCamera.header.frame_id = frameId;
+				baseToCamera.header.stamp = time;
+				rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform);
+				tfBroadcaster.sendTransform(baseToCamera);
+			}
+
+			if(!odom.pose().isNull())
+			{
+				geometry_msgs::TransformStamped odomToBase;
+				odomToBase.child_frame_id = frameId;
+				odomToBase.header.frame_id = odomFrameId;
+				odomToBase.header.stamp = time;
+				rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform);
+				tfBroadcaster.sendTransform(odomToBase);
+			}
+
+			if(!scanPub.getTopic().empty() || !scanCloudPub.getTopic().empty())
+			{
+				geometry_msgs::TransformStamped baseToLaserScan;
+				baseToLaserScan.child_frame_id = scanFrameId;
+				baseToLaserScan.header.frame_id = frameId;
+				baseToLaserScan.header.stamp = time;
+				rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform);
+				tfBroadcaster.sendTransform(baseToLaserScan);
+			}
+		}
+		if(!odom.pose().isNull())
+		{
+			if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);
+
+			if(odometryPub.getNumSubscribers())
+			{
+				nav_msgs::Odometry odomMsg;
+				odomMsg.child_frame_id = frameId;
+				odomMsg.header.frame_id = odomFrameId;
+				odomMsg.header.stamp = time;
+				rtabmap_conversions::transformToPoseMsg(odom.pose(), odomMsg.pose.pose);
+				UASSERT(odomMsg.pose.covariance.size() == 36 &&
+						odom.covariance().total() == 36 &&
+						odom.covariance().type() == CV_64FC1);
+				memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
+				odometryPub.publish(odomMsg);
+			}
+		}
+
+		// Publish async topics first (so that they can catched by rtabmap before the image topics)
+		if(globalPosePub.getNumSubscribers() > 0 &&
+			!odom.data().globalPose().isNull() &&
+			odom.data().globalPoseCovariance().cols==6 &&
+			odom.data().globalPoseCovariance().rows==6)
+		{
+			geometry_msgs::PoseWithCovarianceStamped msg;
+			rtabmap_conversions::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose);
+			memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double));
+			msg.header.frame_id = frameId;
+			msg.header.stamp = time;
+			globalPosePub.publish(msg);
+		}
+
+		if(odom.data().gps().stamp() > 0.0)
+		{
+			sensor_msgs::NavSatFix msg;
+			msg.longitude = odom.data().gps().longitude();
+			msg.latitude = odom.data().gps().latitude();
+			msg.altitude = odom.data().gps().altitude();
+			msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
+			msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error();
+			msg.header.frame_id = frameId;
+			msg.header.stamp.fromSec(odom.data().gps().stamp());
+			gpsFixPub.publish(msg);
+		}
+
+		if(type >= 0)
+		{
+			if(rgbCamInfoPub.getNumSubscribers() && type == 0)
+			{
+				rgbCamInfoPub.publish(camInfoA);
+			}
+			if(leftCamInfoPub.getNumSubscribers() && type == 1)
+			{
+				leftCamInfoPub.publish(camInfoA);
+			}
+			if(depthCamInfoPub.getNumSubscribers() && type == 0)
+			{
+				depthCamInfoPub.publish(camInfoB);
+			}
+			if(rightCamInfoPub.getNumSubscribers() && type == 1)
+			{
+				rightCamInfoPub.publish(camInfoB);
+			}
+		}
+
+		if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
+		{
+			cv_bridge::CvImage img;
+			if(odom.data().imageRaw().channels() == 1)
+			{
+				img.encoding = sensor_msgs::image_encodings::MONO8;
+			}
+			else
+			{
+				img.encoding = sensor_msgs::image_encodings::BGR8;
+			}
+			img.image = odom.data().imageRaw();
+			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
+			imageRosMsg->header.frame_id = cameraFrameId;
+			imageRosMsg->header.stamp = time;
+
+			if(imagePub.getNumSubscribers())
+			{
+				imagePub.publish(imageRosMsg);
+			}
+			if(rgbPub.getNumSubscribers() && type == 0)
+			{
+				rgbPub.publish(imageRosMsg);
+			}
+			if(leftPub.getNumSubscribers() && type == 1)
+			{
+				leftPub.publish(imageRosMsg);
+				leftCamInfoPub.publish(camInfoA);
+			}
+		}
+
+		if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0)
+		{
+			cv_bridge::CvImage img;
+			if(odom.data().depthRaw().type() == CV_32FC1)
+			{
+				img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
+			}
+			else
+			{
+				img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
+			}
+			img.image = odom.data().depthRaw();
+			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
+			imageRosMsg->header.frame_id = cameraFrameId;
+			imageRosMsg->header.stamp = time;
+
+			depthPub.publish(imageRosMsg);
+			depthCamInfoPub.publish(camInfoB);
+		}
+
+		if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1)
+		{
+			cv_bridge::CvImage img;
+			img.encoding = sensor_msgs::image_encodings::MONO8;
+			img.image = odom.data().rightRaw();
+			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
+			imageRosMsg->header.frame_id = cameraFrameId;
+			imageRosMsg->header.stamp = time;
+
+			rightPub.publish(imageRosMsg);
+			rightCamInfoPub.publish(camInfoB);
+		}
+
+		if(!odom.data().laserScanRaw().isEmpty())
+		{
+			if(scanPub.getNumSubscribers() && odom.data().laserScanRaw().is2d())
+			{
+				//inspired from pointcloud_to_laserscan package
+				sensor_msgs::LaserScan msg;
+				msg.header.frame_id = scanFrameId;
+				msg.header.stamp = time;
+
+				msg.angle_min = scanAngleMin;
+				msg.angle_max = scanAngleMax;
+				msg.angle_increment = scanAngleIncrement;
+				msg.time_increment = 0.0;
+				msg.scan_time = 0;
+				msg.range_min = scanRangeMin;
+				msg.range_max = scanRangeMax;
+				if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
+				{
+					msg.angle_min = odom.data().laserScanRaw().angleMin();
+					msg.angle_max = odom.data().laserScanRaw().angleMax();
+					msg.angle_increment = odom.data().laserScanRaw().angleIncrement();
+					msg.range_min = odom.data().laserScanRaw().rangeMin();
+					msg.range_max = odom.data().laserScanRaw().rangeMax();
+				}
+
+				uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
+				msg.ranges.assign(rangesSize, 0.0);
+
+				const cv::Mat & scan = odom.data().laserScanRaw().data();
+				for (int i=0; i<scan.cols; ++i)
+				{
+					const float * ptr = scan.ptr<float>(0,i);
+					double range = hypot(ptr[0], ptr[1]);
+					if (range >= msg.range_min && range <=msg.range_max)
+					{
+						double angle = atan2(ptr[1], ptr[0]);
+						if (angle >= msg.angle_min && angle <= msg.angle_max)
+						{
+							int index = (angle - msg.angle_min) / msg.angle_increment;
+							if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
+							{
+								msg.ranges[index] = range;
+							}
+						}
+					}
+				}
+
+				scanPub.publish(msg);
+			}
+			else if(scanCloudPub.getNumSubscribers())
+			{
+				sensor_msgs::PointCloud2 msg;
+				pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg);
+				msg.header.frame_id = scanFrameId;
+				msg.header.stamp = time;
+				scanCloudPub.publish(msg);
+			}
+		}
+
+		if(odom.data().userDataRaw().type() == CV_8SC1 &&
+		   odom.data().userDataRaw().cols >= 7 && // including null str ending
+		   odom.data().userDataRaw().rows == 1 &&
+		   memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
+		{
+			//GOAL format detected, remove it from the user data and send it as goal event
+			std::string goalStr = (const char *)odom.data().userDataRaw().data;
+			if(!goalStr.empty())
+			{
+				std::list<std::string> strs = uSplit(goalStr, ':');
+				if(strs.size() == 2)
+				{
+					int goalId = atoi(strs.rbegin()->c_str());
+
+					if(goalId > 0)
+					{
+						ROS_WARN("Goal %d detected, calling rtabmap's set_goal service!", goalId);
+						rtabmap_msgs::SetGoal setGoalSrv;
+						setGoalSrv.request.node_id = goalId;
+						setGoalSrv.request.node_label = "";
+						if(!ros::service::call("set_goal", setGoalSrv))
+						{
+							ROS_ERROR("Can't call \"set_goal\" service");
+						}
+					}
+				}
+			}
+		}
+
+		ros::spinOnce();
+
+		while(ros::ok())
+		{
+#ifndef _WIN32
+			if (spacehit()) {
+				paused = !paused;
+				if(paused)
+				{
+					ROS_INFO("paused!");
+				}
+				else
+				{
+					ROS_INFO("resumed!");
+				}
+			}
+#endif
+
+			if(!paused)
+			{
+				break;
+			}
+
+			uSleep(100);
+			ros::spinOnce();
+		}
+
+		timer.restart();
+		cameraInfo = rtabmap::CameraInfo();
+		data = reader.takeImage(&cameraInfo);
+		odomInfo.reg.covariance = cameraInfo.odomCovariance;
+		odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo);
+		acquisitionTime = timer.ticks();
+	}
+
+
+	return 0;
+}

rtabmap_util/src/ImuToTFNode.cpp

@@ -0,0 +1,48 @@
+/*
+Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "imu_to_tf");
+
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		nargv.push_back(argv[i]);
+	}
+
+	nodelet::Loader nodelet;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_util/imu_to_tf", remap, nargv);
+	ros::spin();
+	return 0;
+}
+

rtabmap_util/src/LidarDeskewingNode.cpp

@@ -0,0 +1,47 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "lidar_deskewing");
+
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		nargv.push_back(argv[i]);
+	}
+
+	nodelet::Loader nodelet;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_util/lidar_deskewing", remap, nargv);
+	ros::spin();
+	return 0;
+}

rtabmap_util/src/MapAssemblerNode.cpp

@@ -0,0 +1,409 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include "rtabmap_msgs/MapData.h"
+#include "rtabmap_conversions/MsgConversion.h"
+#include "rtabmap_util/MapsManager.h"
+#include "rtabmap_msgs/GetMap.h"
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_mapping.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/core/Graph.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UFile.h>
+#include <pcl_ros/transforms.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <std_srvs/Empty.h>
+
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+#include <octomap_msgs/GetOctomap.h>
+#include <octomap_msgs/conversions.h>
+#include <rtabmap/core/OctoMap.h>
+#endif
+#endif
+
+using namespace rtabmap;
+
+class MapAssembler
+{
+
+public:
+	MapAssembler(int & argc, char** argv) :
+		localGridsRegenerated_(false)
+	{
+		ros::NodeHandle pnh("~");
+		ros::NodeHandle nh;
+
+		std::string configPath;
+		pnh.param("config_path", configPath, configPath);
+		pnh.param("regenerate_local_grids", localGridsRegenerated_, localGridsRegenerated_);
+
+		//parameters
+		rtabmap::ParametersMap parameters;
+		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
+		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
+		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
+		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM"));
+		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp())));
+		if(!configPath.empty())
+		{
+			if(UFile::exists(configPath.c_str()))
+			{
+				ROS_INFO( "%s: Loading parameters from %s", ros::this_node::getName().c_str(), configPath.c_str());
+				rtabmap::ParametersMap allParameters;
+				Parameters::readINI(configPath.c_str(), allParameters);
+				// only update odometry parameters
+				for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+				{
+					ParametersMap::iterator jter = allParameters.find(iter->first);
+					if(jter!=allParameters.end())
+					{
+						iter->second = jter->second;
+					}
+				}
+			}
+			else
+			{
+				ROS_ERROR( "Config file \"%s\" not found!", configPath.c_str());
+			}
+		}
+		for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		{
+			std::string vStr;
+			bool vBool;
+			int vInt;
+			double vDouble;
+			if(pnh.getParam(iter->first, vStr))
+			{
+				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), vStr.c_str());
+				iter->second = vStr;
+			}
+			else if(pnh.getParam(iter->first, vBool))
+			{
+				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uBool2Str(vBool).c_str());
+				iter->second = uBool2Str(vBool);
+			}
+			else if(pnh.getParam(iter->first, vDouble))
+			{
+				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vDouble).c_str());
+				iter->second = uNumber2Str(vDouble);
+			}
+			else if(pnh.getParam(iter->first, vInt))
+			{
+				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vInt).c_str());
+				iter->second = uNumber2Str(vInt);
+			}
+		}
+
+		rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argc, argv);
+		for(rtabmap::ParametersMap::iterator iter=argParameters.begin(); iter!=argParameters.end(); ++iter)
+		{
+			rtabmap::ParametersMap::iterator jter = parameters.find(iter->first);
+			if(jter!=parameters.end())
+			{
+				ROS_INFO( "Update %s parameter \"%s\"=\"%s\" from arguments", ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.c_str());
+				jter->second = iter->second;
+			}
+		}
+
+		// Backward compatibility
+		for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
+			iter!=Parameters::getRemovedParameters().end();
+			++iter)
+		{
+			std::string vStr;
+			if(pnh.getParam(iter->first, vStr))
+			{
+				if(iter->second.first && parameters.find(iter->second.second) != parameters.end())
+				{
+					// can be migrated
+					parameters.at(iter->second.second)= vStr;
+					ROS_WARN( "%s: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
+							ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
+				}
+				else
+				{
+					if(iter->second.second.empty())
+					{
+						ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore!",
+								ros::this_node::getName().c_str(), iter->first.c_str());
+					}
+					else
+					{
+						ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
+								ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str());
+					}
+				}
+			}
+		}
+
+		// set private parameters
+		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		{
+			pnh.setParam(iter->first, iter->second);
+		}
+
+		ROS_INFO("%s: regenerate_local_grids          = %s", ros::this_node::getName().c_str(), localGridsRegenerated_?"true":"false");
+		mapsManager_.init(nh, pnh, ros::this_node::getName(), false);
+		mapsManager_.backwardCompatibilityParameters(pnh, parameters);
+		mapsManager_.setParameters(parameters);
+
+		std::list<std::string> splitName = uSplit(nh.resolveName("mapData"), '/');
+		std::string rtabmapNs;
+		for(std::list<std::string>::iterator iter=splitName.begin(); iter!=splitName.end() && iter!=--splitName.end(); ++iter)
+		{
+			if(!rtabmapNs.empty())
+			{
+				rtabmapNs += "/";
+			}
+			rtabmapNs += *iter;
+		}
+		ROS_INFO("Rtabmap namespace is \"%s\", deduced from topic \"%s\"", rtabmapNs.c_str(), nh.resolveName("mapData").c_str());
+		if(rtabmapNs.empty())
+		{
+			rtabmapNs = "get_map_data";
+		}
+		else
+		{
+			rtabmapNs += "/get_map_data";
+		}
+
+		rtabmap_msgs::GetMap getMapSrv;
+		getMapSrv.request.global = false;
+		getMapSrv.request.optimized = true;
+		getMapSrv.request.graphOnly = false;
+		if(ros::service::waitForService(rtabmapNs, 5000))
+		{
+			if(!ros::service::call(rtabmapNs, getMapSrv))
+			{
+				ROS_WARN("Cannot call \"%s\" service", rtabmapNs.c_str());
+			}
+			else
+			{
+				ROS_INFO("Called \"%s\" service, initializing cache...", rtabmapNs.c_str());
+				processMapData(getMapSrv.response.data);
+				ROS_INFO("Called \"%s\" service, initializing cache... done! The map"
+						" will be assembled on next subscriber connection.", rtabmapNs.c_str());
+			}
+		}
+		else
+		{
+			ROS_WARN("Service \"%s\" not available after waiting for 5 seconds, "
+					"may not be a problem if rtabmap is started afterwards. If rtabmap "
+					"is started after in localization mode, call /rtabmap/publish_maps "
+					"service with graph_only=false to make sure map_assembler has all the data.", rtabmapNs.c_str());
+		}
+
+
+		mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);
+
+		// private services
+		resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this);
+
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+		octomapBinarySrv_ = pnh.advertiseService("octomap_binary", &MapAssembler::octomapBinaryCallback, this);
+		octomapFullSrv_ = pnh.advertiseService("octomap_full", &MapAssembler::octomapFullCallback, this);
+#endif
+#endif
+	}
+
+	~MapAssembler()
+	{
+	}
+
+	void mapDataReceivedCallback(const rtabmap_msgs::MapDataConstPtr & msg)
+	{
+		processMapData(*msg);
+	}
+	void processMapData(const rtabmap_msgs::MapData & msg)
+	{
+		UTimer timer;
+
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> constraints;
+		Transform mapOdom;
+		rtabmap_conversions::mapGraphFromROS(msg.graph, poses, constraints, mapOdom);
+		for(unsigned int i=0; i<msg.nodes.size(); ++i)
+		{
+			if(msg.nodes[i].image.size() ||
+			   msg.nodes[i].depth.size() ||
+			   msg.nodes[i].laserScan.size())
+			{
+				Signature data = rtabmap_conversions::nodeDataFromROS(msg.nodes[i]);
+				if(localGridsRegenerated_)
+				{
+					data.sensorData().setOccupancyGrid(cv::Mat(), cv::Mat(), cv::Mat(), 0, cv::Point3f());
+				}
+				uInsert(nodes_, std::make_pair(msg.nodes[i].id, data));
+			}
+		}
+
+		// create a tmp signature with latest sensory data
+		if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
+		{
+			Signature tmpS = nodes_.at(poses.rbegin()->first);
+			SensorData tmpData = tmpS.sensorData();
+			tmpData.setId(0);
+			uInsert(nodes_, std::make_pair(0, Signature(0, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData)));
+			poses.insert(std::make_pair(0, poses.rbegin()->second));
+		}
+
+		// Update maps
+		if(!nodes_.empty())
+		{
+			poses = mapsManager_.updateMapCaches(
+					poses,
+					0,
+					false,
+					false,
+					nodes_);
+		}
+		double updateTime = timer.ticks();
+
+		mapFrameId_ = msg.header.frame_id;
+		optimizedPoses_ = poses;
+
+		mapsManager_.publishMaps(poses, msg.header.stamp, msg.header.frame_id);
+
+		ROS_INFO("map_assembler: Updating = %fs, Publishing data = %fs (subscribers=%s)", updateTime, timer.ticks(), mapsManager_.hasSubscribers()?"true":"false");
+	}
+
+	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+	{
+		ROS_INFO("map_assembler: reset!");
+		mapsManager_.clear();
+		return true;
+	}
+
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+	bool octomapBinaryCallback(
+			octomap_msgs::GetOctomap::Request  &req,
+			octomap_msgs::GetOctomap::Response &res)
+	{
+		ROS_INFO("Sending binary map data on service request");
+		res.map.header.frame_id = mapFrameId_;
+		res.map.header.stamp = ros::Time::now();
+
+		mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
+
+		const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
+		bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map);
+		return success;
+	}
+
+	bool octomapFullCallback(
+			octomap_msgs::GetOctomap::Request  &req,
+			octomap_msgs::GetOctomap::Response &res)
+	{
+		ROS_INFO("Sending full map data on service request");
+		res.map.header.frame_id = mapFrameId_;
+		res.map.header.stamp = ros::Time::now();
+
+		mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
+
+		const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
+		bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map);
+		return success;
+	}
+#endif
+#endif
+
+private:
+	rtabmap_util::MapsManager mapsManager_;
+	std::map<int, Signature> nodes_;
+	std::map<int, Transform> optimizedPoses_;
+	std::string mapFrameId_;
+
+	ros::Subscriber mapDataTopic_;
+
+	ros::ServiceServer resetService_;
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+	ros::ServiceServer octomapBinarySrv_;
+	ros::ServiceServer octomapFullSrv_;
+#endif
+#endif
+	bool localGridsRegenerated_;
+};
+
+
+int main(int argc, char** argv)
+{
+	ULogger::setLevel(ULogger::kError);
+	ULogger::setType(ULogger::kTypeConsole);
+
+	ros::init(argc, argv, "map_assembler");
+
+	// process "--params" argument
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--params") == 0)
+		{
+			rtabmap::ParametersMap parameters;
+			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
+			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
+			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
+			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM"));
+			uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp())));
+			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 parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
+		else if(strcmp(argv[i], "--udebug") == 0)
+		{
+			ULogger::setLevel(ULogger::kDebug);
+		}
+		else if(strcmp(argv[i], "--uinfo") == 0)
+		{
+			ULogger::setLevel(ULogger::kInfo);
+		}
+	}
+
+	MapAssembler assembler(argc, argv);
+	ros::spin();
+	return 0;
+}

rtabmap_util/src/MapOptimizerNode.cpp

@@ -0,0 +1,345 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include "rtabmap_msgs/MapData.h"
+#include "rtabmap_msgs/MapGraph.h"
+#include "rtabmap_conversions/MsgConversion.h"
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/Graph.h>
+#include <rtabmap/core/Optimizer.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <ros/subscriber.h>
+#include <ros/publisher.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <boost/thread.hpp>
+
+using namespace rtabmap;
+
+class MapOptimizer
+{
+
+public:
+	MapOptimizer() :
+		mapFrameId_("map"),
+		odomFrameId_("odom"),
+		globalOptimization_(true),
+		optimizeFromLastNode_(false),
+		mapToOdom_(rtabmap::Transform::getIdentity()),
+		transformThread_(0)
+	{
+		ros::NodeHandle nh;
+		ros::NodeHandle pnh("~");
+
+		double epsilon = 0.0;
+		bool robust = true;
+		bool slam2d =false;
+		int strategy = 0; // 0=TORO, 1=g2o, 2=GTSAM
+		int iterations = 100;
+		bool ignoreVariance = false;
+
+		pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
+		pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
+		pnh.param("iterations", iterations, iterations);
+		pnh.param("ignore_variance", ignoreVariance, ignoreVariance);
+		pnh.param("global_optimization", globalOptimization_, globalOptimization_);
+		pnh.param("optimize_from_last_node", optimizeFromLastNode_, optimizeFromLastNode_);
+		pnh.param("epsilon", epsilon, epsilon);
+		pnh.param("robust", robust, robust);
+		pnh.param("slam_2d", slam2d, slam2d);
+		pnh.param("strategy", strategy, strategy);
+
+
+		UASSERT(iterations > 0);
+
+		ParametersMap parameters;
+		parameters.insert(ParametersPair(Parameters::kOptimizerStrategy(), uNumber2Str(strategy)));
+		parameters.insert(ParametersPair(Parameters::kOptimizerEpsilon(), uNumber2Str(epsilon)));
+		parameters.insert(ParametersPair(Parameters::kOptimizerIterations(), uNumber2Str(iterations)));
+		parameters.insert(ParametersPair(Parameters::kOptimizerRobust(), uBool2Str(robust)));
+		parameters.insert(ParametersPair(Parameters::kRegForce3DoF(), uBool2Str(slam2d)));
+		parameters.insert(ParametersPair(Parameters::kOptimizerVarianceIgnored(), uBool2Str(ignoreVariance)));
+		optimizer_ = Optimizer::create(parameters);
+
+		double tfDelay = 0.05; // 20 Hz
+		bool publishTf = true;
+		pnh.param("publish_tf", publishTf, publishTf);
+		pnh.param("tf_delay", tfDelay, tfDelay);
+
+		mapDataTopic_ = nh.subscribe("mapData", 1, &MapOptimizer::mapDataReceivedCallback, this);
+		mapDataPub_ = nh.advertise<rtabmap_msgs::MapData>(nh.resolveName("mapData")+"_optimized", 1);
+		mapGraphPub_ = nh.advertise<rtabmap_msgs::MapGraph>(nh.resolveName("mapData")+"Graph_optimized", 1);
+
+		if(publishTf)
+		{
+			ROS_INFO("map_optimizer will publish tf between frames \"%s\" and \"%s\"", mapFrameId_.c_str(), odomFrameId_.c_str());
+			ROS_INFO("map_optimizer: map_frame_id = %s", mapFrameId_.c_str());
+			ROS_INFO("map_optimizer: odom_frame_id = %s", odomFrameId_.c_str());
+			ROS_INFO("map_optimizer: tf_delay = %f", tfDelay);
+			transformThread_ = new boost::thread(boost::bind(&MapOptimizer::publishLoop, this, tfDelay));
+		}
+	}
+
+	~MapOptimizer()
+	{
+		if(transformThread_)
+		{
+			transformThread_->join();
+			delete transformThread_;
+		}
+	}
+
+	void publishLoop(double tfDelay)
+	{
+		if(tfDelay == 0)
+			return;
+		ros::Rate r(1.0 / tfDelay);
+		while(ros::ok())
+		{
+			mapToOdomMutex_.lock();
+			ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfDelay);
+			geometry_msgs::TransformStamped msg;
+			msg.child_frame_id = odomFrameId_;
+			msg.header.frame_id = mapFrameId_;
+			msg.header.stamp = tfExpiration;
+			rtabmap_conversions::transformToGeometryMsg(mapToOdom_, msg.transform);
+			tfBroadcaster_.sendTransform(msg);
+			mapToOdomMutex_.unlock();
+			r.sleep();
+		}
+	}
+
+	void mapDataReceivedCallback(const rtabmap_msgs::MapDataConstPtr & msg)
+	{
+		// save new poses and constraints
+		// Assuming that nodes/constraints are all linked together
+		UASSERT(msg->graph.posesId.size() == msg->graph.poses.size());
+
+		bool dataChanged = false;
+
+		std::multimap<int, Link> newConstraints;
+		for(unsigned int i=0; i<msg->graph.links.size(); ++i)
+		{
+			Link link = rtabmap_conversions::linkFromROS(msg->graph.links[i]);
+			newConstraints.insert(std::make_pair(link.from(), link));
+
+			bool edgeAlreadyAdded = false;
+			for(std::multimap<int, Link>::iterator iter = cachedConstraints_.lower_bound(link.from());
+					iter != cachedConstraints_.end() && iter->first == link.from();
+					++iter)
+			{
+				if(iter->second.to() == link.to())
+				{
+					edgeAlreadyAdded = true;
+					if(iter->second.transform().getDistanceSquared(link.transform()) > 0.0001)
+					{
+						ROS_WARN("%d ->%d (%s vs %s)",iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str(),
+								link.transform().prettyPrint().c_str());
+						dataChanged = true;
+					}
+				}
+			}
+			if(!edgeAlreadyAdded)
+			{
+				cachedConstraints_.insert(std::make_pair(link.from(), link));
+			}
+		}
+
+		std::map<int, Signature> newNodeInfos;
+		// add new odometry poses
+		for(unsigned int i=0; i<msg->nodes.size(); ++i)
+		{
+			int id = msg->nodes[i].id;
+			Transform pose = rtabmap_conversions::transformFromPoseMsg(msg->nodes[i].pose);
+			Signature s = rtabmap_conversions::nodeInfoFromROS(msg->nodes[i]);
+			newNodeInfos.insert(std::make_pair(id, s));
+
+			std::pair<std::map<int, Signature>::iterator, bool> p = cachedNodeInfos_.insert(std::make_pair(id, s));
+			if(!p.second && pose.getDistanceSquared(cachedNodeInfos_.at(id).getPose()) > 0.0001)
+			{
+				dataChanged = true;
+			}
+		}
+
+		if(dataChanged)
+		{
+			ROS_WARN("Graph data has changed! Reset cache...");
+			cachedConstraints_ = newConstraints;
+			cachedNodeInfos_ = newNodeInfos;
+		}
+
+		//match poses in the graph
+		std::multimap<int, Link> constraints;
+		std::map<int, Signature> nodeInfos;
+		if(globalOptimization_)
+		{
+			constraints = cachedConstraints_;
+			nodeInfos = cachedNodeInfos_;
+		}
+		else
+		{
+			constraints = newConstraints;
+			for(unsigned int i=0; i<msg->graph.posesId.size(); ++i)
+			{
+				std::map<int, Signature>::iterator iter = cachedNodeInfos_.find(msg->graph.posesId[i]);
+				if(iter != cachedNodeInfos_.end())
+				{
+					nodeInfos.insert(*iter);
+				}
+				else
+				{
+					ROS_ERROR("Odometry pose of node %d not found in cache!", msg->graph.posesId[i]);
+					return;
+				}
+			}
+		}
+
+		std::map<int, Transform> poses;
+		for(std::map<int, Signature>::iterator iter=nodeInfos.begin(); iter!=nodeInfos.end(); ++iter)
+		{
+			poses.insert(std::make_pair(iter->first, iter->second.getPose()));
+		}
+
+		// Optimize only if there is a subscriber
+		if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
+		{
+			UTimer timer;
+			std::map<int, Transform> optimizedPoses;
+			Transform mapCorrection = Transform::getIdentity();
+			std::map<int, rtabmap::Transform> posesOut;
+			std::multimap<int, rtabmap::Link> linksOut;
+			if(poses.size() > 1 && constraints.size() > 0)
+			{
+				int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first;
+				optimizer_->getConnectedGraph(
+						fromId,
+						poses,
+						constraints,
+						posesOut,
+						linksOut);
+				optimizedPoses = optimizer_->optimize(fromId, posesOut, linksOut);
+				mapToOdomMutex_.lock();
+				mapCorrection = optimizedPoses.at(posesOut.rbegin()->first) * posesOut.rbegin()->second.inverse();
+				mapToOdom_ = mapCorrection;
+				mapToOdomMutex_.unlock();
+			}
+			else if(poses.size() == 1 && constraints.size() == 0)
+			{
+				optimizedPoses = poses;
+			}
+			else if(poses.size() == 0 && constraints.size())
+			{
+				ROS_ERROR("map_optimizer: Poses=%d and edges=%d: poses must "
+					   "not be null if there are edges.",
+					  (int)poses.size(), (int)constraints.size());
+			}
+
+			rtabmap_msgs::MapData outputDataMsg;
+			rtabmap_msgs::MapGraph outputGraphMsg;
+			rtabmap_conversions::mapGraphToROS(optimizedPoses,
+					linksOut,
+					mapCorrection,
+					outputGraphMsg);
+
+			if(mapGraphPub_.getNumSubscribers())
+			{
+				outputGraphMsg.header = msg->header;
+				mapGraphPub_.publish(outputGraphMsg);
+			}
+
+			if(mapDataPub_.getNumSubscribers())
+			{
+				outputDataMsg.header = msg->header;
+				outputDataMsg.graph = outputGraphMsg;
+				outputDataMsg.nodes = msg->nodes;
+				if(posesOut.size() > msg->nodes.size())
+				{
+					std::set<int> addedNodes;
+					for(unsigned int i=0; i<msg->nodes.size(); ++i)
+					{
+						addedNodes.insert(msg->nodes[i].id);
+					}
+					std::list<int> toAdd;
+					for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
+					{
+						if(addedNodes.find(iter->first) == addedNodes.end())
+						{
+							toAdd.push_back(iter->first);
+						}
+					}
+					if(toAdd.size())
+					{
+						int oi = outputDataMsg.nodes.size();
+						outputDataMsg.nodes.resize(outputDataMsg.nodes.size()+toAdd.size());
+						for(std::list<int>::iterator iter=toAdd.begin(); iter!=toAdd.end(); ++iter)
+						{
+							UASSERT(cachedNodeInfos_.find(*iter) != cachedNodeInfos_.end());
+							rtabmap_conversions::nodeDataToROS(cachedNodeInfos_.at(*iter), outputDataMsg.nodes[oi]);
+							++oi;
+						}
+					}
+				}
+				mapDataPub_.publish(outputDataMsg);
+			}
+
+			ROS_INFO("Time graph optimization = %f s", timer.ticks());
+		}
+	}
+
+private:
+	std::string mapFrameId_;
+	std::string odomFrameId_;
+	bool globalOptimization_;
+	bool optimizeFromLastNode_;
+	Optimizer * optimizer_;
+
+	rtabmap::Transform mapToOdom_;
+	boost::mutex mapToOdomMutex_;
+
+	ros::Subscriber mapDataTopic_;
+
+	ros::Publisher mapDataPub_;
+	ros::Publisher mapGraphPub_;
+
+	std::multimap<int, Link> cachedConstraints_;
+	std::map<int, Signature> cachedNodeInfos_;
+
+	tf2_ros::TransformBroadcaster tfBroadcaster_;
+	boost::thread* transformThread_;
+};
+
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "map_optimizer");
+	MapOptimizer optimizer;
+	ros::spin();
+	return 0;
+}

rtabmap_util/src/OdomMsgToTFNode.cpp

@@ -0,0 +1,92 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <rtabmap_conversions/MsgConversion.h>
+
+class OdomMsgToTF
+{
+
+public:
+	OdomMsgToTF() :
+		frameId_(""),
+		odomFrameId_("")
+	{
+		ros::NodeHandle pnh("~");
+		pnh.param("frame_id", frameId_, frameId_);
+		pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
+
+		ros::NodeHandle nh;
+		odomTopic_ = nh.subscribe("odom", 1, &OdomMsgToTF::odomReceivedCallback, this);
+	}
+
+	virtual ~OdomMsgToTF(){}
+
+	void odomReceivedCallback(const nav_msgs::OdometryConstPtr & msg)
+	{
+		if(frameId_.empty())
+		{
+			frameId_ = msg->child_frame_id;
+		}
+		if(odomFrameId_.empty())
+		{
+			odomFrameId_ = msg->header.frame_id;
+		}
+		geometry_msgs::TransformStamped t;
+		rtabmap::Transform pose = rtabmap_conversions::transformFromPoseMsg(msg->pose.pose);
+		if(pose.isNull())
+		{
+			ROS_WARN("Odometry received is null! Cannot send tf...");
+		}
+		else
+		{
+			t.child_frame_id = frameId_;
+			t.header.frame_id = odomFrameId_;
+			t.header.stamp = msg->header.stamp;
+			rtabmap_conversions::transformToGeometryMsg(pose, t.transform);
+			tfBroadcaster_.sendTransform(t);
+		}
+	}
+
+private:
+	std::string frameId_;
+	std::string odomFrameId_;
+
+	ros::Subscriber odomTopic_;
+	tf2_ros::TransformBroadcaster tfBroadcaster_;
+};
+
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "odom_msg_to_tf");
+	OdomMsgToTF odomToTf;
+	ros::spin();
+	return 0;
+}

rtabmap_util/src/PointCloudAggregatorNode.cpp

@@ -0,0 +1,42 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "point_cloud_aggregator");
+
+	nodelet::Loader nodelet;
+	nodelet::V_string nargv;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_util/point_cloud_aggregator", remap, nargv);
+	ros::spin();
+	return 0;
+}

rtabmap_util/src/PointCloudAssemblerNode.cpp

@@ -0,0 +1,44 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "point_cloud_assembler");
+
+	nodelet::Loader nodelet;
+	nodelet::V_string nargv;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_util/point_cloud_assembler", remap, nargv);
+	ros::spin();
+	return 0;
+}
+
+

rtabmap_util/src/PointCloudToDepthImageNode.cpp

@@ -0,0 +1,44 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "pointcloud_to_depthimage");
+
+	nodelet::Loader nodelet;
+	nodelet::V_string nargv;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_util/pointcloud_to_depthimage", remap, nargv);
+	ros::spin();
+	return 0;
+}
+
+

rtabmap_util/src/RGBDRelayNode.cpp

@@ -0,0 +1,47 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "rgbd_relay");
+
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		nargv.push_back(argv[i]);
+	}
+
+	nodelet::Loader nodelet;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_util/rgbd_relay", remap, nargv);
+	ros::spin();
+	return 0;
+}

rtabmap_util/src/RGBDSplitNode.cpp

@@ -0,0 +1,47 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "rgbd_split");
+
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		nargv.push_back(argv[i]);
+	}
+
+	nodelet::Loader nodelet;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_util/rgbd_split", remap, nargv);
+	ros::spin();
+	return 0;
+}

rtabmap_util/src/nodelets/disparity_to_depth.cpp

@@ -0,0 +1,140 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <stereo_msgs/DisparityImage.h>
+
+#include <image_transport/image_transport.h>
+
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_util
+{
+
+class DisparityToDepth : public nodelet::Nodelet
+{
+public:
+	DisparityToDepth() {}
+
+	virtual ~DisparityToDepth(){}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		image_transport::ImageTransport it(nh);
+		pub32f_ = it.advertise("depth", 1);
+		pub16u_ = it.advertise("depth_raw", 1);
+		sub_ = nh.subscribe("disparity", 1, &DisparityToDepth::callback, this);
+	}
+
+	void callback(const stereo_msgs::DisparityImageConstPtr& disparityMsg)
+	{
+		if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0)
+		{
+			NODELET_ERROR("Input type must be disparity=32FC1");
+			return;
+		}
+
+		bool publish32f = pub32f_.getNumSubscribers();
+		bool publish16u = pub16u_.getNumSubscribers();
+
+		if(publish32f || publish16u)
+		{
+			// sensor_msgs::image_encodings::TYPE_32FC1
+			cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
+
+			cv::Mat depth32f;
+			cv::Mat depth16u;
+			if(publish32f)
+			{
+				depth32f = cv::Mat::zeros(disparity.rows, disparity.cols, CV_32F);
+			}
+			if(publish16u)
+			{
+				depth16u = cv::Mat::zeros(disparity.rows, disparity.cols, CV_16U);
+			}
+			for (int i = 0; i < disparity.rows; i++)
+			{
+				for (int j = 0; j < disparity.cols; j++)
+				{
+					float disparity_value = disparity.at<float>(i,j);
+					if (disparity_value > disparityMsg->min_disparity && disparity_value < disparityMsg->max_disparity)
+					{
+						// baseline * focal / disparity
+						float depth = disparityMsg->T * disparityMsg->f / disparity_value;
+						if(publish32f)
+						{
+							depth32f.at<float>(i,j) = depth;
+						}
+						if(publish16u)
+						{
+							depth16u.at<unsigned short>(i,j) = (unsigned short)(depth*1000.0f);
+						}
+					}
+				}
+			}
+
+			if(publish32f)
+			{
+				// convert to ROS sensor_msg::Image
+				cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_32FC1, depth32f);
+				sensor_msgs::Image depthMsg;
+				cvDepth.toImageMsg(depthMsg);
+
+				//publish the message
+				pub32f_.publish(depthMsg);
+			}
+
+			if(publish16u)
+			{
+				// convert to ROS sensor_msg::Image
+				cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_16UC1, depth16u);
+				sensor_msgs::Image depthMsg;
+				cvDepth.toImageMsg(depthMsg);
+
+				//publish the message
+				pub16u_.publish(depthMsg);
+			}
+		}
+}
+
+private:
+	image_transport::Publisher pub32f_;
+	image_transport::Publisher pub16u_;
+	ros::Subscriber sub_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_util::DisparityToDepth, nodelet::Nodelet);
+}

rtabmap_util/src/nodelets/imu_to_tf.cpp

@@ -0,0 +1,122 @@
+/*
+Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+#include <sensor_msgs/Imu.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/LinearMath/Matrix3x3.h>
+#include <tf/transform_listener.h>
+
+namespace rtabmap_util
+{
+
+class ImuToTF : public nodelet::Nodelet
+{
+public:
+	ImuToTF() :
+		fixedFrameId_("odom"),
+		waitForTransformDuration_(0.1)
+	{}
+
+	virtual ~ImuToTF()
+	{
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
+		pnh.param("base_frame_id", baseFrameId_, baseFrameId_);
+		pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
+		NODELET_INFO("fixed_frame_id: %s", fixedFrameId_.c_str());
+		NODELET_INFO("base_frame_id: %s", baseFrameId_.c_str());
+
+		sub_ = nh.subscribe<sensor_msgs::Imu>("imu/data", 1, &ImuToTF::imuCallback, this);
+	}
+
+	void imuCallback(const sensor_msgs::ImuConstPtr & msg)
+	{
+		tf::Quaternion q;
+		tf::quaternionMsgToTF(msg->orientation, q);
+		tf::StampedTransform st;
+		st.setRotation(q);
+
+		st.frame_id_ = fixedFrameId_;
+		st.stamp_ = msg->header.stamp;
+
+		if(!baseFrameId_.empty() &&
+			baseFrameId_.compare(msg->header.frame_id) != 0)
+		{
+			try
+			{
+				std::string errorMsg;
+				if(!tfListener_.waitForTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp, ros::Duration(waitForTransformDuration_), ros::Duration(0.01), &errorMsg))
+				{
+					NODELET_ERROR("Could not get transform from %s to %s after %f seconds (for stamp=%f)! Error=\"%s\".",
+							baseFrameId_.c_str(), msg->header.frame_id.c_str(), 0.1, msg->header.stamp.toSec(), errorMsg.c_str());
+					return;
+				}
+
+				tf::StampedTransform tmp;
+				tfListener_.lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp, tmp);
+				tf::Transform t = tmp.inverse()*st*tmp;
+				st.setRotation(t.getRotation());
+				st.child_frame_id_ = baseFrameId_;
+			}
+			catch(tf::TransformException & ex)
+			{
+				NODELET_ERROR("(getting transform %s -> %s) %s", baseFrameId_.c_str(), msg->header.frame_id.c_str(), ex.what());
+				return;
+			}
+		}
+		else
+		{
+			st.child_frame_id_ = msg->header.frame_id;
+		}
+		st.setOrigin(tf::Vector3(0,0,0));
+
+		pub_.sendTransform(st);
+	}
+
+private:
+	ros::Subscriber sub_;
+	tf::TransformBroadcaster pub_;
+	std::string fixedFrameId_;
+	std::string baseFrameId_;
+	tf::TransformListener tfListener_;
+	double waitForTransformDuration_;
+};
+
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_util::ImuToTF, nodelet::Nodelet);
+}
+

rtabmap_util/src/nodelets/lidar_deskewing.cpp

@@ -0,0 +1,121 @@
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+
+#include <tf/transform_listener.h>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+
+#include <laser_geometry/laser_geometry.h>
+
+#include <pcl_ros/transforms.h>
+
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap_conversions/MsgConversion.h>
+
+namespace rtabmap_util
+{
+
+class LidarDeskewing : public nodelet::Nodelet
+{
+public:
+	LidarDeskewing() :
+		waitForTransformDuration_(0.01),
+		slerp_(false),
+		tfListener_(0)
+	{
+	}
+
+	virtual ~LidarDeskewing()
+	{
+	}
+
+private:
+	virtual void onInit()
+	{
+		tfListener_ = new tf::TransformListener();
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
+		pnh.param("wait_for_transform", waitForTransformDuration_, waitForTransformDuration_);
+		pnh.param("slerp", slerp_, slerp_);
+
+		NODELET_INFO("fixed_frame_id:  %s", fixedFrameId_.c_str());
+		NODELET_INFO("wait_for_transform:  %fs", waitForTransformDuration_);
+		NODELET_INFO("slerp:  %s", slerp_?"true":"false");
+
+		if(fixedFrameId_.empty())
+		{
+			NODELET_FATAL("fixed_frame_id parameter cannot be empty!");
+		}
+
+		pubScan_ = nh.advertise<sensor_msgs::PointCloud2>(nh.resolveName("input_scan") + "/deskewed", 1);
+		pubCloud_ = nh.advertise<sensor_msgs::PointCloud2>(nh.resolveName("input_cloud") + "/deskewed", 1);
+		subScan_ = nh.subscribe("input_scan", 1, &LidarDeskewing::callbackScan, this);
+		subCloud_ = nh.subscribe("input_cloud", 1, &LidarDeskewing::callbackCloud, this);
+	}
+
+	void callbackScan(const sensor_msgs::LaserScanConstPtr & msg)
+	{
+		// make sure the frame of the laser is updated during the whole scan time
+		rtabmap::Transform tmpT = rtabmap_conversions::getTransform(
+				msg->header.frame_id,
+				fixedFrameId_,
+				msg->header.stamp,
+				msg->header.stamp + ros::Duration().fromSec(msg->ranges.size()*msg->time_increment),
+				*tfListener_,
+				waitForTransformDuration_);
+		if(tmpT.isNull())
+		{
+			return;
+		}
+
+		sensor_msgs::PointCloud2 scanOut;
+		laser_geometry::LaserProjection projection;
+		projection.transformLaserScanToPointCloud(fixedFrameId_, *msg, scanOut, *tfListener_);
+
+		sensor_msgs::PointCloud2 scanOutDeskewed;
+		if(!pcl_ros::transformPointCloud(msg->header.frame_id, scanOut, scanOutDeskewed, *tfListener_))
+		{
+			ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
+					fixedFrameId_.c_str(), msg->header.frame_id.c_str(), msg->header.stamp.toSec());
+			return;
+		}
+		pubScan_.publish(scanOutDeskewed);
+	}
+
+	void callbackCloud(const sensor_msgs::PointCloud2ConstPtr & msg)
+	{
+		sensor_msgs::PointCloud2 msgDeskewed;
+		if(rtabmap_conversions::deskew(*msg, msgDeskewed, fixedFrameId_, *tfListener_, waitForTransformDuration_, slerp_))
+		{
+			pubCloud_.publish(msgDeskewed);
+		}
+		else
+		{
+			// Just republish the msg to not breakdown downstream
+			// A warning should be already shown (see deskew() source code)
+			ROS_WARN("deskewing failed! returning possible skewed cloud!");
+			pubCloud_.publish(msg);
+		}
+	}
+
+private:
+	ros::Publisher pubScan_;
+	ros::Publisher pubCloud_;
+	ros::Subscriber subScan_;
+	ros::Subscriber subCloud_;
+	std::string fixedFrameId_;
+	double waitForTransformDuration_;
+	bool slerp_;
+	tf::TransformListener * tfListener_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_util::LidarDeskewing, nodelet::Nodelet);
+}
+
+

rtabmap_util/src/nodelets/obstacles_detection.cpp

@@ -0,0 +1,460 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/filters/filter.h>
+
+#include <tf/transform_listener.h>
+
+#include <sensor_msgs/PointCloud2.h>
+
+#include <rtabmap_conversions/MsgConversion.h>
+
+#include "rtabmap/core/OccupancyGrid.h"
+#include "rtabmap/utilite/UStl.h"
+
+namespace rtabmap_util
+{
+
+class ObstaclesDetection : public nodelet::Nodelet
+{
+public:
+	ObstaclesDetection() :
+		frameId_("base_link"),
+		waitForTransform_(false),
+		mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()),
+		warned_(false)
+	{}
+
+	virtual ~ObstaclesDetection()
+	{}
+
+private:
+
+	void parameterMoved(
+			ros::NodeHandle & nh,
+			const std::string & rosName,
+			const std::string & parameterName,
+			rtabmap::ParametersMap & parameters)
+	{
+		if(nh.hasParam(rosName))
+		{
+			rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid");
+			rtabmap::ParametersMap::const_iterator iter =gridParameters.find(parameterName);
+			if(iter != gridParameters.end())
+			{
+				NODELET_ERROR("obstacles_detection: Parameter \"%s\" has moved from "
+						 "rtabmap_conversions to rtabmap library. Use "
+						 "parameter \"%s\" instead. The value is still "
+						 "copied to new parameter name.",
+						 rosName.c_str(),
+						 parameterName.c_str());
+				std::string type = rtabmap::Parameters::getType(parameterName);
+				if(type.compare("float") || type.compare("double"))
+				{
+					double v = uStr2Double(iter->second);
+					nh.getParam(rosName, v);
+					parameters.insert(rtabmap::ParametersPair(parameterName, uNumber2Str(v)));
+				}
+				else if(type.compare("int") || type.compare("unsigned int"))
+				{
+					int v = uStr2Int(iter->second);
+					nh.getParam(rosName, v);
+					parameters.insert(rtabmap::ParametersPair(parameterName, uNumber2Str(v)));
+				}
+				else
+				{
+					NODELET_ERROR("Not handled type \"%s\" for parameter \"%s\"", type.c_str(), parameterName.c_str());
+				}
+			}
+			else
+			{
+				NODELET_ERROR("Parameter \"%s\" not found in default parameters.", parameterName.c_str());
+			}
+		}
+	}
+
+	virtual void onInit()
+	{
+		ROS_DEBUG("_"); // not sure why, but all NODELET_*** log are not shown if a normal ROS_*** is not called!?
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		ULogger::setType(ULogger::kTypeConsole);
+		ULogger::setLevel(ULogger::kWarning);
+
+		int queueSize = 10;
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("frame_id", frameId_, frameId_);
+		pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
+		pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
+
+		if(pnh.hasParam("optimize_for_close_objects"))
+		{
+			NODELET_ERROR("\"optimize_for_close_objects\" parameter doesn't exist "
+					"anymore. Use rtabmap_conversions/obstacles_detection_old nodelet to use "
+					"the old interface.");
+		}
+
+		rtabmap::ParametersMap parameters;
+
+		// Backward compatibility
+		for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=rtabmap::Parameters::getRemovedParameters().begin();
+			iter!=rtabmap::Parameters::getRemovedParameters().end();
+			++iter)
+		{
+			std::string vStr;
+			bool vBool;
+			int vInt;
+			double vDouble;
+			std::string paramValue;
+			if(pnh.getParam(iter->first, vStr))
+			{
+				paramValue = vStr;
+			}
+			else if(pnh.getParam(iter->first, vBool))
+			{
+				paramValue = uBool2Str(vBool);
+			}
+			else if(pnh.getParam(iter->first, vDouble))
+			{
+				paramValue = uNumber2Str(vDouble);
+			}
+			else if(pnh.getParam(iter->first, vInt))
+			{
+				paramValue = uNumber2Str(vInt);
+			}
+			if(!paramValue.empty())
+			{
+				if(iter->second.first)
+				{
+					// can be migrated
+					uInsert(parameters, rtabmap::ParametersPair(iter->second.second, paramValue));
+					NODELET_ERROR("obstacles_detection: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
+							iter->first.c_str(), iter->second.second.c_str(), paramValue.c_str());
+				}
+				else
+				{
+					if(iter->second.second.empty())
+					{
+						NODELET_ERROR("obstacles_detection: Parameter \"%s\" doesn't exist anymore!",
+								iter->first.c_str());
+					}
+					else
+					{
+						NODELET_ERROR("obstacles_detection: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
+								iter->first.c_str(), iter->second.second.c_str());
+					}
+				}
+			}
+		}
+
+		rtabmap::ParametersMap gridParameters2 = rtabmap::Parameters::getDefaultParameters();
+		rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid");
+		for(rtabmap::ParametersMap::iterator iter=gridParameters.begin(); iter!=gridParameters.end(); ++iter)
+		{
+			std::string vStr;
+			bool vBool;
+			int vInt;
+			double vDouble;
+			if(pnh.getParam(iter->first, vStr))
+			{
+				NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
+				iter->second = vStr;
+			}
+			else if(pnh.getParam(iter->first, vBool))
+			{
+				NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
+				iter->second = uBool2Str(vBool);
+			}
+			else if(pnh.getParam(iter->first, vDouble))
+			{
+				NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
+				iter->second = uNumber2Str(vDouble);
+			}
+			else if(pnh.getParam(iter->first, vInt))
+			{
+				NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
+				iter->second = uNumber2Str(vInt);
+			}
+		}
+		uInsert(parameters, gridParameters);
+		parameterMoved(pnh, "proj_voxel_size", rtabmap::Parameters::kGridCellSize(), parameters);
+		parameterMoved(pnh, "ground_normal_angle", rtabmap::Parameters::kGridMaxGroundAngle(), parameters);
+		parameterMoved(pnh, "min_cluster_size", rtabmap::Parameters::kGridMinClusterSize(), parameters);
+		parameterMoved(pnh, "normal_estimation_radius", rtabmap::Parameters::kGridClusterRadius(), parameters);
+		parameterMoved(pnh, "cluster_radius", rtabmap::Parameters::kGridClusterRadius(), parameters);
+		parameterMoved(pnh, "max_obstacles_height", rtabmap::Parameters::kGridMaxObstacleHeight(), parameters);
+		parameterMoved(pnh, "max_ground_height", rtabmap::Parameters::kGridMaxGroundHeight(), parameters);
+		parameterMoved(pnh, "detect_flat_obstacles", rtabmap::Parameters::kGridFlatObstacleDetected(), parameters);
+		parameterMoved(pnh, "normal_k", rtabmap::Parameters::kGridNormalK(), parameters);
+
+		UASSERT(uContains(parameters, rtabmap::Parameters::kGridMapFrameProjection()));
+		mapFrameProjection_ = uStr2Bool(parameters.at(rtabmap::Parameters::kGridMapFrameProjection()));
+		if(mapFrameProjection_ && mapFrameId_.empty())
+		{
+			NODELET_ERROR("obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str());
+		}
+
+		grid_.parseParameters(parameters);
+
+		cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this);
+
+		groundPub_ = nh.advertise<sensor_msgs::PointCloud2>("ground", 1);
+		obstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles", 1);
+		projObstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("proj_obstacles", 1);
+	}
+
+
+
+	void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
+	{
+		ros::WallTime time = ros::WallTime::now();
+
+		if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0 && projObstaclesPub_.getNumSubscribers() == 0)
+		{
+			// no one wants the results
+			return;
+		}
+
+		rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
+		try
+		{
+			if(waitForTransform_)
+			{
+				if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
+				{
+					NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
+					return;
+				}
+			}
+			tf::StampedTransform tmp;
+			tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
+			localTransform = rtabmap_conversions::transformFromTF(tmp);
+		}
+		catch(tf::TransformException & ex)
+		{
+			NODELET_ERROR("%s",ex.what());
+			return;
+		}
+
+		rtabmap::Transform pose = rtabmap::Transform::getIdentity();
+		if(!mapFrameId_.empty())
+		{
+			try
+			{
+				if(waitForTransform_)
+				{
+					if(!tfListener_.waitForTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, ros::Duration(1)))
+					{
+						NODELET_ERROR("Could not get transform from %s to %s after 1 second!", mapFrameId_.c_str(), frameId_.c_str());
+						return;
+					}
+				}
+				tf::StampedTransform tmp;
+				tfListener_.lookupTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, tmp);
+				pose = rtabmap_conversions::transformFromTF(tmp);
+			}
+			catch(tf::TransformException & ex)
+			{
+				NODELET_ERROR("%s",ex.what());
+				return;
+			}
+		}
+
+		UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
+				uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::fromROSMsg(*cloudMsg, *inputCloud);
+		if(inputCloud->isOrganized())
+		{
+			std::vector<int> indices;
+			pcl::removeNaNFromPointCloud(*inputCloud, *inputCloud, indices);
+		}
+		else if(!inputCloud->is_dense && inputCloud->height == 1)
+		{
+			if(!warned_)
+			{
+				NODELET_WARN("Detected possible wrong format of point cloud \"%s\", it is "
+						"indicated that it is not dense, but there is only one row. "
+						"Assuming it is dense... This message will only appear once.", cloudSub_.getTopic().c_str());
+				warned_ = true;
+			}
+			inputCloud->is_dense = true;
+		}
+
+		//Common variables for all strategies
+		pcl::IndicesPtr ground, obstacles;
+		pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud<pcl::PointXYZ>);
+
+		if(inputCloud->size())
+		{
+			inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform);
+
+			pcl::IndicesPtr flatObstacles(new std::vector<int>);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = grid_.segmentCloud<pcl::PointXYZ>(
+					inputCloud,
+					pcl::IndicesPtr(new std::vector<int>),
+					pose,
+					cv::Point3f(localTransform.x(), localTransform.y(), localTransform.z()),
+					ground,
+					obstacles,
+					&flatObstacles);
+
+			if(cloud->size() && ((ground.get() && ground->size()) || (obstacles.get() && obstacles->size())))
+			{
+				if(groundPub_.getNumSubscribers() &&
+						ground.get() && ground->size())
+				{
+					pcl::copyPointCloud(*cloud, *ground, *groundCloud);
+				}
+
+				if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) &&
+						obstacles.get() && obstacles->size())
+				{
+					// remove flat obstacles from obstacles
+					std::set<int> flatObstaclesSet;
+					if(projObstaclesPub_.getNumSubscribers())
+					{
+						flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end());
+					}
+
+					obstaclesCloud->resize(obstacles->size());
+					obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size());
+
+					int oi=0;
+					for(unsigned int i=0; i<obstacles->size(); ++i)
+					{
+						obstaclesCloud->points[i] = cloud->at(obstacles->at(i));
+						if(flatObstaclesSet.size() == 0 ||
+						   flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
+						{
+							obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
+							obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0;
+							++oi;
+						}
+
+					}
+
+					obstaclesCloudWithoutFlatSurfaces->resize(oi);
+				}
+
+				if(!localTransform.isIdentity() || !pose.isIdentity())
+				{
+					//transform back in topic frame for 3d clouds and base frame for 2d clouds
+
+					float roll, pitch, yaw;
+					pose.getEulerAngles(roll, pitch, yaw);
+					rtabmap::Transform t = rtabmap::Transform(0,0, mapFrameProjection_?pose.z():0, roll, pitch, 0);
+
+					if(obstaclesCloudWithoutFlatSurfaces->size() && !pose.isIdentity())
+					{
+						obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::transformPointCloud(obstaclesCloudWithoutFlatSurfaces, t.inverse());
+					}
+
+					t = (t*localTransform).inverse();
+					if(groundCloud->size())
+					{
+						groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, t);
+					}
+					if(obstaclesCloud->size())
+					{
+						obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, t);
+					}
+				}
+			}
+		}
+		else
+		{
+			ROS_WARN("obstacles_detection: Input cloud is empty! (%d x %d, is_dense=%d)", cloudMsg->width, cloudMsg->height, cloudMsg->is_dense?1:0);
+		}
+
+		if(groundPub_.getNumSubscribers())
+		{
+			sensor_msgs::PointCloud2 rosCloud;
+			pcl::toROSMsg(*groundCloud, rosCloud);
+			rosCloud.header = cloudMsg->header;
+
+			//publish the message
+			groundPub_.publish(rosCloud);
+		}
+
+		if(obstaclesPub_.getNumSubscribers())
+		{
+			sensor_msgs::PointCloud2 rosCloud;
+			pcl::toROSMsg(*obstaclesCloud, rosCloud);
+			rosCloud.header = cloudMsg->header;
+
+			//publish the message
+			obstaclesPub_.publish(rosCloud);
+		}
+
+		if(projObstaclesPub_.getNumSubscribers())
+		{
+			sensor_msgs::PointCloud2 rosCloud;
+			pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, rosCloud);
+			rosCloud.header.stamp = cloudMsg->header.stamp;
+			rosCloud.header.frame_id = frameId_;
+
+			//publish the message
+			projObstaclesPub_.publish(rosCloud);
+		}
+
+		NODELET_DEBUG("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec());
+	}
+
+private:
+	std::string frameId_;
+	std::string mapFrameId_;
+	bool waitForTransform_;
+
+	rtabmap::OccupancyGrid grid_;
+	bool mapFrameProjection_;
+	bool warned_;
+
+	tf::TransformListener tfListener_;
+
+	ros::Publisher groundPub_;
+	ros::Publisher obstaclesPub_;
+	ros::Publisher projObstaclesPub_;
+
+	ros::Subscriber cloudSub_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_util::ObstaclesDetection, nodelet::Nodelet);
+}
+
+

rtabmap_util/src/nodelets/point_cloud_aggregator.cpp

@@ -0,0 +1,463 @@
+/*
+Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/io/pcd_io.h>
+
+#include <pcl_ros/transforms.h>
+
+#include <tf/transform_listener.h>
+
+#include <sensor_msgs/PointCloud2.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 <message_filters/sync_policies/exact_time.h>
+
+#include <rtabmap_conversions/MsgConversion.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/util3d_filtering.h>
+
+namespace rtabmap_util
+{
+
+/**
+ * Nodelet used to merge point clouds from different sensors into a single
+ * assembled cloud. If fixed_frame_id is set and approx_sync is true,
+ * the clouds are adjusted to include the displacement of the robot
+ * in the output cloud.
+ */
+class PointCloudAggregator : public nodelet::Nodelet
+{
+public:
+	PointCloudAggregator() :
+		warningThread_(0),
+		callbackCalled_(false),
+		exactSync4_(0),
+		approxSync4_(0),
+		exactSync3_(0),
+		approxSync3_(0),
+		exactSync2_(0),
+		approxSync2_(0),
+		waitForTransformDuration_(0.1),
+		xyzOutput_(false)
+	{}
+
+	virtual ~PointCloudAggregator()
+	{
+	    delete exactSync4_;
+	    delete approxSync4_;
+	    delete exactSync3_;
+	    delete approxSync3_;
+	    delete exactSync2_;
+	    delete approxSync2_;
+
+	    if(warningThread_)
+		{
+			callbackCalled_=true;
+			warningThread_->join();
+			delete warningThread_;
+		}
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 5;
+		int count = 2;
+		bool approx=true;
+		double approxSyncMaxInterval = 0.0;
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("frame_id", frameId_, frameId_);
+		pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
+		pnh.param("approx_sync", approx, approx);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
+		pnh.param("count", count, count);
+		pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
+		pnh.param("xyz_output", xyzOutput_, xyzOutput_);
+
+		cloudSub_1_.subscribe(nh, "cloud1", 1);
+		cloudSub_2_.subscribe(nh, "cloud2", 1);
+
+		std::string subscribedTopicsMsg;
+		if(count == 4)
+		{
+			cloudSub_3_.subscribe(nh, "cloud3", 1);
+			cloudSub_4_.subscribe(nh, "cloud4", 1);
+			if(approx)
+			{
+				approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
+				if(approxSyncMaxInterval > 0.0)
+					approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+				approxSync4_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+			}
+			else
+			{
+				exactSync4_ = new message_filters::Synchronizer<ExactSync4Policy>(ExactSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
+				exactSync4_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+			}
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s,\n   %s",
+					getName().c_str(),
+					approx?"approx":"exact",
+					approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+					cloudSub_1_.getTopic().c_str(),
+					cloudSub_2_.getTopic().c_str(),
+					cloudSub_3_.getTopic().c_str(),
+					cloudSub_4_.getTopic().c_str());
+		}
+		else if(count == 3)
+		{
+			cloudSub_3_.subscribe(nh, "cloud3", 1);
+			if(approx)
+			{
+				approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
+				if(approxSyncMaxInterval > 0.0)
+					approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+				approxSync3_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+			}
+			else
+			{
+				exactSync3_ = new message_filters::Synchronizer<ExactSync3Policy>(ExactSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
+				exactSync3_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+			}
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s",
+					getName().c_str(),
+					approx?"approx":"exact",
+					approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+					cloudSub_1_.getTopic().c_str(),
+					cloudSub_2_.getTopic().c_str(),
+					cloudSub_3_.getTopic().c_str());
+		}
+		else
+		{
+			if(approx)
+			{
+				approxSync2_ = new message_filters::Synchronizer<ApproxSync2Policy>(ApproxSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
+				if(approxSyncMaxInterval > 0.0)
+					approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+				approxSync2_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, boost::placeholders::_1, boost::placeholders::_2));
+			}
+			else
+			{
+				exactSync2_ = new message_filters::Synchronizer<ExactSync2Policy>(ExactSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
+				exactSync2_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, boost::placeholders::_1, boost::placeholders::_2));
+			}
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s",
+					getName().c_str(),
+					approx?"approx":"exact",
+					approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+					cloudSub_1_.getTopic().c_str(),
+					cloudSub_2_.getTopic().c_str());
+		}
+
+		cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("combined_cloud", 1);
+
+		warningThread_ = new boost::thread(boost::bind(&PointCloudAggregator::warningLoop, this, subscribedTopicsMsg, approx));
+		NODELET_INFO("%s", subscribedTopicsMsg.c_str());
+	}
+
+	void clouds4_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
+						 const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2,
+						 const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3,
+						 const sensor_msgs::PointCloud2ConstPtr & cloudMsg_4)
+	{
+		std::vector<sensor_msgs::PointCloud2ConstPtr> clouds;
+		clouds.push_back(cloudMsg_1);
+		clouds.push_back(cloudMsg_2);
+		clouds.push_back(cloudMsg_3);
+		clouds.push_back(cloudMsg_4);
+
+		combineClouds(clouds);
+	}
+	void clouds3_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
+	                     const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2,
+	                     const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3)
+	{
+		std::vector<sensor_msgs::PointCloud2ConstPtr> clouds;
+		clouds.push_back(cloudMsg_1);
+		clouds.push_back(cloudMsg_2);
+		clouds.push_back(cloudMsg_3);
+
+		combineClouds(clouds);
+	}
+	void clouds2_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
+						 const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2)
+	{
+		std::vector<sensor_msgs::PointCloud2ConstPtr> clouds;
+		clouds.push_back(cloudMsg_1);
+		clouds.push_back(cloudMsg_2);
+
+		combineClouds(clouds);
+	}
+	void combineClouds(const std::vector<sensor_msgs::PointCloud2ConstPtr> & cloudMsgs)
+	{
+		callbackCalled_ = true;
+		ROS_ASSERT(cloudMsgs.size() > 1);
+		if(cloudPub_.getNumSubscribers())
+		{
+			pcl::PCLPointCloud2::Ptr output(new pcl::PCLPointCloud2);
+
+			std::string frameId = frameId_;
+			if(!frameId.empty() && frameId.compare(cloudMsgs[0]->header.frame_id) != 0)
+			{
+				sensor_msgs::PointCloud2 tmp;
+				pcl_ros::transformPointCloud(frameId, *cloudMsgs[0], tmp, tfListener_);
+				pcl_conversions::toPCL(tmp, *output);
+			}
+			else
+			{
+				pcl_conversions::toPCL(*cloudMsgs[0], *output);
+				frameId = cloudMsgs[0]->header.frame_id;
+			}
+
+			if(xyzOutput_ && !output->data.empty())
+			{
+				// convert only if not already XYZ cloud
+				bool hasField[4] = {false};
+				for(size_t i=0; i<output->fields.size(); ++i)
+				{
+					if(output->fields[i].name.compare("x") == 0)
+					{
+						hasField[0] = true;
+					}
+					else if(output->fields[i].name.compare("y") == 0)
+					{
+						hasField[1] = true;
+					}
+					else if(output->fields[i].name.compare("z") == 0)
+					{
+						hasField[2] = true;
+					}
+					else
+					{
+						hasField[3] = true; // other
+						break;
+					}
+				}
+				if(hasField[0] && hasField[1] && hasField[2] && !hasField[3])
+				{
+					// do nothing, already XYZ
+				}
+				else
+				{
+					pcl::PointCloud<pcl::PointXYZ> cloudxyz;
+					pcl::fromPCLPointCloud2(*output, cloudxyz);
+					pcl::toPCLPointCloud2(cloudxyz, *output);
+				}
+			}
+
+			for(unsigned int i=1; i<cloudMsgs.size(); ++i)
+			{
+				rtabmap::Transform cloudDisplacement;
+				if(!fixedFrameId_.empty() &&
+				   cloudMsgs[0]->header.stamp != cloudMsgs[i]->header.stamp)
+				{
+					// approx sync
+					cloudDisplacement = rtabmap_conversions::getTransform(
+							frameId, //sourceTargetFrame
+							fixedFrameId_, //fixedFrame
+							cloudMsgs[i]->header.stamp, //stampSource
+							cloudMsgs[0]->header.stamp, //stampTarget
+							tfListener_,
+							waitForTransformDuration_);
+				}
+
+				pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2);
+				if(frameId.compare(cloudMsgs[i]->header.frame_id) != 0)
+				{
+					sensor_msgs::PointCloud2 tmp;
+					pcl_ros::transformPointCloud(frameId, *cloudMsgs[i], tmp, tfListener_);
+					if(!cloudDisplacement.isNull())
+					{
+						sensor_msgs::PointCloud2 tmp2;
+						pcl_ros::transformPointCloud(cloudDisplacement.toEigen4f(), tmp, tmp2);
+						pcl_conversions::toPCL(tmp2, *cloud2);
+					}
+					else
+					{
+						pcl_conversions::toPCL(tmp, *cloud2);
+					}
+
+				}
+				else
+				{
+					if(!cloudDisplacement.isNull())
+					{
+						sensor_msgs::PointCloud2 tmp;
+						pcl_ros::transformPointCloud(cloudDisplacement.toEigen4f(), *cloudMsgs[i], tmp);
+						pcl_conversions::toPCL(tmp, *cloud2);
+					}
+					else
+					{
+						pcl_conversions::toPCL(*cloudMsgs[i], *cloud2);
+					}
+				}
+
+				if(!cloud2->is_dense)
+				{
+					// remove nans
+					cloud2 = rtabmap::util3d::removeNaNFromPointCloud(cloud2);
+				}
+
+				if(xyzOutput_ && !cloud2->data.empty())
+				{
+					// convert only if not already XYZ cloud
+					bool hasField[4] = {false};
+					for(size_t i=0; i<cloud2->fields.size(); ++i)
+					{
+						if(cloud2->fields[i].name.compare("x") == 0)
+						{
+							hasField[0] = true;
+						}
+						else if(cloud2->fields[i].name.compare("y") == 0)
+						{
+							hasField[1] = true;
+						}
+						else if(cloud2->fields[i].name.compare("z") == 0)
+						{
+							hasField[2] = true;
+						}
+						else
+						{
+							hasField[3] = true; // other
+							break;
+						}
+					}
+					if(hasField[0] && hasField[1] && hasField[2] && !hasField[3])
+					{
+						// do nothing, already XYZ
+					}
+					else
+					{
+						pcl::PointCloud<pcl::PointXYZ> cloudxyz;
+						pcl::fromPCLPointCloud2(*cloud2, cloudxyz);
+						pcl::toPCLPointCloud2(cloudxyz, *cloud2);
+					}
+				}
+
+				if(output->data.empty())
+				{
+					output = cloud2;
+				}
+				else if(!cloud2->data.empty())
+				{
+
+					if(output->fields.size() != cloud2->fields.size())
+					{
+						ROS_WARN("%s: Input topics don't have all the "
+								"same number of fields (cloud1=%d, cloud%d=%d), concatenation "
+								"may fails. You can enable \"xyz_output\" option "
+								"to convert all inputs to XYZ.",
+								getName().c_str(),
+								(int)output->fields.size(),
+								i+1,
+								(int)output->fields.size());
+					}
+
+					pcl::PCLPointCloud2::Ptr tmp_output(new pcl::PCLPointCloud2);
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+					pcl::concatenate(*output, *cloud2, *tmp_output);
+#else
+					pcl::concatenatePointCloud(*output, *cloud2, *tmp_output);
+#endif
+					//Make sure row_step is the sum of both
+					tmp_output->row_step = tmp_output->width * tmp_output->point_step;
+					output = tmp_output;
+				}
+			}
+
+			sensor_msgs::PointCloud2 rosCloud;
+			pcl_conversions::moveFromPCL(*output, rosCloud);
+			rosCloud.header.stamp = cloudMsgs[0]->header.stamp;
+			rosCloud.header.frame_id = frameId;
+			cloudPub_.publish(rosCloud);
+		}
+	}
+
+	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
+	{
+		ros::Duration r(5.0);
+		while(!callbackCalled_)
+		{
+			r.sleep();
+			if(!callbackCalled_)
+			{
+				ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+						"header are set. %s%s",
+						getName().c_str(),
+						approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
+							"topics should have all the exact timestamp for the callback to be called.",
+						subscribedTopicsMsg.c_str());
+			}
+		}
+	}
+
+	boost::thread * warningThread_;
+	bool callbackCalled_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ExactSync4Policy;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ApproxSync4Policy;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ExactSync3Policy;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ApproxSync3Policy;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ExactSync2Policy;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ApproxSync2Policy;
+	message_filters::Synchronizer<ExactSync4Policy>* exactSync4_;
+	message_filters::Synchronizer<ApproxSync4Policy>* approxSync4_;
+	message_filters::Synchronizer<ExactSync3Policy>* exactSync3_;
+	message_filters::Synchronizer<ApproxSync3Policy>* approxSync3_;
+	message_filters::Synchronizer<ExactSync2Policy>* exactSync2_;
+	message_filters::Synchronizer<ApproxSync2Policy>* approxSync2_;
+	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_1_;
+	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_2_;
+	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_3_;
+	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_4_;
+
+	ros::Publisher cloudPub_;
+
+	std::string frameId_;
+	std::string fixedFrameId_;
+	double waitForTransformDuration_;
+	bool xyzOutput_;
+	tf::TransformListener tfListener_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_util::PointCloudAggregator, nodelet::Nodelet);
+}
+

rtabmap_util/src/nodelets/point_cloud_assembler.cpp

@@ -0,0 +1,578 @@
+/*
+Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/radius_outlier_removal.h>
+
+#include <pcl_ros/transforms.h>
+
+#include <tf/transform_listener.h>
+
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/exact_time.h>
+
+#include <rtabmap_conversions/MsgConversion.h>
+#include <rtabmap_msgs/OdomInfo.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/Version.h>
+
+namespace rtabmap_util
+{
+
+/**
+ * This nodelet can assemble a number of clouds (max_clouds) coming
+ * from the same sensor, taking into account the displacement of the robot based on
+ * fixed_frame_id, then publish the resulting cloud.
+ * If fixed_frame_id is set to "" (empty), the nodelet will subscribe to
+ * an odom topic that should have the exact same stamp than to input cloud.
+ * The output cloud has the same stamp and frame than the last assembled cloud.
+ */
+class PointCloudAssembler : public nodelet::Nodelet
+{
+public:
+	PointCloudAssembler() :
+		warningThread_(0),
+		callbackCalled_(false),
+		exactSync_(0),
+		exactInfoSync_(0),
+		maxClouds_(0),
+		assemblingTime_(0),
+		skipClouds_(0),
+		cloudsSkipped_(0),
+		circularBuffer_(false),
+		linearUpdate_(0),
+		angularUpdate_(0),
+		waitForTransformDuration_(0.1),
+		rangeMin_(0),
+		rangeMax_(0),
+		voxelSize_(0),
+		noiseRadius_(0),
+		noiseMinNeighbors_(5),
+		removeZ_(false),
+		fixedFrameId_("odom"),
+		frameId_("")
+	{}
+
+	virtual ~PointCloudAssembler()
+	{
+		delete exactSync_;
+		delete exactInfoSync_;
+
+		if(warningThread_)
+		{
+			callbackCalled_=true;
+			warningThread_->join();
+			delete warningThread_;
+		}
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 5;
+		bool subscribeOdomInfo = false;
+
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
+		pnh.param("frame_id", frameId_, frameId_);
+		pnh.param("max_clouds", maxClouds_, maxClouds_);
+		pnh.param("assembling_time", assemblingTime_, assemblingTime_);
+		pnh.param("skip_clouds", skipClouds_, skipClouds_);
+		pnh.param("circular_buffer", circularBuffer_, circularBuffer_);
+		pnh.param("linear_update", linearUpdate_, linearUpdate_);
+		pnh.param("angular_update", angularUpdate_, angularUpdate_);
+		pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
+		pnh.param("range_min", rangeMin_, rangeMin_);
+		pnh.param("range_max", rangeMax_, rangeMax_);
+		pnh.param("voxel_size", voxelSize_, voxelSize_);
+		pnh.param("noise_radius", noiseRadius_, noiseRadius_);
+		pnh.param("noise_min_neighbors", noiseMinNeighbors_, noiseMinNeighbors_);
+		pnh.param("remove_z", removeZ_, removeZ_);
+		pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
+		ROS_ASSERT(maxClouds_>0 || assemblingTime_ >0.0);
+
+		ROS_INFO("%s: queue_size=%d", getName().c_str(), queueSize);
+		ROS_INFO("%s: fixed_frame_id=%s", getName().c_str(), fixedFrameId_.c_str());
+		ROS_INFO("%s: frame_id=%s", getName().c_str(), frameId_.c_str());
+		ROS_INFO("%s: max_clouds=%d", getName().c_str(), maxClouds_);
+		ROS_INFO("%s: assembling_time=%fs", getName().c_str(), assemblingTime_);
+		ROS_INFO("%s: skip_clouds=%d", getName().c_str(), skipClouds_);
+		ROS_INFO("%s: circular_buffer=%s", getName().c_str(), circularBuffer_?"true":"false");
+		ROS_INFO("%s: linear_update=%f m", getName().c_str(), linearUpdate_);
+		ROS_INFO("%s: angular_update=%f rad", getName().c_str(), angularUpdate_);
+		ROS_INFO("%s: wait_for_transform_duration=%f", getName().c_str(), waitForTransformDuration_);
+		ROS_INFO("%s: range_min=%f", getName().c_str(), rangeMin_);
+		ROS_INFO("%s: range_max=%f", getName().c_str(), rangeMax_);
+		ROS_INFO("%s: voxel_size=%fm", getName().c_str(), voxelSize_);
+		ROS_INFO("%s: noise_radius=%fm", getName().c_str(), noiseRadius_);
+		ROS_INFO("%s: noise_min_neighbors=%d", getName().c_str(), noiseMinNeighbors_);
+		ROS_INFO("%s: remove_z=%s", getName().c_str(), removeZ_?"true":"false");
+
+		if(maxClouds_==0 && assemblingTime_ ==0.0)
+		{
+			ROS_ERROR("point_cloud_assembler: max_cloud or assembling_time parameters should be set!");
+			exit(-1);
+		}
+
+		cloudsSkipped_ = skipClouds_;
+
+		std::string subscribedTopicsMsg;
+		if(!fixedFrameId_.empty())
+		{
+			cloudSub_ = nh.subscribe("cloud", queueSize, &PointCloudAssembler::callbackCloud, this);
+			subscribedTopicsMsg = uFormat("\n%s subscribed to %s",
+								getName().c_str(),
+								cloudSub_.getTopic().c_str());
+		}
+		else if(subscribeOdomInfo)
+		{
+			syncCloudSub_.subscribe(nh, "cloud", 1);
+			syncOdomSub_.subscribe(nh, "odom", 1);
+			syncOdomInfoSub_.subscribe(nh, "odom_info", 1);
+			exactInfoSync_ = new message_filters::Synchronizer<syncInfoPolicy>(syncInfoPolicy(queueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_);
+			exactInfoSync_->registerCallback(boost::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdomInfo, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (exact sync):\n   %s,\n   %s",
+								getName().c_str(),
+								syncCloudSub_.getTopic().c_str(),
+								syncOdomSub_.getTopic().c_str(),
+								syncOdomInfoSub_.getTopic().c_str());
+
+			warningThread_ = new boost::thread(boost::bind(&PointCloudAssembler::warningLoop, this, subscribedTopicsMsg));
+		}
+		else
+		{
+			syncCloudSub_.subscribe(nh, "cloud", 1);
+			syncOdomSub_.subscribe(nh, "odom", 1);
+			exactSync_ = new message_filters::Synchronizer<syncPolicy>(syncPolicy(queueSize), syncCloudSub_, syncOdomSub_);
+			exactSync_->registerCallback(boost::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdom, this, boost::placeholders::_1, boost::placeholders::_2));
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (exact sync):\n   %s,\n   %s",
+								getName().c_str(),
+								syncCloudSub_.getTopic().c_str(),
+								syncOdomSub_.getTopic().c_str());
+
+			warningThread_ = new boost::thread(boost::bind(&PointCloudAssembler::warningLoop, this, subscribedTopicsMsg));
+		}
+
+		cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("assembled_cloud", 1);
+
+		NODELET_INFO("%s", subscribedTopicsMsg.c_str());
+	}
+
+	void callbackCloudOdom(
+			const sensor_msgs::PointCloud2ConstPtr & cloudMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg)
+	{
+		callbackCalled_ = true;
+		rtabmap::Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
+		if(!odom.isNull())
+		{
+			fixedFrameId_ = odomMsg->header.frame_id;
+			callbackCloud(cloudMsg);
+		}
+		else
+		{
+			NODELET_WARN("Reseting point cloud assembler as null odometry has been received.");
+			clouds_.clear();
+		}
+	}
+
+	void callbackCloudOdomInfo(
+			const sensor_msgs::PointCloud2ConstPtr & cloudMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const rtabmap_msgs::OdomInfoConstPtr & odomInfoMsg)
+	{
+		callbackCalled_ = true;
+		rtabmap::Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
+		if(!odom.isNull())
+		{
+			if(odomInfoMsg->keyFrameAdded)
+			{
+				fixedFrameId_ = odomMsg->header.frame_id;
+				callbackCloud(cloudMsg);
+			}
+			else
+			{
+				NODELET_INFO("Skipping non keyframe...");
+			}
+		}
+		else
+		{
+			NODELET_WARN("Reseting point cloud assembler as null odometry has been received.");
+			clouds_.clear();
+		}
+	}
+
+	sensor_msgs::PointCloud2 removeField(const sensor_msgs::PointCloud2 & input, const std::string & field)
+	{
+		sensor_msgs::PointCloud2 output;
+		int offset = 0;
+		std::vector<int> inputFieldIndex;
+		for(size_t i=0; i<input.fields.size(); ++i)
+		{
+			if(input.fields[i].name.compare(field) == 0)
+			{
+				continue;
+			}
+			else
+			{
+				sensor_msgs::PointField outputField = input.fields[i];
+				outputField.offset = offset;
+				offset += outputField.count * sizeOfPointField(outputField.datatype);
+				output.fields.push_back(outputField);
+				inputFieldIndex.push_back(i);
+			}
+		}
+		output.header = input.header;
+		output.height = input.height;
+		output.width = input.width;
+		output.is_bigendian = input.is_bigendian;
+		output.is_dense = input.is_dense;
+		output.point_step = offset;
+		output.row_step = output.width * output.point_step;
+		output.data.resize(output.height*output.row_step);
+		int total = output.height*output.width;
+		for(int i=0; i<total; ++i)
+		{
+			// for each point, copy fields
+			int oi = i*output.point_step;
+			int pi = i*input.point_step;
+			for(size_t j=0;j<output.fields.size(); ++j)
+			{
+				memcpy(&output.data[oi + output.fields[j].offset],
+					   &input.data[pi + input.fields[inputFieldIndex[j]].offset],
+					   output.fields[j].count * sizeOfPointField(output.fields[j].datatype));
+			}
+		}
+		return output;
+	}
+
+	void callbackCloud(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
+	{
+		if(cloudPub_.getNumSubscribers())
+		{
+			UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
+					uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
+
+			if(skipClouds_<=0 || cloudsSkipped_ >= skipClouds_)
+			{
+				cloudsSkipped_ = 0;
+
+				rtabmap::Transform pose = rtabmap_conversions::getTransform(
+						fixedFrameId_, //fromFrame
+						cloudMsg->header.frame_id, //toFrame
+						cloudMsg->header.stamp,
+						tfListener_,
+						waitForTransformDuration_);
+
+				if(pose.isNull())
+				{
+					ROS_ERROR("Cloud not transform all clouds! Resetting...");
+					clouds_.clear();
+					return;
+				}
+
+				bool isMoving = true;
+				if(!previousPose_.isNull() && (linearUpdate_>0 || angularUpdate_>0))
+				{
+					rtabmap::Transform delta = previousPose_.inverse()*pose;
+					float roll, pitch, yaw;
+					delta.getEulerAngles(roll, pitch, yaw);
+					isMoving = fabs(delta.x()) > linearUpdate_ ||
+									fabs(delta.y()) > linearUpdate_ ||
+									fabs(delta.z()) > linearUpdate_ ||
+									(angularUpdate_>0.0f && (
+										fabs(roll) > angularUpdate_ ||
+										fabs(pitch) > angularUpdate_ ||
+										fabs(yaw) > angularUpdate_));
+				}
+
+				pcl::PCLPointCloud2::Ptr newCloud(new pcl::PCLPointCloud2);
+				if(rangeMin_ > 0.0 || rangeMax_ > 0.0 || voxelSize_ > 0.0f)
+				{
+					pcl_conversions::toPCL(*cloudMsg, *newCloud);
+					rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*newCloud);
+					scan = rtabmap::util3d::commonFiltering(scan, 1, rangeMin_, rangeMax_, voxelSize_);
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+					std::uint64_t stamp = newCloud->header.stamp;
+#else
+					pcl::uint64_t stamp = newCloud->header.stamp;
+#endif
+					newCloud = rtabmap::util3d::laserScanToPointCloud2(scan, pose);
+					newCloud->header.stamp = stamp;
+				}
+				else
+				{
+					sensor_msgs::PointCloud2 output;
+					pcl_ros::transformPointCloud(pose.toEigen4f(), *cloudMsg, output);
+					pcl_conversions::toPCL(output, *newCloud);
+				}
+
+				if(!newCloud->is_dense)
+				{
+					// remove nans
+					newCloud = rtabmap::util3d::removeNaNFromPointCloud(newCloud);
+				}
+
+				clouds_.push_back(newCloud);
+
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+				bool reachedMaxSize =
+						((int)clouds_.size() >= maxClouds_ && maxClouds_ > 0)
+						||
+						((*newCloud).header.stamp >= clouds_.front()->header.stamp + static_cast<std::uint64_t>(assemblingTime_*1000000.0) && assemblingTime_ > 0.0);
+#else
+				bool reachedMaxSize =
+						((int)clouds_.size() >= maxClouds_ && maxClouds_ > 0)
+						||
+						((*newCloud).header.stamp >= clouds_.front()->header.stamp + static_cast<pcl::uint64_t>(assemblingTime_*1000000.0) && assemblingTime_ > 0.0);
+#endif
+
+				if( circularBuffer_ || reachedMaxSize )
+				{
+					pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2);
+					for(std::list<pcl::PCLPointCloud2::Ptr>::iterator iter=clouds_.begin(); iter!=clouds_.end(); ++iter)
+					{
+						if(assembled->data.empty())
+						{
+							*assembled = *(*iter);
+						}
+						else
+						{
+							pcl::PCLPointCloud2Ptr assembledTmp(new pcl::PCLPointCloud2);
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+							pcl::concatenate(*assembled, *(*iter), *assembledTmp);
+#else
+							pcl::concatenatePointCloud(*assembled, *(*iter), *assembledTmp);
+#endif
+							//Make sure row_step is the sum of both
+							assembledTmp->row_step = assembled->row_step + (*iter)->row_step;
+							assembled = assembledTmp;
+						}
+					}
+
+					sensor_msgs::PointCloud2 rosCloud;
+					if(voxelSize_>0.0)
+					{
+						// estimate if there would be an overflow
+						int x_idx=-1, y_idx=-1, z_idx=-1;
+						for (std::size_t d = 0; d < assembled->fields.size (); ++d)
+						{
+							if (assembled->fields[d].name.compare("x")==0)
+								x_idx = d;
+							if (assembled->fields[d].name.compare("y")==0)
+								y_idx = d;
+							if (assembled->fields[d].name.compare("z")==0)
+								z_idx = d;
+						}
+						bool overflow = false;
+						if(x_idx>=0 && y_idx>=0 && z_idx>=0) {
+							Eigen::Vector4f min_p, max_p;
+							pcl::getMinMax3D(assembled, x_idx, y_idx, z_idx, min_p, max_p);
+							float inverseVoxelSize = 1.0f/voxelSize_;
+							std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverseVoxelSize)+1;
+							std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverseVoxelSize)+1;
+							std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverseVoxelSize)+1;
+
+							if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
+							{
+								overflow = true;
+							}
+						}
+						if(overflow)
+						{
+							rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*assembled);
+							scan = rtabmap::util3d::commonFiltering(scan, 1, 0, 0, voxelSize_);
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+							std::uint64_t stamp = assembled->header.stamp;
+#else
+							pcl::uint64_t stamp = assembled->header.stamp;
+#endif
+							assembled = rtabmap::util3d::laserScanToPointCloud2(scan);
+							assembled->header.stamp = stamp;
+						}
+						else
+						{
+							pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
+							filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
+							filter.setInputCloud(assembled);
+							pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
+							filter.filter(*output);
+							assembled = output;
+						}
+					}
+					if(noiseRadius_>0.0 && noiseMinNeighbors_>0)
+					{
+						pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> filter;
+						filter.setRadiusSearch(noiseRadius_);
+						filter.setMinNeighborsInRadius(noiseMinNeighbors_);
+						filter.setInputCloud(assembled);
+						pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
+						filter.filter(*output);
+						assembled = output;
+					}
+
+					pcl_conversions::moveFromPCL(*assembled, rosCloud);
+					rtabmap::Transform t = pose;
+					if(!frameId_.empty())
+					{
+						// transform in target frame_id instead of sensor frame
+						t = rtabmap_conversions::getTransform(
+								fixedFrameId_, //fromFrame
+								frameId_, //toFrame
+								cloudMsg->header.stamp,
+								tfListener_,
+								waitForTransformDuration_);
+						if(t.isNull())
+						{
+							ROS_ERROR("Cloud not transform back assembled clouds in target frame \"%s\"! Resetting...", frameId_.c_str());
+							clouds_.clear();
+							return;
+						}
+					}
+					pcl_ros::transformPointCloud(t.toEigen4f().inverse(), rosCloud, rosCloud);
+
+					if(removeZ_)
+					{
+						rosCloud = removeField(rosCloud, "z");
+					}
+
+					rosCloud.header = cloudMsg->header;
+					if(!frameId_.empty())
+					{
+						rosCloud.header.frame_id = frameId_;
+					}
+					cloudPub_.publish(rosCloud);
+					if(circularBuffer_)
+					{
+						if(!isMoving)
+						{
+							clouds_.pop_back();
+						}
+						else
+						{
+							previousPose_ = pose;
+							if(reachedMaxSize)
+							{
+								clouds_.pop_front();
+							}
+						}
+					}
+					else
+					{
+						clouds_.clear();
+						previousPose_.setNull();
+					}
+				}
+				else if(!isMoving)
+				{
+					clouds_.pop_back();
+				}
+				else
+				{
+					previousPose_ = pose;
+				}
+			}
+			else
+			{
+				++cloudsSkipped_;
+			}
+		}
+	}
+
+	void warningLoop(const std::string & subscribedTopicsMsg)
+	{
+		ros::Duration r(5.0);
+		while(!callbackCalled_)
+		{
+			r.sleep();
+			if(!callbackCalled_)
+			{
+				ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+						"header are set. %s",
+						getName().c_str(),
+						subscribedTopicsMsg.c_str());
+			}
+		}
+	}
+
+private:
+	boost::thread * warningThread_;
+	bool callbackCalled_;
+
+	ros::Subscriber cloudSub_;
+	ros::Publisher cloudPub_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> syncPolicy;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, nav_msgs::Odometry, rtabmap_msgs::OdomInfo> syncInfoPolicy;
+	message_filters::Synchronizer<syncPolicy>* exactSync_;
+	message_filters::Synchronizer<syncInfoPolicy>* exactInfoSync_;
+	message_filters::Subscriber<sensor_msgs::PointCloud2> syncCloudSub_;
+	message_filters::Subscriber<nav_msgs::Odometry> syncOdomSub_;
+	message_filters::Subscriber<rtabmap_msgs::OdomInfo> syncOdomInfoSub_;
+
+	int maxClouds_;
+	int skipClouds_;
+	int cloudsSkipped_;
+	bool circularBuffer_;
+	double linearUpdate_;
+	double angularUpdate_;
+	double assemblingTime_;
+	double waitForTransformDuration_;
+	double rangeMin_;
+	double rangeMax_;
+	double voxelSize_;
+	double noiseRadius_;
+	int noiseMinNeighbors_;
+	bool removeZ_;
+	std::string fixedFrameId_;
+	std::string frameId_;
+	tf::TransformListener tfListener_;
+	rtabmap::Transform previousPose_;
+
+	std::list<pcl::PCLPointCloud2::Ptr> clouds_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_util::PointCloudAssembler, nodelet::Nodelet);
+}
+

rtabmap_util/src/nodelets/point_cloud_xyz.cpp

@@ -0,0 +1,425 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+
+#include <rtabmap_conversions/MsgConversion.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 <stereo_msgs/DisparityImage.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <image_geometry/pinhole_camera_model.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d_surface.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UStl.h"
+
+namespace rtabmap_util
+{
+
+class PointCloudXYZ : public nodelet::Nodelet
+{
+public:
+	PointCloudXYZ() :
+		maxDepth_(0.0),
+		minDepth_(0.0),
+		voxelSize_(0.0),
+		decimation_(1),
+		noiseFilterRadius_(0.0),
+		noiseFilterMinNeighbors_(5),
+		normalK_(0),
+		normalRadius_(0.0),
+		filterNaNs_(false),
+		approxSyncDepth_(0),
+		approxSyncDisparity_(0),
+		exactSyncDepth_(0),
+		exactSyncDisparity_(0)
+	{}
+
+	virtual ~PointCloudXYZ()
+	{
+		if(approxSyncDepth_)
+			delete approxSyncDepth_;
+		if(approxSyncDisparity_)
+			delete approxSyncDisparity_;
+		if(exactSyncDepth_)
+			delete exactSyncDepth_;
+		if(exactSyncDisparity_)
+			delete exactSyncDisparity_;
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		bool approxSync = true;
+		std::string roiStr;
+		double approxSyncMaxInterval = 0.0;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("max_depth", maxDepth_, maxDepth_);
+		pnh.param("min_depth", minDepth_, minDepth_);
+		pnh.param("voxel_size", voxelSize_, voxelSize_);
+		pnh.param("decimation", decimation_, decimation_);
+		pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
+		pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
+		pnh.param("normal_k", normalK_, normalK_);
+		pnh.param("normal_radius", normalRadius_, normalRadius_);
+		pnh.param("filter_nans", filterNaNs_, filterNaNs_);
+		pnh.param("roi_ratios", roiStr, roiStr);
+
+		// Deprecated
+		if(pnh.hasParam("cut_left"))
+		{
+			ROS_ERROR("\"cut_left\" parameter is replaced by \"roi_ratios\". It will be ignored.");
+		}
+		if(pnh.hasParam("cut_right"))
+		{
+			ROS_ERROR("\"cut_right\" parameter is replaced by \"roi_ratios\". It will be ignored.");
+		}
+		if(pnh.hasParam("special_filter_close_object"))
+		{
+			ROS_ERROR("\"special_filter_close_object\" parameter is removed. This kind of processing "
+					  "should be done before or after this nodelet. See old implementation here: "
+					  "https://github.com/introlab/rtabmap_ros/blob/f0026b071c7c54fbcc71df778dd7e17f52f78fc4/src/nodelets/point_cloud_xyz.cpp#L178-L201.");
+		}
+
+		//parse roi (region of interest)
+		roiRatios_.resize(4, 0);
+		if(!roiStr.empty())
+		{
+			std::list<std::string> strValues = uSplit(roiStr, ' ');
+			if(strValues.size() != 4)
+			{
+				ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
+			}
+			else
+			{
+				std::vector<float> tmpValues(4);
+				unsigned int i=0;
+				for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
+				{
+					tmpValues[i] = uStr2Float(*jter);
+					++i;
+				}
+
+				if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
+					tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
+					tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
+					tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
+				{
+					roiRatios_ = tmpValues;
+				}
+				else
+				{
+					ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
+				}
+			}
+		}
+
+		NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
+
+		if(approxSync)
+		{
+			approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, boost::placeholders::_1, boost::placeholders::_2));
+
+			approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSyncDisparity_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, boost::placeholders::_1, boost::placeholders::_2));
+		}
+		else
+		{
+			exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
+			exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, boost::placeholders::_1, boost::placeholders::_2));
+
+			exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
+			exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, boost::placeholders::_1, boost::placeholders::_2));
+		}
+
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle depth_pnh(pnh, "depth");
+		image_transport::ImageTransport depth_it(depth_nh);
+		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		cameraInfoSub_.subscribe(depth_nh, "camera_info", 1);
+
+		disparitySub_.subscribe(nh, "disparity/image", 1);
+		disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", 1);
+
+		cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
+	}
+
+	void callback(
+			  const sensor_msgs::ImageConstPtr& depthMsg,
+			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
+		   depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
+		   depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
+		{
+			NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16");
+			return;
+		}
+
+		if(cloudPub_.getNumSubscribers())
+		{
+			ros::WallTime time = ros::WallTime::now();
+
+			cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depthMsg);
+			cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
+
+			rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
+
+			pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
+
+			cv::Mat depth = imageDepthPtr->image;
+			if( roiRatios_.size() == 4 &&
+				((roiRatios_[0] > 0.0f && roiRatios_[0] <= 1.0f) ||
+				 (roiRatios_[1] > 0.0f && roiRatios_[1] <= 1.0f) ||
+				 (roiRatios_[2] > 0.0f && roiRatios_[2] <= 1.0f) ||
+				 (roiRatios_[3] > 0.0f && roiRatios_[3] <= 1.0f)))
+			{
+				cv::Rect roiDepth = rtabmap::util2d::computeRoi(depth, roiRatios_);
+				cv::Rect roiRgb;
+				if(model.imageWidth() && model.imageHeight())
+				{
+					roiRgb = rtabmap::util2d::computeRoi(model.imageSize(), roiRatios_);
+				}
+				if(	roiDepth.width%decimation_==0 &&
+					roiDepth.height%decimation_==0 &&
+					(roiRgb.width != 0 ||
+					   (roiRgb.width%decimation_==0 &&
+						roiRgb.height%decimation_==0)))
+				{
+					depth = cv::Mat(depth, roiDepth);
+					if(model.imageWidth() != 0 && model.imageHeight() != 0)
+					{
+						model = model.roi(roiRgb);
+					}
+					else
+					{
+						model = model.roi(roiDepth);
+					}
+				}
+				else
+				{
+					NODELET_ERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
+						  "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
+						  "by decimation parameter (%d). Ignoring ROI ratios...",
+						  roiRatios_[0],
+						  roiRatios_[1],
+						  roiRatios_[2],
+						  roiRatios_[3],
+						  roiDepth.width,
+						  roiDepth.height,
+						  roiRgb.width,
+						  roiRgb.height,
+						  decimation_);
+				}
+			}
+
+			pcl::IndicesPtr indices(new std::vector<int>);
+			pclCloud = rtabmap::util3d::cloudFromDepth(
+					depth,
+					model,
+					decimation_,
+					maxDepth_,
+					minDepth_,
+					indices.get());
+			processAndPublish(pclCloud, indices, depthMsg->header);
+
+			NODELET_DEBUG("point_cloud_xyz from depth time = %f s", (ros::WallTime::now() - time).toSec());
+		}
+	}
+
+	void callbackDisparity(
+			const stereo_msgs::DisparityImageConstPtr& disparityMsg,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
+		   disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
+		{
+			NODELET_ERROR("Input type must be disparity=32FC1 or 16SC1");
+			return;
+		}
+
+		cv::Mat disparity;
+		if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
+		{
+			disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
+		}
+		else
+		{
+			disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_16SC1, const_cast<uchar*>(disparityMsg->image.data.data()));
+		}
+
+		if(cloudPub_.getNumSubscribers())
+		{
+			ros::WallTime time = ros::WallTime::now();
+
+			cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
+
+			pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
+			rtabmap::CameraModel leftModel = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
+			UASSERT(disparity.cols == leftModel.imageWidth() && disparity.rows == leftModel.imageHeight());
+			rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), disparityMsg->T);
+			pcl::IndicesPtr indices(new std::vector<int>);
+			pclCloud = rtabmap::util3d::cloudFromDisparity(
+					cv::Mat(disparity, roi),
+					stereoModel,
+					decimation_,
+					maxDepth_,
+					minDepth_,
+					indices.get());
+
+			processAndPublish(pclCloud, indices, disparityMsg->header);
+
+			NODELET_DEBUG("point_cloud_xyz from disparity time = %f s", (ros::WallTime::now() - time).toSec());
+		}
+	}
+
+	void processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::Header & header)
+	{
+		if(indices->size() && voxelSize_ > 0.0)
+		{
+			pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
+			pclCloud->is_dense = true;
+		}
+
+		// Do radius filtering after voxel filtering ( a lot faster)
+		if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
+		{
+			if(pclCloud->is_dense)
+			{
+				indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
+			}
+			else
+			{
+				indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_);
+			}
+			pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::copyPointCloud(*pclCloud, *indices, *tmp);
+			pclCloud = tmp;
+		}
+
+		sensor_msgs::PointCloud2 rosCloud;
+		if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_ > 0 || normalRadius_ > 0.0f))
+		{
+			//compute normals
+			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
+			pcl::PointCloud<pcl::PointNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointNormal>);
+			pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
+			if(filterNaNs_)
+			{
+				pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
+			}
+			pcl::toROSMsg(*pclCloudNormal, rosCloud);
+		}
+		else
+		{
+			if(filterNaNs_ && !pclCloud->is_dense)
+			{
+				pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
+			}
+			pcl::toROSMsg(*pclCloud, rosCloud);
+		}
+		rosCloud.header.stamp = header.stamp;
+		rosCloud.header.frame_id = header.frame_id;
+
+		//publish the message
+		cloudPub_.publish(rosCloud);
+	}
+
+private:
+
+	double maxDepth_;
+	double minDepth_;
+	double voxelSize_;
+	int decimation_;
+	double noiseFilterRadius_;
+	int noiseFilterMinNeighbors_;
+	int normalK_;
+	double normalRadius_;
+	bool filterNaNs_;
+	std::vector<float> roiRatios_;
+
+	ros::Publisher cloudPub_;
+
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+
+	message_filters::Subscriber<stereo_msgs::DisparityImage> disparitySub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> disparityCameraInfoSub_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
+	message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
+
+	typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyApproxSyncDisparityPolicy;
+	message_filters::Synchronizer<MyApproxSyncDisparityPolicy> * approxSyncDisparity_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
+	message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
+
+	typedef message_filters::sync_policies::ExactTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyExactSyncDisparityPolicy;
+	message_filters::Synchronizer<MyExactSyncDisparityPolicy> * exactSyncDisparity_;
+
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_util::PointCloudXYZ, nodelet::Nodelet);
+}
+

rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp

@@ -0,0 +1,610 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <rtabmap_conversions/MsgConversion.h>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include <stereo_msgs/DisparityImage.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <image_geometry/pinhole_camera_model.h>
+#include <image_geometry/stereo_camera_model.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d_surface.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UStl.h"
+
+namespace rtabmap_util
+{
+
+class PointCloudXYZRGB : public nodelet::Nodelet
+{
+public:
+	PointCloudXYZRGB() :
+		maxDepth_(0.0),
+		minDepth_(0.0),
+		voxelSize_(0.0),
+		decimation_(1),
+		noiseFilterRadius_(0.0),
+		noiseFilterMinNeighbors_(5),
+		normalK_(0),
+		normalRadius_(0.0),
+		filterNaNs_(false),
+		approxSyncDepth_(0),
+		approxSyncDisparity_(0),
+		approxSyncStereo_(0),
+		exactSyncDepth_(0),
+		exactSyncDisparity_(0),
+		exactSyncStereo_(0)
+	{}
+
+	virtual ~PointCloudXYZRGB()
+	{
+		if(approxSyncDepth_)
+			delete approxSyncDepth_;
+		if(approxSyncDisparity_)
+			delete approxSyncDisparity_;
+		if(approxSyncStereo_)
+			delete approxSyncStereo_;
+		if(exactSyncDepth_)
+			delete exactSyncDepth_;
+		if(exactSyncDisparity_)
+			delete exactSyncDisparity_;
+		if(exactSyncStereo_)
+			delete exactSyncStereo_;
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		bool approxSync = true;
+		std::string roiStr;
+		double approxSyncMaxInterval = 0.0;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("max_depth", maxDepth_, maxDepth_);
+		pnh.param("min_depth", minDepth_, minDepth_);
+		pnh.param("voxel_size", voxelSize_, voxelSize_);
+		pnh.param("decimation", decimation_, decimation_);
+		pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
+		pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
+		pnh.param("normal_k", normalK_, normalK_);
+		pnh.param("normal_radius", normalRadius_, normalRadius_);
+		pnh.param("filter_nans", filterNaNs_, filterNaNs_);
+		pnh.param("roi_ratios", roiStr, roiStr);
+
+		//parse roi (region of interest)
+		roiRatios_.resize(4, 0);
+		if(!roiStr.empty())
+		{
+			std::list<std::string> strValues = uSplit(roiStr, ' ');
+			if(strValues.size() != 4)
+			{
+				ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
+			}
+			else
+			{
+				std::vector<float> tmpValues(4);
+				unsigned int i=0;
+				for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
+				{
+					tmpValues[i] = uStr2Float(*jter);
+					++i;
+				}
+
+				if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
+					tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
+					tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
+					tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
+				{
+					roiRatios_ = tmpValues;
+				}
+				else
+				{
+					ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
+				}
+			}
+		}
+
+		// StereoBM parameters
+		stereoBMParameters_ = rtabmap::Parameters::getDefaultParameters("StereoBM");
+		for(rtabmap::ParametersMap::iterator iter=stereoBMParameters_.begin(); iter!=stereoBMParameters_.end(); ++iter)
+		{
+			std::string vStr;
+			bool vBool;
+			int vInt;
+			double vDouble;
+			if(pnh.getParam(iter->first, vStr))
+			{
+				NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
+				iter->second = vStr;
+			}
+			else if(pnh.getParam(iter->first, vBool))
+			{
+				NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
+				iter->second = uBool2Str(vBool);
+			}
+			else if(pnh.getParam(iter->first, vDouble))
+			{
+				NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
+				iter->second = uNumber2Str(vDouble);
+			}
+			else if(pnh.getParam(iter->first, vInt))
+			{
+				NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
+				iter->second = uNumber2Str(vInt);
+			}
+		}
+
+		NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
+
+		cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
+
+		rgbdImageSub_ = nh.subscribe("rgbd_image", 1, &PointCloudXYZRGB::rgbdImageCallback, this);
+
+		if(approxSync)
+		{
+			approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+
+			approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSyncDisparity_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+
+			approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSyncStereo_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+		}
+		else
+		{
+			exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+			exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+
+			exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
+			exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+
+			exactSyncStereo_ = new message_filters::Synchronizer<MyExactSyncStereoPolicy>(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
+			exactSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+		}
+
+		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);
+
+		ros::NodeHandle left_nh(nh, "left");
+		ros::NodeHandle right_nh(nh, "right");
+		ros::NodeHandle left_pnh(pnh, "left");
+		ros::NodeHandle right_pnh(pnh, "right");
+		image_transport::ImageTransport left_it(left_nh);
+		image_transport::ImageTransport right_it(right_nh);
+		image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
+		image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
+
+		imageDisparitySub_.subscribe(nh, "disparity", 1);
+
+		imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft);
+		imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight);
+		cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
+		cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
+	}
+
+	void depthCallback(
+			  const sensor_msgs::ImageConstPtr& image,
+			  const sensor_msgs::ImageConstPtr& imageDepth,
+			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
+		   !(imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
+			 imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
+			 imageDepth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
+		{
+			NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
+			return;
+		}
+
+		if(cloudPub_.getNumSubscribers())
+		{
+			ros::WallTime time = ros::WallTime::now();
+
+			cv_bridge::CvImageConstPtr imagePtr;
+			if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
+			{
+				imagePtr = cv_bridge::toCvShare(image);
+			}
+			else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+					image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+			{
+				imagePtr = cv_bridge::toCvShare(image, "mono8");
+			}
+			else
+			{
+				imagePtr = cv_bridge::toCvShare(image, "bgr8");
+			}
+
+			cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
+
+			rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
+
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
+
+			cv::Mat rgb = imagePtr->image;
+			cv::Mat depth = imageDepthPtr->image;
+			if( roiRatios_.size() == 4 &&
+				((roiRatios_[0] > 0.0f && roiRatios_[0] <= 1.0f) ||
+				 (roiRatios_[1] > 0.0f && roiRatios_[1] <= 1.0f) ||
+				 (roiRatios_[2] > 0.0f && roiRatios_[2] <= 1.0f) ||
+				 (roiRatios_[3] > 0.0f && roiRatios_[3] <= 1.0f)))
+			{
+				cv::Rect roiDepth = rtabmap::util2d::computeRoi(depth, roiRatios_);
+				cv::Rect roiRgb = rtabmap::util2d::computeRoi(rgb, roiRatios_);
+				if(	roiDepth.width%decimation_==0 &&
+					roiDepth.height%decimation_==0 &&
+					roiRgb.width%decimation_==0 &&
+					roiRgb.height%decimation_==0)
+				{
+					depth = cv::Mat(depth, roiDepth);
+					rgb = cv::Mat(rgb, roiRgb);
+					model = model.roi(roiRgb);
+				}
+				else
+				{
+					NODELET_ERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
+						  "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
+						  "by decimation parameter (%d). Ignoring ROI ratios...",
+						  roiRatios_[0],
+						  roiRatios_[1],
+						  roiRatios_[2],
+						  roiRatios_[3],
+						  roiDepth.width,
+						  roiDepth.height,
+						  roiRgb.width,
+						  roiRgb.height,
+						  decimation_);
+				}
+			}
+
+			pcl::IndicesPtr indices(new std::vector<int>);
+			pclCloud = rtabmap::util3d::cloudFromDepthRGB(
+					rgb,
+					depth,
+					model,
+					decimation_,
+					maxDepth_,
+					minDepth_,
+					indices.get());
+
+
+			processAndPublish(pclCloud, indices, imagePtr->header);
+
+			NODELET_DEBUG("point_cloud_xyzrgb from RGB-D time = %f s", (ros::WallTime::now() - time).toSec());
+		}
+	}
+
+	void disparityCallback(
+			const sensor_msgs::ImageConstPtr& image,
+			const stereo_msgs::DisparityImageConstPtr& imageDisparity,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		cv_bridge::CvImageConstPtr imagePtr;
+		if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
+		{
+			imagePtr = cv_bridge::toCvShare(image);
+		}
+		else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+				image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+		{
+			imagePtr = cv_bridge::toCvShare(image, "mono8");
+		}
+		else
+		{
+			imagePtr = cv_bridge::toCvShare(image, "bgr8");
+		}
+
+		if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
+		   imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
+		{
+			NODELET_ERROR("Input type must be disparity=32FC1 or 16SC1");
+			return;
+		}
+
+		cv::Mat disparity;
+		if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
+		{
+			disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_32FC1, const_cast<uchar*>(imageDisparity->image.data.data()));
+		}
+		else
+		{
+			disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_16SC1, const_cast<uchar*>(imageDisparity->image.data.data()));
+		}
+
+		if(cloudPub_.getNumSubscribers())
+		{
+			ros::WallTime time = ros::WallTime::now();
+
+			cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
+
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
+			rtabmap::CameraModel leftModel = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
+			UASSERT(disparity.cols == leftModel.imageWidth() && disparity.rows == leftModel.imageHeight());
+			UASSERT(imagePtr->image.cols == leftModel.imageWidth() && imagePtr->image.rows == leftModel.imageHeight());
+			rtabmap::StereoCameraModel stereoModel(imageDisparity->f, imageDisparity->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), imageDisparity->T);
+			pcl::IndicesPtr indices(new std::vector<int>);
+			pclCloud = rtabmap::util3d::cloudFromDisparityRGB(
+					cv::Mat(imagePtr->image, roi),
+					cv::Mat(disparity, roi),
+					stereoModel,
+					decimation_,
+					maxDepth_,
+					minDepth_,
+					indices.get());
+
+			processAndPublish(pclCloud, indices, imageDisparity->header);
+
+			NODELET_DEBUG("point_cloud_xyzrgb from disparity time = %f s", (ros::WallTime::now() - time).toSec());
+		}
+	}
+
+	void stereoCallback(const sensor_msgs::ImageConstPtr& imageLeft,
+			const sensor_msgs::ImageConstPtr& imageRight,
+			const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
+			const sensor_msgs::CameraInfoConstPtr& camInfoRight)
+	{
+		if(!(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+				imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+				imageLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+				imageLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
+				imageLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
+				imageLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
+			!(imageRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+				imageRight->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+				imageRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+				imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
+				imageRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
+				imageRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
+		{
+			NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (enc=%s)", imageLeft->encoding.c_str());
+			return;
+		}
+
+		if(cloudPub_.getNumSubscribers())
+		{
+			ros::WallTime time = ros::WallTime::now();
+
+			cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
+			if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+				imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+			{
+				ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8");
+			}
+			else
+			{
+				ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8");
+			}
+			ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8");
+
+			if(roiRatios_[0]!=0.0f || roiRatios_[1]!=0.0f || roiRatios_[2]!=0.0f || roiRatios_[3]!=0.0f)
+			{
+				ROS_WARN("\"roi_ratios\" set but ignored for stereo images.");
+			}
+
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
+			pcl::IndicesPtr indices(new std::vector<int>);
+			pclCloud = rtabmap::util3d::cloudFromStereoImages(
+					ptrLeftImage->image,
+					ptrRightImage->image,
+					rtabmap_conversions::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight),
+					decimation_,
+					maxDepth_,
+					minDepth_,
+					indices.get(),
+					stereoBMParameters_);
+
+			processAndPublish(pclCloud, indices, imageLeft->header);
+
+			NODELET_DEBUG("point_cloud_xyzrgb from stereo time = %f s", (ros::WallTime::now() - time).toSec());
+		}
+	}
+
+	void rgbdImageCallback(const rtabmap_msgs::RGBDImageConstPtr & image)
+	{
+		if(cloudPub_.getNumSubscribers())
+		{
+			ros::WallTime time = ros::WallTime::now();
+
+			rtabmap::SensorData data = rtabmap_conversions::rgbdImageFromROS(image);
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
+			pcl::IndicesPtr indices(new std::vector<int>);
+			if(data.isValid())
+			{
+				pclCloud = rtabmap::util3d::cloudRGBFromSensorData(
+						data,
+						decimation_,
+						maxDepth_,
+						minDepth_,
+						indices.get(),
+						stereoBMParameters_,
+						roiRatios_);
+
+				processAndPublish(pclCloud, indices, image->header);
+			}
+
+			NODELET_DEBUG("point_cloud_xyzrgb from rgbd_image time = %f s", (ros::WallTime::now() - time).toSec());
+		}
+	}
+
+	void processAndPublish(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::Header & header)
+	{
+		if(indices->size() && voxelSize_ > 0.0)
+		{
+			pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
+			pclCloud->is_dense = true;
+		}
+
+		// Do radius filtering after voxel filtering ( a lot faster)
+		if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
+		{
+			if(pclCloud->is_dense)
+			{
+				indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
+			}
+			else
+			{
+				indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_);
+			}
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
+			pcl::copyPointCloud(*pclCloud, *indices, *tmp);
+			pclCloud = tmp;
+		}
+
+		sensor_msgs::PointCloud2 rosCloud;
+		if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_ > 0 || normalRadius_ > 0.0f))
+		{
+			//compute normals
+			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+			pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
+			if(filterNaNs_)
+			{
+				pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
+			}
+			pcl::toROSMsg(*pclCloudNormal, rosCloud);
+		}
+		else
+		{
+			if(filterNaNs_ && !pclCloud->is_dense)
+			{
+				pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
+			}
+			pcl::toROSMsg(*pclCloud, rosCloud);
+		}
+		rosCloud.header.stamp = header.stamp;
+		rosCloud.header.frame_id = header.frame_id;
+
+		//publish the message
+		cloudPub_.publish(rosCloud);
+	}
+
+private:
+
+	double maxDepth_;
+	double minDepth_;
+	double voxelSize_;
+	int decimation_;
+	double noiseFilterRadius_;
+	int noiseFilterMinNeighbors_;
+	int normalK_;
+	double normalRadius_;
+	bool filterNaNs_;
+	std::vector<float> roiRatios_;
+	rtabmap::ParametersMap stereoBMParameters_;
+
+	ros::Publisher cloudPub_;
+
+	ros::Subscriber rgbdImageSub_;
+
+	image_transport::SubscriberFilter imageSub_;
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+
+	message_filters::Subscriber<stereo_msgs::DisparityImage> imageDisparitySub_;
+
+	image_transport::SubscriberFilter imageLeft_;
+	image_transport::SubscriberFilter imageRight_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
+	message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyApproxSyncDisparityPolicy;
+	message_filters::Synchronizer<MyApproxSyncDisparityPolicy> * approxSyncDisparity_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncStereoPolicy;
+	message_filters::Synchronizer<MyApproxSyncStereoPolicy> * approxSyncStereo_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
+	message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyExactSyncDisparityPolicy;
+	message_filters::Synchronizer<MyExactSyncDisparityPolicy> * exactSyncDisparity_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncStereoPolicy;
+	message_filters::Synchronizer<MyExactSyncStereoPolicy> * exactSyncStereo_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_util::PointCloudXYZRGB, nodelet::Nodelet);
+}
+

rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp

@@ -0,0 +1,311 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+#include <ros/publisher.h>
+#include <ros/subscriber.h>
+
+#include <rtabmap_conversions/MsgConversion.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/utilite/ULogger.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 <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/transforms.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+
+namespace rtabmap_util
+{
+
+class PointCloudToDepthImage : public nodelet::Nodelet
+{
+public:
+	PointCloudToDepthImage() :
+		listener_(0),
+		waitForTransform_(0.1),
+		fillHolesSize_ (0),
+		fillHolesError_(0.1),
+		fillIterations_(1),
+		decimation_(1),
+		upscale_(false),
+		upscaleDepthErrorRatio_(0.02),
+		approxSync_(0),
+		exactSync_(0)
+			{}
+
+	virtual ~PointCloudToDepthImage()
+	{
+		delete listener_;
+		if(approxSync_)
+		{
+			delete approxSync_;
+		}
+		if(exactSync_)
+		{
+			delete exactSync_;
+		}
+	}
+
+private:
+	virtual void onInit()
+	{
+		listener_ = new tf::TransformListener();
+
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		bool approx = true;
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
+		pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
+		pnh.param("fill_holes_size", fillHolesSize_, fillHolesSize_);
+		pnh.param("fill_holes_error", fillHolesError_, fillHolesError_);
+		pnh.param("fill_iterations", fillIterations_, fillIterations_);
+		pnh.param("decimation", decimation_, decimation_);
+		pnh.param("approx", approx, approx);
+		pnh.param("upscale", upscale_, upscale_);
+		pnh.param("upscale_depth_error_ratio", upscaleDepthErrorRatio_, upscaleDepthErrorRatio_);
+
+		if(fixedFrameId_.empty() && approx)
+		{
+			ROS_FATAL("fixed_frame_id should be set when using approximate "
+					"time synchronization (approx=true)! If the robot "
+					"is moving, it could be \"odom\". If not moving, it "
+					"could be \"base_link\".");
+		}
+
+		ROS_INFO("Params:");
+		ROS_INFO("  approx=%s", approx?"true":"false");
+		ROS_INFO("  queue_size=%d", queueSize);
+		ROS_INFO("  fixed_frame_id=%s", fixedFrameId_.c_str());
+		ROS_INFO("  wait_for_transform=%fs", waitForTransform_);
+		ROS_INFO("  fill_holes_size=%d pixels (0=disabled)", fillHolesSize_);
+		ROS_INFO("  fill_holes_error=%f", fillHolesError_);
+		ROS_INFO("  fill_iterations=%d", fillIterations_);
+		ROS_INFO("  decimation=%d", decimation_);
+		ROS_INFO("  upscale=%s (upscale_depth_error_ratio=%f)", upscale_?"true":"false", upscaleDepthErrorRatio_);
+
+		image_transport::ImageTransport it(nh);
+		depthImage16Pub_ = it.advertise("image_raw", 1); // 16 bits unsigned in mm
+		depthImage32Pub_ = it.advertise("image", 1);     // 32 bits float in meters
+		pointCloudTransformedPub_ = nh.advertise<sensor_msgs::PointCloud2>(nh.resolveName("cloud")+"_transformed", 1);
+		cameraInfo16Pub_ = nh.advertise<sensor_msgs::CameraInfo>(nh.resolveName("image_raw")+"/camera_info", 1);
+		cameraInfo32Pub_ = nh.advertise<sensor_msgs::CameraInfo>(nh.resolveName("image")+"/camera_info", 1);
+
+		if(approx)
+		{
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_);
+			approxSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, boost::placeholders::_1, boost::placeholders::_2));
+		}
+		else
+		{
+			fixedFrameId_.clear();
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_);
+			exactSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, boost::placeholders::_1, boost::placeholders::_2));
+		}
+
+		pointCloudSub_.subscribe(nh, "cloud", 1);
+		cameraInfoSub_.subscribe(nh, "camera_info", 1);
+	}
+
+	void callback(
+			const sensor_msgs::PointCloud2ConstPtr & pointCloud2Msg,
+			const sensor_msgs::CameraInfoConstPtr & cameraInfoMsg)
+	{
+		if(depthImage32Pub_.getNumSubscribers() > 0 || depthImage16Pub_.getNumSubscribers() > 0)
+		{
+			double cloudStamp = pointCloud2Msg->header.stamp.toSec();
+			double infoStamp = cameraInfoMsg->header.stamp.toSec();
+
+			rtabmap::Transform cloudDisplacement = rtabmap::Transform::getIdentity();
+			if(!fixedFrameId_.empty())
+			{
+				// approx sync
+				cloudDisplacement = rtabmap_conversions::getTransform(
+						pointCloud2Msg->header.frame_id,
+						fixedFrameId_,
+						cameraInfoMsg->header.stamp,
+						pointCloud2Msg->header.stamp,
+						*listener_,
+						waitForTransform_);
+			}
+
+			if(cloudDisplacement.isNull())
+			{
+				return;
+			}
+
+			rtabmap::Transform cloudToCamera = rtabmap_conversions::getTransform(
+					pointCloud2Msg->header.frame_id,
+					cameraInfoMsg->header.frame_id,
+					cameraInfoMsg->header.stamp,
+					*listener_,
+					waitForTransform_);
+
+			if(cloudToCamera.isNull())
+			{
+				return;
+			}
+
+			rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera;
+
+			rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfoMsg, localTransform);
+			sensor_msgs::CameraInfo cameraInfoMsgOut = *cameraInfoMsg;
+			if(decimation_ > 1)
+			{
+				if(model.imageWidth()%decimation_ == 0 && model.imageHeight()%decimation_ == 0)
+				{
+					float scale = 1.0f/float(decimation_);
+					model = model.scaled(scale);
+
+					rtabmap_conversions::cameraModelToROS(model, cameraInfoMsgOut);
+				}
+				else
+				{
+					ROS_ERROR("decimation (%d) not valid for image size %dx%d",
+							decimation_,
+							model.imageWidth(),
+							model.imageHeight());
+				}
+			}
+
+			UASSERT_MSG(pointCloud2Msg->data.size() == pointCloud2Msg->row_step*pointCloud2Msg->height,
+					uFormat("data=%d row_step=%d height=%d", pointCloud2Msg->data.size(), pointCloud2Msg->row_step, pointCloud2Msg->height).c_str());
+
+			pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
+			pcl_conversions::toPCL(*pointCloud2Msg, *cloud);
+
+			cv_bridge::CvImage depthImage;
+
+			if(cloud->data.empty())
+			{
+				ROS_WARN("Received an empty cloud on topic \"%s\"! A depth image with all zeros is returned.", pointCloudSub_.getTopic().c_str());
+				depthImage.image = cv::Mat::zeros(model.imageSize(), CV_32FC1);
+			}
+			else
+			{
+				depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform());
+
+				if(fillHolesSize_ > 0 && fillIterations_ > 0)
+				{
+					for(int i=0; i<fillIterations_;++i)
+					{
+						depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
+					}
+				}
+
+				if(pointCloudTransformedPub_.getNumSubscribers()>0)
+				{
+					sensor_msgs::PointCloud2 pointCloud2Out;
+					pcl_ros::transformPointCloud(model.localTransform().inverse().toEigen4f(), *pointCloud2Msg, pointCloud2Out);
+					pointCloud2Out.header = cameraInfoMsg->header;
+					pointCloudTransformedPub_.publish(pointCloud2Out);
+				}
+			}
+
+			depthImage.header = cameraInfoMsg->header;
+
+			if(decimation_>1 && upscale_)
+			{
+				depthImage.image = rtabmap::util2d::interpolate(depthImage.image, decimation_, upscaleDepthErrorRatio_);
+			}
+
+			if(depthImage32Pub_.getNumSubscribers())
+			{
+				depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
+				depthImage32Pub_.publish(depthImage.toImageMsg());
+				if(cameraInfo32Pub_.getNumSubscribers())
+				{
+					cameraInfo32Pub_.publish(cameraInfoMsgOut);
+				}
+			}
+
+			if(depthImage16Pub_.getNumSubscribers())
+			{
+				depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
+				depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image);
+				depthImage16Pub_.publish(depthImage.toImageMsg());
+				if(cameraInfo16Pub_.getNumSubscribers())
+				{
+					cameraInfo16Pub_.publish(cameraInfoMsgOut);
+				}
+			}
+
+			if( cloudStamp != pointCloud2Msg->header.stamp.toSec() ||
+				infoStamp != cameraInfoMsg->header.stamp.toSec())
+			{
+				NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+						"sure the node publishing the topics doesn't override the same data after publishing them. A "
+						"solution is to use this node within another nodelet manager. Stamps: "
+						"cloud=%f->%f info=%f->%f",
+						cloudStamp, pointCloud2Msg->header.stamp.toSec(),
+						infoStamp, cameraInfoMsg->header.stamp.toSec());
+			}
+		}
+	}
+
+private:
+	image_transport::Publisher depthImage16Pub_;
+	image_transport::Publisher depthImage32Pub_;
+	ros::Publisher cameraInfo16Pub_;
+	ros::Publisher cameraInfo32Pub_;
+	ros::Publisher pointCloudTransformedPub_;
+	message_filters::Subscriber<sensor_msgs::PointCloud2> pointCloudSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+	std::string fixedFrameId_;
+	tf::TransformListener * listener_;
+	double waitForTransform_;
+	int fillHolesSize_;
+	double fillHolesError_;
+	int fillIterations_;
+	int decimation_;
+	bool upscale_;
+	double upscaleDepthErrorRatio_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
+	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::CameraInfo> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_util::PointCloudToDepthImage, nodelet::Nodelet);
+}

rtabmap_util/src/nodelets/rgbd_relay.cpp

@@ -0,0 +1,205 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CompressedImage.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/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <boost/thread.hpp>
+
+#include "rtabmap_msgs/RGBDImage.h"
+#include "rtabmap_conversions/MsgConversion.h"
+
+#include "rtabmap/core/Compression.h"
+#include "rtabmap/utilite/UConversion.h"
+
+namespace rtabmap_util
+{
+
+class RGBDRelay : public nodelet::Nodelet
+{
+public:
+	RGBDRelay() :
+		compress_(false),
+		uncompress_(false)
+	{}
+
+	virtual ~RGBDRelay()
+	{
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		bool approxSync = true;
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("compress", compress_, compress_);
+		pnh.param("uncompress", uncompress_, uncompress_);
+
+		NODELET_INFO("%s: queue_size  = %d", getName().c_str(), queueSize);
+
+		rgbdImageSub_ = nh.subscribe("rgbd_image", 1, &RGBDRelay::callback, this);
+		rgbdImagePub_ = nh.advertise<rtabmap_msgs::RGBDImage>(nh.resolveName("rgbd_image") + "_relay", 1);
+	}
+
+	void callback(const rtabmap_msgs::RGBDImageConstPtr& input)
+	{
+		if(rgbdImagePub_.getNumSubscribers())
+		{
+			if(!compress_ && !uncompress_)
+			{
+				//just republish it
+				rgbdImagePub_.publish(input);
+				return;
+			}
+
+			rtabmap_msgs::RGBDImage output;
+			output.header = input->header;
+			output.rgb_camera_info = input->rgb_camera_info;
+			output.depth_camera_info = input->depth_camera_info;
+			output.key_points = input->key_points;
+			output.points = input->points;
+			output.descriptors = input->descriptors;
+			output.global_descriptor = input->global_descriptor;
+
+			rtabmap::StereoCameraModel stereoModel = rtabmap_conversions::stereoCameraModelFromROS(input->rgb_camera_info, input->depth_camera_info, rtabmap::Transform::getIdentity());
+
+			if(compress_)
+			{
+				if(!input->rgb_compressed.data.empty())
+				{
+					// already compressed, just copy pointer
+					output.rgb_compressed = input->rgb_compressed;
+				}
+				else if(!input->rgb.data.empty())
+				{
+#ifdef CV_BRIDGE_HYDRO
+					ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
+#else
+					cv_bridge::CvImageConstPtr rgb = cv_bridge::toCvShare(input->rgb, input);
+					rgb->toCompressedImageMsg(output.rgb_compressed, cv_bridge::JPG);
+#endif
+				}
+
+				if(!input->depth_compressed.data.empty())
+				{
+					// already compressed, just copy pointer
+					output.depth_compressed = input->depth_compressed;
+				}
+				else if(!input->depth.data.empty())
+				{
+					if(stereoModel.isValidForProjection())
+					{
+						// right stereo image
+						cv_bridge::CvImageConstPtr imageRightPtr = cv_bridge::toCvShare(input->depth, input);
+						imageRightPtr->toCompressedImageMsg(output.depth_compressed, cv_bridge::JPG);
+					}
+					else
+					{
+						// depth image
+						cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(input->depth, input);
+						output.depth_compressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
+						output.depth_compressed.format = "png";
+					}
+				}
+			}
+			if(uncompress_)
+			{
+				if(!input->rgb.data.empty())
+				{
+					// already raw, just copy pointer
+					output.rgb = input->rgb;
+				}
+				if(!input->rgb_compressed.data.empty())
+				{
+#ifdef CV_BRIDGE_HYDRO
+					ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
+#else
+					cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(output.rgb);
+#endif
+				}
+
+				if(!input->depth.data.empty())
+				{
+					// already raw, just copy pointer
+					output.depth = input->depth;
+				}
+				else if(input->depth_compressed.format.compare("jpg")==0)
+				{
+					// right stereo image
+#ifdef CV_BRIDGE_HYDRO
+					ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
+#else
+					cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(output.depth);
+#endif
+				}
+				else
+				{
+					// depth image
+					cv_bridge::CvImagePtr ptr = boost::make_shared<cv_bridge::CvImage>();
+					ptr->header = input->depth_compressed.header;
+					ptr->image = rtabmap::uncompressImage(input->depth_compressed.data);
+					ROS_ASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1);
+					ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
+					ptr->toImageMsg(output.depth);
+				}
+			}
+
+			rgbdImagePub_.publish(output);
+		}
+	}
+
+private:
+
+	bool compress_;
+	bool uncompress_;
+	ros::Subscriber rgbdImageSub_;
+	ros::Publisher rgbdImagePub_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_util::RGBDRelay, nodelet::Nodelet);
+}
+

rtabmap_util/src/nodelets/rgbd_split.cpp

@@ -0,0 +1,147 @@
+/*
+Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CompressedImage.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/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <boost/thread.hpp>
+
+#include "rtabmap_msgs/RGBDImage.h"
+#include "rtabmap_conversions/MsgConversion.h"
+
+#include "rtabmap/core/Compression.h"
+#include "rtabmap/utilite/UConversion.h"
+
+namespace rtabmap_util
+{
+
+class RGBDSplit : public nodelet::Nodelet
+{
+public:
+	RGBDSplit()
+	{}
+
+	virtual ~RGBDSplit()
+	{
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		pnh.param("queue_size", queueSize, queueSize);
+
+		NODELET_INFO("%s: queue_size  = %d", getName().c_str(), queueSize);
+
+		ros::NodeHandle rgb_nh(nh, nh.resolveName("rgbd_image") + "/rgb");
+		ros::NodeHandle depth_nh(nh, nh.resolveName("rgbd_image") + "/depth");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::ImageTransport depth_it(depth_nh);
+
+		rgbPub_ = rgb_it.advertiseCamera("image", 1);
+		depthPub_ = depth_it.advertiseCamera("image", 1);
+
+		rgbdImageSub_ = nh.subscribe("rgbd_image", 1, &RGBDSplit::callback, this);
+	}
+
+	void callback(const rtabmap_msgs::RGBDImageConstPtr& input)
+	{
+		if(rgbPub_.getNumSubscribers())
+		{
+			sensor_msgs::Image outputImage;
+			sensor_msgs::CameraInfo outputCameraInfo;
+			outputImage.header = outputCameraInfo.header = input->header;
+			outputCameraInfo = input->rgb_camera_info;
+
+			if(!input->rgb.data.empty())
+			{
+				// already raw, just copy pointer
+				outputImage = input->rgb;
+			}
+			else if(!input->rgb_compressed.data.empty())
+			{
+#ifdef CV_BRIDGE_HYDRO
+				ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
+#else
+				cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(outputImage);
+#endif
+			}
+			rgbPub_.publish(outputImage, outputCameraInfo);
+		}
+
+		if(depthPub_.getNumSubscribers())
+		{
+			sensor_msgs::Image outputImage;
+			sensor_msgs::CameraInfo outputCameraInfo;
+			outputCameraInfo = input->depth_camera_info;
+
+			if(!input->depth.data.empty())
+			{
+				// already raw, just copy pointer
+				outputImage = input->depth;
+			}
+			else if(!input->depth_compressed.data.empty())
+			{
+#ifdef CV_BRIDGE_HYDRO
+				ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
+#else
+				cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(outputImage);
+#endif
+			}
+			outputImage.header = outputCameraInfo.header = input->header;
+			depthPub_.publish(outputImage, outputCameraInfo);
+		}
+	}
+
+private:
+	ros::Subscriber rgbdImageSub_;
+	image_transport::CameraPublisher rgbPub_;
+	image_transport::CameraPublisher depthPub_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_util::RGBDSplit, nodelet::Nodelet);
+}
+