0.11.10: Updated LaserScanInfo interface. Updated NodeData msg with occupancy grids and laser scan local transform. MapsManager is now using directly the occupancy grids saved in nodes for 3D cloud, octomap and grid map.

src/CoreNode.cpp

@@ -51,9 +51,8 @@ int main(int argc, char** argv)
 		else if(strcmp(argv[i], "--params") == 0 || strcmp(argv[i], "--params-all") == 0)
 		{
 			rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
-			uInsert(parameters,
-					std::make_pair(rtabmap::Parameters::kRtabmapWorkingDirectory(),
-					UDirectory::homeDir()+"/.ros")); // change default to ~/.ros
+			uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDCreateOccupancyGrid(), "true")); // default true in ROS
+			uInsert(parameters,	rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros")); // change default to ~/.ros
 
 			if(strcmp(argv[i], "--params") == 0)
 			{

src/CoreWrapper.cpp

@@ -52,6 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/OdometryEvent.h>
 #include <rtabmap/core/Version.h>
+#include <rtabmap/core/StereoDense.h>
 
 #include <pcl_conversions/pcl_conversions.h>
 
@@ -124,6 +125,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 		stereoApproxTFSync_(0),
 		stereoExactTFSync_(0),
 		transformThread_(0),
+		stereoToDepth_(false),
 		rate_(Parameters::defaultRtabmapDetectionRate()),
 		createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
 		time_(ros::Time::now()),
@@ -208,6 +210,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 	pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
 	pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
 	pnh.param("flip_scan", flipScan_, flipScan_);
+	pnh.param("stereo_to_depth", stereoToDepth_, stereoToDepth_);
 
 	if(!tfPrefix.empty())
 	{
@@ -285,12 +288,15 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 		databasePath_ = UDirectory::currentDir(true) + databasePath_;
 	}
 
+	ParametersMap allParameters = Parameters::getDefaultParameters();
+	uInsert(allParameters, ParametersPair(Parameters::kRGBDCreateOccupancyGrid(), "true")); // default true in ROS
+	uInsert(allParameters, ParametersPair(Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros")); // change default to ~/.ros
+
 	// load parameters
-	parameters_ = loadParameters(configPath_);
+	loadParameters(configPath_, parameters_);
 
 	// update parameters with user input parameters (private)
-	uInsert(parameters_, std::make_pair(Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros")); // change default to ~/.ros
-	for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
+	for(ParametersMap::iterator iter=allParameters.begin(); iter!=allParameters.end(); ++iter)
 	{
 		std::string vStr;
 		bool vBool;
@@ -299,31 +305,31 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 		if(pnh.getParam(iter->first, vStr))
 		{
 			ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
-			iter->second = vStr;
 
 			if(iter->first.compare(Parameters::kRtabmapWorkingDirectory()) == 0)
 			{
-				iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir());
+				vStr = uReplaceChar(vStr, '~', UDirectory::homeDir());
 			}
 			else if(iter->first.compare(Parameters::kKpDictionaryPath()) == 0)
 			{
-				iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir());
+				vStr = uReplaceChar(vStr, '~', UDirectory::homeDir());
 			}
+			uInsert(parameters_, ParametersPair(iter->first, vStr));
 		}
 		else if(pnh.getParam(iter->first, vBool))
 		{
 			ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
-			iter->second = uBool2Str(vBool);
+			uInsert(parameters_, ParametersPair(iter->first, uBool2Str(vBool)));
 		}
 		else if(pnh.getParam(iter->first, vDouble))
 		{
 			ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
-			iter->second = uNumber2Str(vDouble);
+			uInsert(parameters_, ParametersPair(iter->first, uNumber2Str(vDouble)));
 		}
 		else if(pnh.getParam(iter->first, vInt))
 		{
 			ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
-			iter->second = uNumber2Str(vInt);
+			uInsert(parameters_, ParametersPair(iter->first, uNumber2Str(vInt)));
 		}
 	}
 
@@ -345,7 +351,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 			if(iter->second.first)
 			{
 				// can be migrated
-				parameters_.at(iter->second.second)= vStr;
+				uInsert(parameters_, ParametersPair(iter->second.second, vStr));
 				ROS_WARN("Rtabmap: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
 						iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
 			}
@@ -365,6 +371,25 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 		}
 	}
 
+	// Backward compatibility (MapsManager)
+	mapsManager_.backwardCompatibilityParameters(parameters_);
+
+	if((subscribeScan2d || subscribeScan3d) && parameters_.find(Parameters::kGridFromDepth()) == parameters_.end())
+	{
+		ROS_WARN("Setting \"%s\" parameter to false (default true) as \"subscribe_scan\" or \"subscribe_scan_cloud\" is "
+				"true. The occupancy grid map will be constructed from "
+				"laser scans. To get occupancy grid map from cloud projection, set \"%s\" "
+				"to true. To suppress this warning, "
+				"add <param name=\"%s\" type=\"string\" value=\"false\"/>",
+				Parameters::kGridFromDepth().c_str(),
+				Parameters::kGridFromDepth().c_str(),
+				Parameters::kGridFromDepth().c_str());
+		parameters_.insert(ParametersPair(Parameters::kGridFromDepth(), "false"));
+	}
+
+	// Add all other parameters (not copied if already exists)
+	parameters_.insert(allParameters.begin(), allParameters.end());
+
 	// set public parameters
 	nh.setParam("is_rtabmap_paused", paused_);
 	for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
@@ -391,9 +416,9 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 		if(!subscribeDepth && !subscribeStereo)
 		{
 			ROS_WARN("ROS param subscribe_depth and subscribe_stereo are false, but RTAB-Map "
-					  "parameter \"RGBD/Enabled\" is true! Please set subscribe_depth or subscribe_stereo "
-					  "to true to use rtabmap node for RGB-D SLAM, or set \"RGBD/Enabled\" to false for loop closure "
-					  "detection on images-only.");
+					  "parameter \"%s\" is true! Please set subscribe_depth or subscribe_stereo "
+					  "to true to use rtabmap node for RGB-D SLAM, or set \"%s\" to false for loop closure "
+					  "detection on images-only.", Parameters::kRGBDEnabled().c_str(), Parameters::kRGBDEnabled().c_str());
 		}
 	}
 
@@ -419,6 +444,12 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 		ROS_INFO("rtabmap: database_path parameter not set, the map will not be saved.");
 	}
 
