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.

CMakeLists.txt

@@ -18,7 +18,7 @@ find_package(rviz)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.11.9 REQUIRED)
+find_package(RTABMap 0.11.10 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 

include/rtabmap_ros/CoreWrapper.h

@@ -77,6 +77,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <actionlib_msgs/GoalStatusArray.h>
 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
 
+namespace rtabmap {
+class StereoDense;
+}
+
 class CoreWrapper
 {
 public:
@@ -226,7 +230,8 @@ private:
 	bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
-	bool getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res);
+	bool getMapDataCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res);
+	bool getMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
 	bool getProjMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
 	bool getGridMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
 	bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
@@ -239,7 +244,7 @@ private:
 	bool octomapFullCallback(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
 #endif
 
-	rtabmap::ParametersMap loadParameters(const std::string & configFile);
+	void loadParameters(const std::string & configFile, rtabmap::ParametersMap & parameters);
 	void saveParameters(const std::string & configFile);
 
 	void publishLoop(double tfDelay, double tfTolerance);
@@ -489,6 +494,7 @@ private:
 	ros::ServiceServer setLogErrorSrv_;
 	ros::ServiceServer getMapDataSrv_;
 	ros::ServiceServer getProjMapSrv_;
+	ros::ServiceServer getMapSrv_;
 	ros::ServiceServer getGridMapSrv_;
 	ros::ServiceServer publishMapDataSrv_;
 	ros::ServiceServer setGoalSrv_;
@@ -504,6 +510,7 @@ private:
 
 	boost::thread* transformThread_;
 
+	bool stereoToDepth_;
 	float rate_;
 	bool createIntermediateNodes_;
 	ros::Time time_;

include/rtabmap_ros/MapsManager.h

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define MAPSMANAGER_H_
 
 #include <rtabmap/core/Signature.h>
+#include <rtabmap/core/Parameters.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <ros/time.h>
@@ -37,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 class OctoMap;
 class Memory;
+class OccupancyGrid;
 
 }  // namespace rtabmap
 
@@ -46,6 +48,8 @@ public:
 	virtual ~MapsManager();
 	void clear();
 	bool hasSubscribers() const;
+	void backwardCompatibilityParameters(rtabmap::ParametersMap & parameters) const;
+	void setParameters(const rtabmap::ParametersMap & parameters);
 
 	std::map<int, rtabmap::Transform> getFilteredPoses(
 			const std::map<int, rtabmap::Transform> & poses);
@@ -53,10 +57,7 @@ public:
 	std::map<int, rtabmap::Transform> updateMapCaches(
 			const std::map<int, rtabmap::Transform> & poses,
 			const rtabmap::Memory * memory,
-			bool updateCloud,
-			bool updateProj,
 			bool updateGrid,
-			bool updateScan,
 			bool updateOctomap,
 			const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>());
 
@@ -65,12 +66,6 @@ public:
 			const ros::Time & stamp,
 			const std::string & mapFrameId);
 
-	cv::Mat generateProjMap(
-			const std::map<int, rtabmap::Transform> & filteredPoses,
-			float & xMin,
-			float & yMin,
-			float & gridCellSize);
-
 	cv::Mat generateGridMap(
 			const std::map<int, rtabmap::Transform> & filteredPoses,
 			float & xMin,
@@ -81,37 +76,20 @@ public:
 
 private:
 	// mapping stuff
-	int cloudDecimation_;
-	double cloudMaxDepth_;
-	double cloudMinDepth_;
-	double cloudVoxelSize_;
-	double cloudFloorCullingHeight_;
-	double cloudCeilingCullingHeight_;
 	bool cloudOutputVoxelized_;
-	bool cloudFrustumCulling_;
-	double cloudNoiseFilteringRadius_;
-	int cloudNoiseFilteringMinNeighbors_;
-	int scanDecimation_;
-	double scanVoxelSize_;
-	bool scanOutputVoxelized_;
-	double projMaxGroundAngle_;
-	int projMinClusterSize_;
-	double projMaxObstaclesHeight_;
-	double projMaxGroundHeight_;
-	bool projDetectFlatObstacles_;
-	bool projMapFrame_;
 	double gridCellSize_;
+	bool gridIncremental_;
 	double gridSize_;
 	bool gridEroded_;
 	double footprintRadius_;
-	bool gridUnknownSpaceFilled_;
-	double gridMaxUnknownSpaceFilledRange_;
 	double mapFilterRadius_;
 	double mapFilterAngle_;
 	bool mapCacheCleanup_;
 	bool negativePosesIgnored_;
 
 	ros::Publisher cloudMapPub_;
+	ros::Publisher cloudGroundPub_;
+	ros::Publisher cloudObstaclesPub_;
 	ros::Publisher projMapPub_;
 	ros::Publisher gridMapPub_;
 	ros::Publisher scanMapPub_;
@@ -121,15 +99,22 @@ private:
 	ros::Publisher octoMapEmptySpace_;
 	ros::Publisher octoMapProj_;
 
-	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
-	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
-	std::map<int, std::vector<rtabmap::CameraModel> > cameraModels_;
-	std::map<int, std::pair<cv::Mat, cv::Mat> > projMaps_; // <ground, obstacles>
+	std::map<int, rtabmap::Transform> assembledGroundPoses_;
+	std::map<int, rtabmap::Transform> assembledObstaclePoses_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledObstacles_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledGround_;
+
+	std::map<int, rtabmap::Transform> gridPoses_;
+	cv::Mat gridMap_;
 	std::map<int, std::pair<cv::Mat, cv::Mat> > gridMaps_; // <ground, obstacles>
+	std::map<int, cv::Point3f> gridMapsViewpoints_;
+
+	rtabmap::OccupancyGrid * occupancyGrid_;
 
 	rtabmap::OctoMap * octomap_;
 	int octomapTreeDepth_;
-	bool octomapGroundIsObstacle_;
+
+	rtabmap::ParametersMap parameters_;
 };
 
 #endif /* MAPSMANAGER_H_ */

msg/NodeData.msg

@@ -33,11 +33,19 @@ geometry_msgs/Transform[] localTransform
 uint8[] laserScan
 int32 laserScanMaxPts
 float32 laserScanMaxRange
+geometry_msgs/Transform laserScanLocalTransform
 
 # compressed user data
 # use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
 uint8[] userData
 
+# compressed occupancy grid
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] grid_ground
+uint8[] grid_obstacles
+float32 grid_cell_size
+Point3f grid_view_point
+
 # std::multimap<wordId, cv::Keypoint>
 # std::multimap<wordId, pcl::PointXYZ>
 int32[] wordIds

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap_ros</name>
-  <version>0.11.9</version>
+  <version>0.11.10</version>
   <description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

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,
+			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,
+			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())
+		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());
+	}
+	else
 	{
-			//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.");
 	}
-	}
-	return false;
+	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,19 +721,63 @@ 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)
+		{
+			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,
+				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,19 +978,63 @@ 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)
+		{
+			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,
+				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),
+					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),
+					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,
+						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,