+	mapsManager_.setParameters(parameters_);
+	if(subscribeStereo)
+	{
+		ROS_INFO("rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false");
+	}
+
 	// Init RTAB-Map
 	rtabmap_.init(parameters_, databasePath_);
 
@@ -436,7 +467,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
 	backupDatabase_ = nh.advertiseService("backup", &CoreWrapper::backupDatabaseCallback, this);
 	setModeLocalizationSrv_ = nh.advertiseService("set_mode_localization", &CoreWrapper::setModeLocalizationCallback, this);
 	setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this);
-	getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
+	getMapDataSrv_ = nh.advertiseService("get_map_data", &CoreWrapper::getMapDataCallback, this);
+	getMapSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
 	getGridMapSrv_ = nh.advertiseService("get_grid_map", &CoreWrapper::getGridMapCallback, this);
 	getProjMapSrv_ = nh.advertiseService("get_proj_map", &CoreWrapper::getProjMapCallback, this);
 	publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
@@ -550,9 +582,8 @@ CoreWrapper::~CoreWrapper()
 	printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str());
 }
 
-ParametersMap CoreWrapper::loadParameters(const std::string & configFile)
+void CoreWrapper::loadParameters(const std::string & configFile, ParametersMap & parameters)
 {
-	ParametersMap parameters = Parameters::getDefaultParameters();
 	if(!configFile.empty())
 	{
 		ROS_INFO("Loading parameters from %s", configFile.c_str());
@@ -562,9 +593,6 @@ ParametersMap CoreWrapper::loadParameters(const std::string & configFile)
 		}
 		Parameters::readINI(configFile.c_str(), parameters);
 	}
-	// otherwise take default parameters
-
-	return parameters;
 }
 
 void CoreWrapper::saveParameters(const std::string & configFile)
@@ -993,12 +1021,14 @@ void CoreWrapper::commonDepthCallback(
 	}
 
 	cv::Mat scan;
+	Transform scanLocalTransform = Transform::getIdentity();
 	if(scan2dMsg.get() != 0)
 	{
 		// make sure the frame of the laser is updated too
-		if(getTransform(frameId_,
+		scanLocalTransform = getTransform(frameId_,
 				scan2dMsg->header.frame_id,
-				scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)).isNull())
+				scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment));
+		if(scanLocalTransform.isNull())
 		{
 			ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scan2dMsg->header.stamp.toSec());
 			return;
@@ -1007,7 +1037,7 @@ void CoreWrapper::commonDepthCallback(
 		//transform in frameId_ frame
 		sensor_msgs::PointCloud2 scanOut;
 		laser_geometry::LaserProjection projection;
-		projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_);
+		projection.transformLaserScanToPointCloud(scan2dMsg->header.frame_id, *scan2dMsg, scanOut, tfListener_);
 		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::fromROSMsg(scanOut, *pclScan);
 
@@ -1024,8 +1054,7 @@ void CoreWrapper::commonDepthCallback(
 				}
 				else
 				{
-					Transform t = odomT.inverse() * sensorT;
-					pclScan = util3d::transformPointCloud(pclScan, t);
+					scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform;
 				}
 
 			}
@@ -1051,13 +1080,12 @@ void CoreWrapper::commonDepthCallback(
 		}
 
 		// sync with odometry stamp
-		Transform localScanTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp);
-		if(localScanTransform.isNull())
+		scanLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp);
+		if(scanLocalTransform.isNull())
 		{
 			ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec());
 			return;
 		}
-		Transform laserOdomT = localScanTransform;
 		if(lastPoseStamp_ != scan3dMsg->header.stamp)
 		{
 			if(!odomT.isNull())
@@ -1070,7 +1098,7 @@ void CoreWrapper::commonDepthCallback(
 				}
 				else
 				{
-					laserOdomT = odomT.inverse() * sensorT * localScanTransform;
+					scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform;
 				}
 
 			}
@@ -1080,10 +1108,6 @@ void CoreWrapper::commonDepthCallback(
 		{
 			pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
 			pcl::fromROSMsg(*scan3dMsg, *pclScan);
-			if(!laserOdomT.isIdentity())
-			{
-				pclScan = util3d::transformPointCloud(pclScan, laserOdomT);
-			}
 			scan = util3d::laserScanFromPointCloud(*pclScan);
 		}
 		else
@@ -1091,11 +1115,6 @@ void CoreWrapper::commonDepthCallback(
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 			pcl::fromROSMsg(*scan3dMsg, *pclScan);
 
-			if(!laserOdomT.isIdentity())
-			{
-				pclScan = util3d::transformPointCloud(pclScan, laserOdomT);
-			}
-
 			if(scanCloudNormalK_ > 0)
 			{
 				//compute normals
@@ -1126,8 +1145,10 @@ void CoreWrapper::commonDepthCallback(
 	}
 
 	SensorData data(scan,
-			scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():(genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0),
-			scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f),
+			LaserScanInfo(
+					scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():(genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0),
+					scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f),
+					scanLocalTransform),
 			rgb,
 			depth,
 			cameraModels,
@@ -1167,6 +1188,77 @@ void CoreWrapper::commonStereoCallback(
 		return;
 	}
 
+	cv_bridge::CvImagePtr ptrLeftImage, ptrRightImage;
+	if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+	   leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+	{
+		ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "mono8");
+	}
+	else
+	{
+		ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "bgr8");
+	}
+	ptrRightImage = cv_bridge::toCvCopy(rightImageMsg, "mono8");
+
+	Transform localTransform = getTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp);
+	if(localTransform.isNull())
+	{
+		return;
+	}
+
+	rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform);
+
+	if(stereoModel.baseline() > 10.0)
+	{
+		static bool shown = false;
+		if(!shown)
+		{
+			ROS_WARN("Detected baseline (%f m) is quite large! Is your "
+					 "right camera_info P(0,3) correctly set? Note that "
+					 "baseline=-P(0,3)/P(0,0). This warning is printed only once.",
+					 stereoModel.baseline());
+			shown = true;
+		}
+	}
+
+	if(stereoToDepth_)
+	{
+		// cv::stereoBM() see "$ rosrun rtabmap_ros rtabmap --params | grep StereoBM" for parameters
+		cv::Mat disparity = util2d::disparityFromStereoImages(ptrLeftImage->image, ptrRightImage->image, parameters_);
+		if(disparity.empty())
+		{
+			ROS_ERROR("Could not compute disparity image (\"stereo_to_depth\" is true)!");
+			return;
+		}
+		cv::Mat depth = util2d::depthFromDisparity(
+						disparity,
+						stereoModel.left().fx(),
+						stereoModel.baseline());
+
+		if(depth.empty())
+		{
+			ROS_ERROR("Could not compute depth image (\"stereo_to_depth\" is true)!");
+			return;
+		}
+		UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
+
+		// move to common depth callback
+		cv_bridge::CvImage imgDepth;
+		if(depth.type() == CV_16UC1)
+		{
+			imgDepth.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
+		}
+		else // CV_32FC1
+		{
+			imgDepth.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
+		}
+		imgDepth.image = depth;
+		sensor_msgs::ImagePtr depthMsg = imgDepth.toImageMsg();
+		depthMsg->header = leftImageMsg->header;
+
+		commonDepthCallback(odomFrameId, leftImageMsg, depthMsg, leftCamInfoMsg, scan2dMsg, scan3dMsg);
+	}
+
 	//for sync transform
 	Transform odomT = getTransform(odomFrameId, frameId_, lastPoseStamp_);
 	if(odomT.isNull() && !odomFrameId_.empty())
@@ -1175,12 +1267,6 @@ void CoreWrapper::commonStereoCallback(
 				odomFrameId.c_str(), frameId_.c_str());
 	}
 
-	Transform localTransform = getTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp);
-	if(localTransform.isNull())
-	{
-		return;
-	}
-
 	// sync with odometry stamp
 	if(lastPoseStamp_ != leftImageMsg->header.stamp)
 	{
@@ -1196,12 +1282,14 @@ void CoreWrapper::commonStereoCallback(
 	}
 
 	cv::Mat scan;
+	Transform scanLocalTransform = Transform::getIdentity();
 	if(scan2dMsg.get() != 0)
 	{
 		// make sure the frame of the laser is updated too
-		if(getTransform(frameId_,
+		scanLocalTransform = getTransform(frameId_,
 				scan2dMsg->header.frame_id,
-				scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)).isNull())
+				scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment));
+		if(scanLocalTransform.isNull())
 		{
 			ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scan2dMsg->header.stamp.toSec());
 			return;
@@ -1211,7 +1299,7 @@ void CoreWrapper::commonStereoCallback(
 		sensor_msgs::PointCloud2 scanOut;
 		laser_geometry::LaserProjection projection;
 		//projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfBuffer_);
-		projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_);
+		projection.transformLaserScanToPointCloud(scan2dMsg->header.frame_id, *scan2dMsg, scanOut, tfListener_);
 		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::fromROSMsg(scanOut, *pclScan);
 
@@ -1225,9 +1313,7 @@ void CoreWrapper::commonStereoCallback(
 				{
 					return;
 				}
-				Transform t = odomT.inverse() * sensorT;
-				pclScan = util3d::transformPointCloud(pclScan, t);
-
+				scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform;
 			}
 		}
 
@@ -1252,13 +1338,12 @@ void CoreWrapper::commonStereoCallback(
 		}
 
 		// sync with odometry stamp
-		Transform localScanTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp);
-		if(localScanTransform.isNull())
+		scanLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp);
+		if(scanLocalTransform.isNull())
 		{
 			ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec());
 			return;
 		}
-		Transform laserOdomT = localScanTransform;
 		if(lastPoseStamp_ != scan3dMsg->header.stamp)
 		{
 			if(!odomT.isNull())
@@ -1271,7 +1356,7 @@ void CoreWrapper::commonStereoCallback(
 				}
 				else
 				{
-					laserOdomT = odomT.inverse() * sensorT * localScanTransform;
+					scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform;
 				}
 
 			}
@@ -1282,11 +1367,6 @@ void CoreWrapper::commonStereoCallback(
 			pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
 			pcl::fromROSMsg(*scan3dMsg, *pclScan);
 
-			if(!laserOdomT.isIdentity())
-			{
-				pclScan = util3d::transformPointCloud(pclScan, laserOdomT);
-			}
-
 			scan = util3d::laserScanFromPointCloud(*pclScan);
 		}
 		else
@@ -1294,11 +1374,6 @@ void CoreWrapper::commonStereoCallback(
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 			pcl::fromROSMsg(*scan3dMsg, *pclScan);
 
-			if(!laserOdomT.isIdentity())
-			{
-				pclScan = util3d::transformPointCloud(pclScan, laserOdomT);
-			}
-
 			if(scanCloudNormalK_ > 0)
 			{
 				//compute normals
@@ -1314,33 +1389,6 @@ void CoreWrapper::commonStereoCallback(
 		}
 	}
 
-	cv_bridge::CvImagePtr ptrLeftImage, ptrRightImage;
-	if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
-	   leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
-	{
-		ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "mono8");
-	}
-	else
-	{
-		ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "bgr8");
-	}
-	ptrRightImage = cv_bridge::toCvCopy(rightImageMsg, "mono8");
-
-	rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform);
-
-	if(stereoModel.baseline() > 10.0)
-	{
-		static bool shown = false;
-		if(!shown)
-		{
-			ROS_WARN("Detected baseline (%f m) is quite large! Is your "
-					 "right camera_info P(0,3) correctly set? Note that "
-					 "baseline=-P(0,3)/P(0,0). This warning is printed only once.",
-					 stereoModel.baseline());
-			shown = true;
-		}
-	}
-
 	ros::Time stamp =   scan2dMsg.get() != 0?scan2dMsg->header.stamp:
 						scan3dMsg.get() != 0?scan3dMsg->header.stamp:
 					    leftImageMsg->header.stamp;
@@ -1352,8 +1400,10 @@ void CoreWrapper::commonStereoCallback(
 	}
 
 	SensorData data(scan,
-			scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():scan3dMsg.get() != 0?scanCloudMaxPoints_:0,
-			scan2dMsg.get() != 0?scan2dMsg->range_max:0,
+			LaserScanInfo(
+					scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():scan3dMsg.get() != 0?scanCloudMaxPoints_:0,
+					scan2dMsg.get() != 0?scan2dMsg->range_max:0,
+					scanLocalTransform),
 			ptrLeftImage->image,
 			ptrRightImage->image,
 			stereoModel,
@@ -1621,9 +1671,6 @@ void CoreWrapper::process(
 						rtabmap_.getMemory(),
 						false,
 						false,
-						false,
-						false,
-						false,
 						tmpSignature);
 
 				timeUpdateMaps = timer.ticks();
@@ -1916,6 +1963,7 @@ bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Emp
 		ROS_INFO("RTAB-Map rate detection = %f Hz", rate_);
 	}
 	rtabmap_.parseParameters(parameters_);
+	mapsManager_.setParameters(parameters_);
 	return true;
 }
 
@@ -2039,7 +2087,7 @@ bool CoreWrapper::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Respon
 	return true;
 }
 
-bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res)
+bool CoreWrapper::getMapDataCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res)
 {
 	ROS_INFO("rtabmap: Getting map (global=%s optimized=%s graphOnly=%s)...",
 			req.global?"true":"false",
@@ -2083,65 +2131,41 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
 
 bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res)
 {
-	std::map<int, rtabmap::Transform> filteredPoses;
-	filteredPoses = mapsManager_.updateMapCaches(
-			rtabmap_.getLocalOptimizedPoses(),
-			rtabmap_.getMemory(),
-			false,
-			true,
-			false,
-			false,
-			false);
-	if(filteredPoses.size())
+	if(parameters_.find(Parameters::kGridFromDepth()) != parameters_.end() &&
+		!uStr2Bool(parameters_.at(Parameters::kGridFromDepth())))
 	{
-		// create the projection map
-		float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
-		cv::Mat pixels = mapsManager_.generateProjMap(filteredPoses, xMin, yMin, gridCellSize);
-
-		if(!pixels.empty())
-		{
-			//init
-			res.map.info.resolution = gridCellSize;
-			res.map.info.origin.position.x = 0.0;
-			res.map.info.origin.position.y = 0.0;
-			res.map.info.origin.position.z = 0.0;
-			res.map.info.origin.orientation.x = 0.0;
-			res.map.info.origin.orientation.y = 0.0;
-			res.map.info.origin.orientation.z = 0.0;
-			res.map.info.origin.orientation.w = 1.0;
-
-			res.map.info.width = pixels.cols;
-			res.map.info.height = pixels.rows;
-			res.map.info.origin.position.x = xMin;
-			res.map.info.origin.position.y = yMin;
-			res.map.data.resize(res.map.info.width * res.map.info.height);
-
-			memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height);
-
-			res.map.header.frame_id = mapFrameId_;
-			res.map.header.stamp = ros::Time::now();
-			return true;
-		}
+		ROS_WARN("/get_proj_map service is deprecated! Call /get_grid_map service "
+					"instead with <param name=\"%s\" type=\"string\" value=\"true\"/>. "
+					"Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see "
+					"all occupancy grid parameters.",
+					Parameters::kGridFromDepth().c_str());
 	}
-	return false;
+	else
+	{
+		ROS_WARN("/get_proj_map service is deprecated! Call /get_grid_map service instead.");
+	}
+	return getGridMapCallback(req, res);
 }
 
 bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res)
+{
+	ROS_WARN("/get_grid_map service is deprecated! Call /get_map service instead.");
+	return getMapCallback(req, res);
+}
+
+bool CoreWrapper::getMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res)
 {
 	std::map<int, rtabmap::Transform> filteredPoses;
 	filteredPoses = mapsManager_.updateMapCaches(
 			rtabmap_.getLocalOptimizedPoses(),
 			rtabmap_.getMemory(),
-			false,
-			false,
 			true,
-			false,
 			false);
 	if(filteredPoses.size())
 	{
 		// create the grid map
 		float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
-		cv::Mat pixels = mapsManager_.generateProjMap(filteredPoses, xMin, yMin, gridCellSize);
+		cv::Mat pixels = mapsManager_.generateGridMap(filteredPoses, xMin, yMin, gridCellSize);
 
 		if(!pixels.empty())
 		{
@@ -2247,9 +2271,6 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 						rtabmap_.getMemory(),
 						false,
 						false,
-						false,
-						false,
-						false,
 						signatures);
 			}
 			else
@@ -2715,7 +2736,7 @@ bool CoreWrapper::octomapBinaryCallback(
 	res.map.header.stamp = ros::Time::now();
 
 	std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
-	poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, false, false, false, true);
+	poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true);
 
 	const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
 	bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map);
@@ -2731,7 +2752,7 @@ bool CoreWrapper::octomapFullCallback(
 	res.map.header.stamp = ros::Time::now();
 
 	std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
-	poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, false, false, false, true);
+	poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true);
 
 	const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
 	bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map);

src/GuiWrapper.cpp

@@ -407,9 +407,9 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
 			getMapSrv.request.global = cmdEvent->value1().toBool();
 			getMapSrv.request.optimized = cmdEvent->value2().toBool();
 			getMapSrv.request.graphOnly = cmdEvent->value3().toBool();
-			if(!ros::service::call("get_map", getMapSrv))
+			if(!ros::service::call("get_map_data", getMapSrv))
 			{
-				ROS_WARN("Can't call \"get_map\" service");
+				ROS_WARN("Can't call \"get_map_data\" service");
 				this->post(new RtabmapEvent3DMap(1)); // service error
 			}
 			else
@@ -588,6 +588,7 @@ void GuiWrapper::commonDepthCallback(
 	cv::Mat depth;
 	std::vector<CameraModel> cameraModels;
 	cv::Mat scan;
+	Transform scanLocalTransform = Transform::getIdentity();
 	rtabmap::OdometryInfo info;
 	bool ignoreData = false;
 
@@ -693,15 +694,20 @@ void GuiWrapper::commonDepthCallback(
 		if(scan2dMsg.get() != 0)
 		{
 			// make sure the frame of the laser is updated too
-			if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull())
+			scanLocalTransform = getTransform(
+					frameId_,
+					scan2dMsg->header.frame_id,
+					scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment));
+			if(scanLocalTransform.isNull())
 			{
+				ROS_ERROR("TF of received scan at time %fs is not set, aborting rtabmapviz update.", scan2dMsg->header.stamp.toSec());
 				return;
 			}
 
 			//transform in frameId_ frame
 			sensor_msgs::PointCloud2 scanOut;
 			laser_geometry::LaserProjection projection;
-			projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_);
+			projection.transformLaserScanToPointCloud(scan2dMsg->header.frame_id, *scan2dMsg, scanOut, tfListener_);
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 			pcl::fromROSMsg(scanOut, *pclScan);
 
@@ -715,18 +721,62 @@ void GuiWrapper::commonDepthCallback(
 					{
 						return;
 					}
-					Transform t = odomT.inverse() * sensorT;
-					pclScan = util3d::transformPointCloud(pclScan, t);
-
+					scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform;
 				}
 			}
 			scan = util3d::laserScanFromPointCloud(*pclScan);
 		}
 		else if(scan3dMsg.get() != 0)
 		{
-			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
-			pcl::fromROSMsg(*scan3dMsg, *pclScan);
-			scan = util3d::laserScanFromPointCloud(*pclScan);
+			bool containNormals = false;
+			for(unsigned int i=0; i<scan3dMsg->fields.size(); ++i)
+			{
+				if(scan3dMsg->fields[i].name.compare("normal_x") == 0)
+				{
+					containNormals = true;
+					break;
+				}
+			}
+
+			// sync with odometry stamp
+			scanLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp);
+			if(scanLocalTransform.isNull())
+			{
+				ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec());
+				return;
+			}
+			if(odomHeader.stamp != scan3dMsg->header.stamp)
+			{
+				if(!odomT.isNull())
+				{
+					Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan3dMsg->header.stamp);
+					if(sensorT.isNull())
+					{
+						ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry "
+								"stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan3dMsg->header.stamp.toSec(), odomHeader.stamp.toSec());
+					}
+					else
+					{
+						scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform;
+					}
+
+				}
+			}
+
+			if(containNormals)
+			{
+				pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
+				pcl::fromROSMsg(*scan3dMsg, *pclScan);
+
+				scan = util3d::laserScanFromPointCloud(*pclScan);
+			}
+			else
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::fromROSMsg(*scan3dMsg, *pclScan);
+
+				scan = util3d::laserScanFromPointCloud(*pclScan);
+			}
 		}
 
 		if(odomInfoMsg.get())
@@ -749,8 +799,10 @@ void GuiWrapper::commonDepthCallback(
 	rtabmap::OdometryEvent odomEvent(
 		rtabmap::SensorData(
 				scan,
-				scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
-				scan2dMsg.get()?(int)scan2dMsg->range_max:0,
+				LaserScanInfo(
+						scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
+						scan2dMsg.get()?(int)scan2dMsg->range_max:0,
+						scanLocalTransform),
 				rgb,
 				depth,
 				cameraModels,
@@ -854,6 +906,7 @@ void GuiWrapper::commonStereoCallback(
 	cv::Mat left;
 	cv::Mat right;
 	cv::Mat scan;
+	Transform scanLocalTransform = Transform::getIdentity();
 	rtabmap::StereoCameraModel stereoModel;
 	rtabmap::OdometryInfo info;
 	bool ignoreData = false;
@@ -898,15 +951,20 @@ void GuiWrapper::commonStereoCallback(
 		if(scan2dMsg.get() != 0)
 		{
 			// make sure the frame of the laser is updated too
-			if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull())
+			scanLocalTransform = getTransform(
+					frameId_,
+					scan2dMsg->header.frame_id,
+					scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment));
+			if(scanLocalTransform.isNull())
 			{
+				ROS_ERROR("TF of received scan at time %fs is not set, aborting rtabmapviz update.", scan2dMsg->header.stamp.toSec());
 				return;
 			}
 
 			//transform in frameId_ frame
 			sensor_msgs::PointCloud2 scanOut;
 			laser_geometry::LaserProjection projection;
-			projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_);
+			projection.transformLaserScanToPointCloud(scan2dMsg->header.frame_id, *scan2dMsg, scanOut, tfListener_);
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 			pcl::fromROSMsg(scanOut, *pclScan);
 
@@ -920,18 +978,62 @@ void GuiWrapper::commonStereoCallback(
 					{
 						return;
 					}
-					Transform t = odomT.inverse() * sensorT;
-					pclScan = util3d::transformPointCloud(pclScan, t);
-
+					scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform;
 				}
 			}
 			scan = util3d::laserScan2dFromPointCloud(*pclScan);
 		}
 		else if(scan3dMsg.get() != 0)
 		{
-			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
-			pcl::fromROSMsg(*scan3dMsg, *pclScan);
-			scan = util3d::laserScanFromPointCloud(*pclScan);
+			bool containNormals = false;
+			for(unsigned int i=0; i<scan3dMsg->fields.size(); ++i)
+			{
+				if(scan3dMsg->fields[i].name.compare("normal_x") == 0)
+				{
+					containNormals = true;
+					break;
+				}
+			}
+
+			// sync with odometry stamp
+			scanLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp);
+			if(scanLocalTransform.isNull())
+			{
+				ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec());
+				return;
+			}
+			if(odomHeader.stamp != scan3dMsg->header.stamp)
+			{
+				if(!odomT.isNull())
+				{
+					Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan3dMsg->header.stamp);
+					if(sensorT.isNull())
+					{
+						ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry "
+								"stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan3dMsg->header.stamp.toSec(), odomHeader.stamp.toSec());
+					}
+					else
+					{
+						scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform;
+					}
+
+				}
+			}
+
+			if(containNormals)
+			{
+				pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
+				pcl::fromROSMsg(*scan3dMsg, *pclScan);
+
+				scan = util3d::laserScanFromPointCloud(*pclScan);
+			}
+			else
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::fromROSMsg(*scan3dMsg, *pclScan);
+
+				scan = util3d::laserScanFromPointCloud(*pclScan);
+			}
 		}
 
 		if(odomInfoMsg.get())
@@ -954,8 +1056,10 @@ void GuiWrapper::commonStereoCallback(
 	rtabmap::OdometryEvent odomEvent(
 		rtabmap::SensorData(
 				scan,
-				scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
-				scan2dMsg.get()?(int)scan2dMsg->range_max:0,
+				LaserScanInfo(
+						scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
+						scan2dMsg.get()?(int)scan2dMsg->range_max:0,
+						scanLocalTransform),
 				left,
 				right,
 				stereoModel,

src/MapAssemblerNode.cpp

@@ -99,9 +99,6 @@ public:
 				0,
 				false,
 				false,
-				false,
-				false,
-				false,
 				nodes_);
 
 		mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);

src/MsgConversion.cpp

@@ -332,16 +332,42 @@ rtabmap::CameraModel cameraModelFromROS(
 		const sensor_msgs::CameraInfo & camInfo,
 		const rtabmap::Transform & localTransform)
 {
-	image_geometry::PinholeCameraModel model;
-	model.fromCameraInfo(camInfo);
+	cv::Mat D;
+	if(camInfo.D.size())
+	{
+		D = cv::Mat(1, camInfo.D.size(), CV_64FC1);
+		memcpy(D.data, camInfo.D.data(), D.cols*sizeof(double));
+	}
+
+	cv:: Mat K;
+	UASSERT(camInfo.K.empty() || camInfo.K.size() == 9);
+	if(!camInfo.K.empty())
+	{
+		K = cv::Mat(3, 3, CV_64FC1);
+		memcpy(K.data, camInfo.K.elems, 9*sizeof(double));
+	}
+
+	cv:: Mat R;
+	UASSERT(camInfo.R.empty() || camInfo.R.size() == 9);
+	if(!camInfo.R.empty())
+	{
+		R = cv::Mat(3, 3, CV_64FC1);
+		memcpy(R.data, camInfo.R.elems, 9*sizeof(double));
+	}
+
+	cv:: Mat P;
+	UASSERT(camInfo.P.empty() || camInfo.P.size() == 12);
+	if(!camInfo.P.empty())
+	{
+		P = cv::Mat(3, 4, CV_64FC1);
+		memcpy(P.data, camInfo.P.elems, 12*sizeof(double));
+	}
+
 	return rtabmap::CameraModel(
-			model.fx(),
-			model.fy(),
-			model.cx(),
-			model.cy(),
-			localTransform,
-			0.0,
-			cv::Size(model.fullResolution().width, model.fullResolution().height));
+			"ros",
+			cv::Size(camInfo.width, camInfo.height),
+			K, D, R, P,
+			localTransform);
 }
 void cameraModelToROS(
 		const rtabmap::CameraModel & model,
@@ -401,16 +427,11 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS(
 		const sensor_msgs::CameraInfo & rightCamInfo,
 		const rtabmap::Transform & localTransform)
 {
-	image_geometry::StereoCameraModel model;
-	model.fromCameraInfo(leftCamInfo, rightCamInfo);
 	return rtabmap::StereoCameraModel(
-			model.left().fx(),
-			model.left().fy(),
-			model.left().cx(),
-			model.left().cy(),
-			model.baseline(),
-			localTransform,
-			cv::Size(model.left().fullResolution().width, model.left().fullResolution().height));
+			"ros",
+			cameraModelFromROS(leftCamInfo, localTransform),
+			cameraModelFromROS(rightCamInfo, localTransform),
+			rtabmap::Transform());
 }
 
 void mapDataFromROS(
@@ -587,8 +608,10 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 			stereoModel.isValidForProjection()?
 				rtabmap::SensorData(
 					compressedMatFromBytes(msg.laserScan),
-					msg.laserScanMaxPts,
-					msg.laserScanMaxRange,
+					rtabmap::LaserScanInfo(
+							msg.laserScanMaxPts,
+							msg.laserScanMaxRange,
+							transformFromGeometryMsg(msg.laserScanLocalTransform)),
 					compressedMatFromBytes(msg.image),
 					compressedMatFromBytes(msg.depth),
 					stereoModel,
@@ -597,8 +620,10 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 					compressedMatFromBytes(msg.userData)):
 				rtabmap::SensorData(
 					compressedMatFromBytes(msg.laserScan),
-					msg.laserScanMaxPts,
-					msg.laserScanMaxRange,
+					rtabmap::LaserScanInfo(
+							msg.laserScanMaxPts,
+							msg.laserScanMaxRange,
+							transformFromGeometryMsg(msg.laserScanLocalTransform)),
 					compressedMatFromBytes(msg.image),
 					compressedMatFromBytes(msg.depth),
 					models,
@@ -608,6 +633,11 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 	s.setWords(words);
 	s.setWords3(words3D);
 	s.setWordsDescriptors(wordsDescriptors);
+	s.sensorData().setOccupancyGrid(
+			compressedMatFromBytes(msg.grid_ground),
+			compressedMatFromBytes(msg.grid_obstacles),
+			msg.grid_cell_size,
+			point3fFromROS(msg.grid_view_point));
 	return s;
 }
 void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg)
@@ -624,8 +654,13 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 	compressedMatToBytes(signature.sensorData().depthOrRightCompressed(), msg.depth);
 	compressedMatToBytes(signature.sensorData().laserScanCompressed(), msg.laserScan);
 	compressedMatToBytes(signature.sensorData().userDataCompressed(), msg.userData);
-	msg.laserScanMaxPts = signature.sensorData().laserScanMaxPts();
-	msg.laserScanMaxRange = signature.sensorData().laserScanMaxRange();
+	compressedMatToBytes(signature.sensorData().gridGroundCellsCompressed(), msg.grid_ground);
+	compressedMatToBytes(signature.sensorData().gridObstacleCellsCompressed(), msg.grid_obstacles);
+	point3fToROS(signature.sensorData().gridViewPoint(), msg.grid_view_point);
+	msg.grid_cell_size = signature.sensorData().gridCellSize();
+	msg.laserScanMaxPts = signature.sensorData().laserScanInfo().maxPoints();
+	msg.laserScanMaxRange = signature.sensorData().laserScanInfo().maxRange();
+	transformToGeometryMsg(signature.sensorData().laserScanInfo().localTransform(), msg.laserScanLocalTransform);
 	msg.baseline = 0;
 	if(signature.sensorData().cameraModels().size())
 	{

src/nodelets/icp_odometry.cpp

@@ -93,9 +93,10 @@ private:
 	void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg)
 	{
 		// make sure the frame of the laser is updated too
-		if(getTransform(this->frameId(),
+		Transform localScanTransform = getTransform(this->frameId(),
 				scanMsg->header.frame_id,
-				scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)).isNull())
+				scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment));
+		if(localScanTransform.isNull())
 		{
 			ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec());
 			return;
@@ -104,7 +105,7 @@ private:
 		//transform in frameId_ frame
 		sensor_msgs::PointCloud2 scanOut;
 		laser_geometry::LaserProjection projection;
-		projection.transformLaserScanToPointCloud(this->frameId(), *scanMsg, scanOut, this->tfListener());
+		projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
 		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::fromROSMsg(scanOut, *pclScan);
 
@@ -112,8 +113,7 @@ private:
 
 		rtabmap::SensorData data(
 				scan,
-				(int)scanMsg->ranges.size(),
-				scanMsg->range_max,
+				LaserScanInfo((int)scanMsg->ranges.size(), scanMsg->range_max, localScanTransform),
 				cv::Mat(),
 				cv::Mat(),
 				CameraModel(),
@@ -147,10 +147,6 @@ private:
 		{
 			pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
 			pcl::fromROSMsg(*cloudMsg, *pclScan);
-			if(!localScanTransform.isIdentity())
-			{
-				pclScan = util3d::transformPointCloud(pclScan, localScanTransform);
-			}
 			scan = util3d::laserScanFromPointCloud(*pclScan);
 		}
 		else
@@ -158,11 +154,6 @@ private:
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 			pcl::fromROSMsg(*cloudMsg, *pclScan);
 
-			if(!localScanTransform.isIdentity())
-			{
-				pclScan = util3d::transformPointCloud(pclScan, localScanTransform);
-			}
-
 			if(scanCloudNormalK_ > 0)
 			{
 				//compute normals
@@ -179,8 +170,7 @@ private:
 
 		rtabmap::SensorData data(
 				scan,
-				scanCloudMaxPoints_,
-				0,
+				LaserScanInfo(scanCloudMaxPoints_, 0, localScanTransform),
 				cv::Mat(),
 				cv::Mat(),
 				CameraModel(),

src/nodelets/rgbdicp_odometry.cpp

@@ -266,12 +266,14 @@ private:
 				cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
 
 				cv::Mat scan;
+				Transform localScanTransform = Transform::getIdentity();
 				if(scanMsg.get() != 0)
 				{
 					// make sure the frame of the laser is updated too
-					if(getTransform(this->frameId(),
+					localScanTransform = getTransform(this->frameId(),
 							scanMsg->header.frame_id,
-							scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)).isNull())
+							scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment));
+					if(localScanTransform.isNull())
 					{
 						ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec());
 						return;
@@ -280,7 +282,7 @@ private:
 					//transform in frameId_ frame
 					sensor_msgs::PointCloud2 scanOut;
 					laser_geometry::LaserProjection projection;
-					projection.transformLaserScanToPointCloud(this->frameId(), *scanMsg, scanOut, this->tfListener());
+					projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
 					pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 					pcl::fromROSMsg(scanOut, *pclScan);
 
@@ -297,7 +299,7 @@ private:
 							break;
 						}
 					}
-					Transform localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp);
+					localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp);
 					if(localScanTransform.isNull())
 					{
 						ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec());
@@ -308,10 +310,6 @@ private:
 					{
 						pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
 						pcl::fromROSMsg(*cloudMsg, *pclScan);
-						if(!localScanTransform.isIdentity())
-						{
-							pclScan = util3d::transformPointCloud(pclScan, localScanTransform);
-						}
 						scan = util3d::laserScanFromPointCloud(*pclScan);
 					}
 					else
@@ -319,11 +317,6 @@ private:
 						pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 						pcl::fromROSMsg(*cloudMsg, *pclScan);
 
-						if(!localScanTransform.isIdentity())
-						{
-							pclScan = util3d::transformPointCloud(pclScan, localScanTransform);
-						}
-
 						if(scanCloudNormalK_ > 0)
 						{
 							//compute normals
@@ -341,8 +334,10 @@ private:
 
 				rtabmap::SensorData data(
 						scan,
-						scanMsg.get() != 0?(int)scanMsg->ranges.size():cloudMsg.get() != 0?scanCloudMaxPoints_:0,
-						scanMsg.get() != 0?scanMsg->range_max:0,
+						LaserScanInfo(
+								scanMsg.get() != 0?(int)scanMsg->ranges.size():cloudMsg.get() != 0?scanCloudMaxPoints_:0,
+								scanMsg.get() != 0?scanMsg->range_max:0,
+								localScanTransform),
 						ptrImage->image,
 						ptrDepth->image,
 						rtabmapModel,