Updated ros-pkg with new rtabmap messages including retrieved data

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

rtabmap/launch/config/appearance_gui.ini

@@ -38,3 +38,4 @@ General\voxelSize2=0.01
 General\decimation2=1
 General\maxDepth2=4
 General\showScans2=true
+General\meshing0=false

rtabmap/msg/Info.msg

@@ -7,11 +7,8 @@
 Header header
 
 int32 refId
-int32 refMapId
 int32 loopClosureId
-int32 loopClosureMapId
 int32 localLoopClosureId
-int32 localLoopClosureMapId
 
 # std::map<int, Poses> poses;
 int32[] nodeIds

rtabmap/msg/InfoEx.msg

@@ -6,21 +6,18 @@
 Header header
 
 int32 refId
-int32 refMapId
 int32 loopClosureId
-int32 loopClosureMapId
 int32 localLoopClosureId
-int32 localLoopClosureMapId
 
-# std::map<int, Poses> poses;
-int32[] nodeIds
-geometry_msgs/Pose[] nodePoses
+# The map data (rgb, depth, depthConstant, depth2D, localTransform, poses, map ids, mapCorrection)
+rtabmap/MapData data
 
-geometry_msgs/Transform mapCorrection
 geometry_msgs/Transform loopClosureTransform
-
 geometry_msgs/Pose currentPose
 
+####
+# For statistics and visualization below...
+####
 # std::map<int, float> posterior;
 int32[] posteriorKeys
 float32[] posteriorValues
@@ -41,29 +38,6 @@ int32[] weightsValues
 string[] statsKeys
 float32[] statsValues
 
-#compressed image
-# use rtabmap::util3d::uncompressImage() from <rtabmap/core/util3d.h>
-uint8[] refImage
-uint8[] loopImage
-
-#compressed depth
-# use rtabmap::util3d::uncompressImage() from <rtabmap/core/util3d.h>
-uint8[] refDepth
-uint8[] loopDepth
-
-#compressed depth2d
-# use rtabmap::util3d::uncompressData() from <rtabmap/core/util3d.h>
-uint8[] refDepth2D
-uint8[] loopDepth2D
-
-#camera info
-float32 refDepthConstant
-float32 loopDepthConstant
-
-#local transform
-geometry_msgs/Transform refLocalTransform
-geometry_msgs/Transform loopLocalTransform
-
 #
 # For features2d : std::multimap<int, cv::Keypoint> words
 #

rtabmap/msg/MapData.msg

@@ -1,6 +1,10 @@
 
 Header header
 
+# Map ids
+int32[] mapIDs
+int32[] maps
+
 # compressed images
 # use rtabmap::util3d::uncompressImage() from <rtabmap/core/util3d.h>
 # std::map<int, uint8[]> images;
@@ -17,7 +21,7 @@ rtabmap/Bytes[] depths
 # use rtabmap::util3d::uncompressData() from <rtabmap/core/util3d.h>
 # std::map<int, uint8[]> depths2d;
 int32[] depth2DIDs
-rtabmap/Bytes[] depths2D
+rtabmap/Bytes[] depth2Ds
 
 # depth constants
 int32[] depthConstantIDs

rtabmap/src/CoreWrapper.cpp

@@ -650,12 +650,12 @@ bool CoreWrapper::publishMapDataCallback(std_srvs::Empty::Request&, std_srvs::Em
 		}
 
 		msg->depth2DIDs.resize(depths2d.size());
-		msg->depths2D.resize(depths2d.size());
+		msg->depth2Ds.resize(depths2d.size());
 		i=0;
 		for(std::map<int, std::vector<unsigned char> >::iterator iter = depths2d.begin(); iter!=depths2d.end(); ++iter)
 		{
 			msg->depth2DIDs[i] = iter->first;
-			msg->depths2D[i].bytes = iter->second;
+			msg->depth2Ds[i].bytes = iter->second;
 			++i;
 		}
 
@@ -709,11 +709,8 @@ void CoreWrapper::publishStats(const Statistics & stats)
 			msg->header.frame_id = mapFrameId_;
 
 			msg->refId = stats.refImageId();
-			msg->refMapId = stats.refImageMapId();
 			msg->loopClosureId = stats.loopClosureId();
-			msg->loopClosureMapId = stats.loopClosureMapId();
 			msg->localLoopClosureId = stats.localLoopClosureId();
-			msg->localLoopClosureMapId = stats.localLoopClosureMapId();
 
 			msg->nodeIds.resize(stats.poses().size());
 			msg->nodePoses.resize(stats.poses().size());
@@ -742,34 +739,28 @@ void CoreWrapper::publishStats(const Statistics & stats)
 			msg->header.frame_id = mapFrameId_;
 
 			msg->refId = stats.refImageId();
-			msg->refMapId = stats.refImageMapId();
 			msg->loopClosureId = stats.loopClosureId();
-			msg->loopClosureMapId = stats.loopClosureMapId();
 			msg->localLoopClosureId = stats.localLoopClosureId();
-			msg->localLoopClosureMapId = stats.localLoopClosureMapId();
 
-			msg->nodeIds.resize(stats.poses().size());
-			msg->nodePoses.resize(stats.poses().size());
+			msg->data.poseIDs.resize(stats.poses().size());
+			msg->data.poses.resize(stats.poses().size());
 			int i=0;
 			for(std::map<int, Transform>::const_iterator iter = stats.poses().begin();
 				iter!=stats.poses().end();
 				++iter)
 			{
-				msg->nodeIds[i] = iter->first;
-				transformToPoseMsg(iter->second, msg->nodePoses[i]);
+				msg->data.poseIDs[i] = iter->first;
+				transformToPoseMsg(iter->second, msg->data.poses[i]);
 				++i;
 			}
 
-			transformToGeometryMsg(stats.mapCorrection(), msg->mapCorrection);
+			transformToGeometryMsg(stats.mapCorrection(), msg->data.mapCorrection);
 			transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform);
 			transformToPoseMsg(stats.currentPose(), msg->currentPose);
 
 			// Detailed info
 			if(stats.extended())
 			{
-				msg->refImage = stats.refImage();
-				msg->loopImage = stats.loopImage();
-
 				//Posterior, likelihood, childCount
 				msg->posteriorKeys = uKeys(stats.posterior());
 				msg->posteriorValues = uValues(stats.posterior());
@@ -819,17 +810,56 @@ void CoreWrapper::publishStats(const Statistics & stats)
 				msg->statsKeys = uKeys(stats.data());
 				msg->statsValues = uValues(stats.data());
 
+				msg->data.mapIDs = uKeys(stats.getMapIds());
+				msg->data.maps = uValues(stats.getMapIds());
+
 				//RGB-D SLAM data
-				msg->refDepth = stats.refDepth();
-				msg->refDepth2D = stats.refDepth2D();
-				msg->loopDepth = stats.loopDepth();
-				msg->loopDepth2D = stats.loopDepth2D();
+				msg->data.imageIDs.resize(stats.getImages().size());
+				msg->data.images.resize(stats.getImages().size());
+				index = 0;
+				for(std::map<int, std::vector<unsigned char> >::const_iterator i=stats.getImages().begin();
+					i!=stats.getImages().end();
+					++i)
+				{
+					msg->data.imageIDs[index] = i->first;
+					msg->data.images[index].bytes = i->second;
+				}
 
-				msg->refDepthConstant = stats.refDepthConstant();
-				msg->loopDepthConstant = stats.loopDepthConstant();
+				msg->data.depthIDs.resize(stats.getDepths().size());
+				msg->data.depths.resize(stats.getDepths().size());
+				index = 0;
+				for(std::map<int, std::vector<unsigned char> >::const_iterator i=stats.getDepths().begin();
+					i!=stats.getDepths().end();
+					++i)
+				{
+					msg->data.depthIDs[index] = i->first;
+					msg->data.depths[index].bytes = i->second;
+				}
 
-				transformToGeometryMsg(stats.refLocalTransform(), msg->refLocalTransform);
-				transformToGeometryMsg(stats.loopLocalTransform(), msg->loopLocalTransform);
+				msg->data.depth2DIDs.resize(stats.getDepth2ds().size());
+				msg->data.depth2Ds.resize(stats.getDepth2ds().size());
+				index = 0;
+				for(std::map<int, std::vector<unsigned char> >::const_iterator i=stats.getDepth2ds().begin();
+					i!=stats.getDepth2ds().end();
+					++i)
+				{
+					msg->data.depth2DIDs[index] = i->first;
+					msg->data.depth2Ds[index].bytes = i->second;
+				}
+
+				msg->data.localTransformIDs.resize(stats.getLocalTransforms().size());
+				msg->data.localTransforms.resize(stats.getLocalTransforms().size());
+				index = 0;
+				for(std::map<int, Transform>::const_iterator i=stats.getLocalTransforms().begin();
+					i!=stats.getLocalTransforms().end();
+					++i)
+				{
+					msg->data.localTransformIDs[index] = i->first;
+					transformToGeometryMsg(i->second, msg->data.localTransforms[index]);
+				}
+
+				msg->data.depthConstantIDs = uKeys(stats.getDepthConstants());
+				msg->data.depthConstants = uValues(stats.getDepthConstants());
 			}
 			infoPubEx_.publish(msg);
 		}

rtabmap/src/GridMapAssemblerNode.cpp

@@ -62,16 +62,19 @@ public:
 
 	void infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
 	{
-		if(!uContains(scans_, msg->refId) && msg->refDepth2D.size())
+		for(unsigned int i=0; i<msg->data.depth2DIDs.size() && i<msg->data.depth2Ds.size(); ++i)
 		{
-			cv::Mat depth2d = util3d::uncompressData(msg->refDepth2D);
-			scans_.insert(std::make_pair(msg->refId, util3d::depth2DToPointCloud(depth2d)));
+			if(!uContains(scans_, msg->data.depth2DIDs[i]))
+			{
+				cv::Mat depth2d = util3d::uncompressData(msg->data.depth2Ds[i].bytes);
+				scans_.insert(std::make_pair(msg->data.depth2DIDs[i], util3d::depth2DToPointCloud(depth2d)));
+			}
 		}
 
 		std::map<int, Transform> poses;
-		for(unsigned int i=0; i<msg->nodeIds.size() && i<msg->nodePoses.size(); ++i)
+		for(unsigned int i=0; i<msg->data.poseIDs.size() && i<msg->data.poses.size(); ++i)
 		{
-			poses.insert(std::make_pair(msg->nodeIds[i], transformFromPoseMsg(msg->nodePoses[i])));
+			poses.insert(std::make_pair(msg->data.poseIDs[i], transformFromPoseMsg(msg->data.poses[i])));
 		}
 
 		if(gridMap_.getNumSubscribers())
@@ -114,18 +117,18 @@ public:
 	{
 		std::map<int, std::vector<unsigned char> > depths2d;
 
-		if(msg->depth2DIDs.size() != msg->depths2D.size())
+		if(msg->depth2DIDs.size() != msg->depth2Ds.size())
 		{
 			ROS_WARN("grid_map_assembler: receiving map... depths2D and depth2DIDs are not the same size (%d vs %d)!",
-					(int)msg->depths2D.size(), (int)msg->depth2DIDs.size());
+					(int)msg->depth2Ds.size(), (int)msg->depth2DIDs.size());
 		}
 
 		// fill maps
-		for(unsigned int i=0; i<msg->depth2DIDs.size() && i < msg->depths2D.size(); ++i)
+		for(unsigned int i=0; i<msg->depth2DIDs.size() && i < msg->depth2Ds.size(); ++i)
 		{
 			if(!uContains(scans_, msg->depth2DIDs[i]))
 			{
-				cv::Mat depth2d = util3d::uncompressData(msg->depths2D[i].bytes);
+				cv::Mat depth2d = util3d::uncompressData(msg->depth2Ds[i].bytes);
 				scans_.insert(std::make_pair(msg->depth2DIDs[i], util3d::depth2DToPointCloud(depth2d)));
 			}
 		}

rtabmap/src/GuiWrapper.cpp

@@ -111,14 +111,8 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
 	stat.setExtended(true); // Extended
 
 	stat.setRefImageId(msg->refId);
-	stat.setRefImageMapId(msg->refMapId);
 	stat.setLoopClosureId(msg->loopClosureId);
-	stat.setLoopClosureMapId(msg->loopClosureMapId);
 	stat.setLocalLoopClosureId(msg->localLoopClosureId);
-	stat.setLocalLoopClosureMapId(msg->localLoopClosureMapId);
-
-	stat.setRefImage(msg->refImage);
-	stat.setLoopImage(msg->loopImage);
 
 	//Posterior, likelihood, childCount
 	std::map<int, float> mapIntFloat;
@@ -179,28 +173,59 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
 	}
 
 	//RGB-D SLAM data
-	stat.setRefDepth(msg->refDepth);
-	stat.setRefDepth2D(msg->refDepth2D);
-	stat.setLoopDepth(msg->loopDepth);
-	stat.setLoopDepth2D(msg->loopDepth2D);
-
-	stat.setRefDepthConstant(msg->refDepthConstant);
-	stat.setLoopDepthConstant(msg->loopDepthConstant);
-
-	stat.setRefLocalTransform(transformFromGeometryMsg(msg->refLocalTransform));
-	stat.setLoopLocalTransform(transformFromGeometryMsg(msg->loopLocalTransform));
-
-	stat.setMapCorrection(transformFromGeometryMsg(msg->mapCorrection));
+	stat.setMapCorrection(transformFromGeometryMsg(msg->data.mapCorrection));
 	stat.setLoopClosureTransform(transformFromGeometryMsg(msg->loopClosureTransform));
 	stat.setCurrentPose(transformFromPoseMsg(msg->currentPose));
 
-	std::map<int, Transform> poses;
-	for(unsigned int i=0; i<msg->nodeIds.size() && i<msg->nodePoses.size(); ++i)
+	std::map<int, std::vector<unsigned char> > images;
+	for(unsigned int i=0; i<msg->data.imageIDs.size() && i<msg->data.images.size(); ++i)
 	{
-		poses.insert(std::make_pair(msg->nodeIds[i], transformFromPoseMsg(msg->nodePoses[i])));
+		images.insert(std::make_pair(msg->data.imageIDs[i], msg->data.images[i].bytes));
+	}
+	stat.setImages(images);
+
+	std::map<int, std::vector<unsigned char> > depths;
+	for(unsigned int i=0; i<msg->data.depthIDs.size() && i<msg->data.depths.size(); ++i)
+	{
+		depths.insert(std::make_pair(msg->data.depthIDs[i], msg->data.depths[i].bytes));
+	}
+	stat.setDepths(depths);
+
+	std::map<int, std::vector<unsigned char> > depth2ds;
+	for(unsigned int i=0; i<msg->data.depth2DIDs.size() && i<msg->data.depth2Ds.size(); ++i)
+	{
+		depth2ds.insert(std::make_pair(msg->data.depth2DIDs[i], msg->data.depth2Ds[i].bytes));
+	}
+	stat.setDepth2ds(depth2ds);
+
+	std::map<int, float> depthConstants;
+	for(unsigned int i=0; i<msg->data.depthConstantIDs.size() && i<msg->data.depthConstants.size(); ++i)
+	{
+		depthConstants.insert(std::make_pair(msg->data.depthConstantIDs[i], msg->data.depthConstants[i]));
+	}
+	stat.setDepthConstants(depthConstants);
+
+	std::map<int, Transform> localTransforms;
+	for(unsigned int i=0; i<msg->data.localTransformIDs.size() && i<msg->data.localTransforms.size(); ++i)
+	{
+		localTransforms.insert(std::make_pair(msg->data.localTransformIDs[i], transformFromGeometryMsg(msg->data.localTransforms[i])));
+	}
+	stat.setLocalTransforms(localTransforms);
+
+	std::map<int, Transform> poses;
+	for(unsigned int i=0; i<msg->data.poseIDs.size() && i<msg->data.poses.size(); ++i)
+	{
+		poses.insert(std::make_pair(msg->data.poseIDs[i], transformFromPoseMsg(msg->data.poses[i])));
 	}
 	stat.setPoses(poses);
 
+	std::map<int, int> mapIds;
+	for(unsigned int i=0; i<msg->data.mapIDs.size() && i<msg->data.maps.size(); ++i)
+	{
+		mapIds.insert(std::make_pair(msg->data.mapIDs[i], msg->data.maps[i]));
+	}
+	stat.setMapIds(mapIds);
+
 	this->post(new RtabmapEvent(stat));
 }
 
@@ -226,10 +251,10 @@ void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
 				(int)msg->depths.size(), (int)msg->depthIDs.size());
 	}
 
-	if(msg->depth2DIDs.size() != msg->depths2D.size())
+	if(msg->depth2DIDs.size() != msg->depth2Ds.size())
 	{
 		ROS_WARN("rtabmapviz: receiving map... depths2D and IDs are not the same size (%d vs %d)!",
-				(int)msg->depths2D.size(), (int)msg->depth2DIDs.size());
+				(int)msg->depth2Ds.size(), (int)msg->depth2DIDs.size());
 	}
 
 	if(msg->depthConstantIDs.size() != msg->depthConstants.size())
@@ -248,9 +273,9 @@ void GuiWrapper::mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
 		depths.insert(std::make_pair(msg->depthIDs[i], msg->depths[i].bytes));
 	}
 
-	for(unsigned int i=0; i<msg->depth2DIDs.size() && i < msg->depths2D.size(); ++i)
+	for(unsigned int i=0; i<msg->depth2DIDs.size() && i < msg->depth2Ds.size(); ++i)
 	{
-		depths2d.insert(std::make_pair(msg->depth2DIDs[i], msg->depths2D[i].bytes));
+		depths2d.insert(std::make_pair(msg->depth2DIDs[i], msg->depth2Ds[i].bytes));
 	}
 
 	for(unsigned int i=0; i<msg->depthConstantIDs.size() && i < msg->depthConstants.size(); ++i)

rtabmap/src/MapAssemblerNode.cpp

@@ -64,50 +64,89 @@ public:
 
 	void infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
 	{
-		if(!uContains(rgbClouds_, msg->refId) && msg->refImage.size() && msg->refDepth.size() && msg->refDepthConstant > 0)
-		{
-			rtabmap::Transform localTransform = transformFromGeometryMsg(msg->refLocalTransform);
-
-			if(!localTransform.isNull())
+			for(unsigned int i=0; i<msg->data.localTransformIDs.size() && i<msg->data.localTransforms.size(); ++i)
 			{
-				cv::Mat image = util3d::uncompressImage(msg->refImage);
-				cv::Mat depth = util3d::uncompressImage(msg->refDepth);
-				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, msg->refDepthConstant, cloudDecimation_);
-
-				if(cloudVoxelSize_ > 0)
+				int id = msg->data.localTransformIDs[i];
+				if(!uContains(rgbClouds_, id))
 				{
-					cloud = util3d::voxelize(cloud, cloudVoxelSize_);
+					rtabmap::Transform localTransform = transformFromGeometryMsg(msg->data.localTransforms[i]);
+					if(!localTransform.isNull())
+					{
+						cv::Mat image, depth;
+						float depthConstant = 0.0f;
+
+						for(unsigned int i=0; i<msg->data.imageIDs.size() && i<msg->data.images.size(); ++i)
+						{
+							if(msg->data.imageIDs[i] == id)
+							{
+								image = util3d::uncompressImage(msg->data.images[i].bytes);
+								break;
+							}
+						}
+						for(unsigned int i=0; i<msg->data.depthIDs.size() && i<msg->data.depths.size(); ++i)
+						{
+							if(msg->data.depthIDs[i] == id)
+							{
+								depth = util3d::uncompressImage(msg->data.depths[i].bytes);
+								break;
+							}
+						}
+						for(unsigned int i=0; i<msg->data.depthConstantIDs.size() && i<msg->data.depthConstants.size(); ++i)
+						{
+							if(msg->data.depthConstantIDs[i] == id)
+							{
+								depthConstant = msg->data.depthConstants[i];
+								break;
+							}
+						}
+
+						if(!image.empty() && !depth.empty() && depthConstant > 0.0f)
+						{
+							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, depthConstant, cloudDecimation_);
+
+							if(cloudVoxelSize_ > 0)
+							{
+								cloud = util3d::voxelize(cloud, cloudVoxelSize_);
+							}
+
+							cloud = util3d::transformPointCloud(cloud, localTransform);
+
+							rgbClouds_.insert(std::make_pair(id, cloud));
+						}
+					}
 				}
-
-				cloud = util3d::transformPointCloud(cloud, localTransform);
-
-				rgbClouds_.insert(std::make_pair(msg->refId, cloud));
 			}
-		}
 
-		if(!uContains(scans_, msg->refId) && msg->refDepth2D.size())
+
+		for(unsigned int i=0; i<msg->data.depth2DIDs.size() && i<msg->data.depth2Ds.size(); ++i)
 		{
-			cv::Mat depth2d = util3d::uncompressData(msg->refDepth2D);
-
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::depth2DToPointCloud(depth2d);
-			if(scanVoxelSize_ > 0)
+			if(!uContains(scans_, msg->data.depth2DIDs[i]))
 			{
-				cloud = util3d::voxelize(cloud, scanVoxelSize_);
-			}
+				cv::Mat depth2d = util3d::uncompressData(msg->data.depth2Ds[i].bytes);
+				if(!depth2d.empty())
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::depth2DToPointCloud(depth2d);
+					if(scanVoxelSize_ > 0)
+					{
+						cloud = util3d::voxelize(cloud, scanVoxelSize_);
+					}
 
-			scans_.insert(std::make_pair(msg->refId, cloud));
+					scans_.insert(std::make_pair(msg->data.depth2DIDs[i], cloud));
+				}
+			}
 		}
 
+
 		if(assembledMapClouds_.getNumSubscribers())
 		{
 			// generate the assembled cloud!
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 
-			for(unsigned int i=0; i<msg->nodeIds.size() && i<msg->nodePoses.size(); ++i)
+			for(unsigned int i=0; i<msg->data.poseIDs.size() && i<msg->data.poses.size(); ++i)
 			{
-				Transform pose = transformFromPoseMsg(msg->nodePoses[i]);
+				Transform pose = transformFromPoseMsg(msg->data.poses[i]);
 
-				std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter = rgbClouds_.find(msg->nodeIds[i]);
+				std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter = rgbClouds_.find(msg->data.poseIDs[i]);
 				if(iter != rgbClouds_.end())
 				{
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(iter->second, pose);
@@ -135,11 +174,11 @@ public:
 			// generate the assembled scan!
 			pcl::PointCloud<pcl::PointXYZ>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
 
-			for(unsigned int i=0; i<msg->nodeIds.size() && i<msg->nodePoses.size(); ++i)
+			for(unsigned int i=0; i<msg->data.poseIDs.size() && i<msg->data.poses.size(); ++i)
 			{
-				Transform pose = transformFromPoseMsg(msg->nodePoses[i]);
+				Transform pose = transformFromPoseMsg(msg->data.poses[i]);
 
-				std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = scans_.find(msg->nodeIds[i]);
+				std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = scans_.find(msg->data.poseIDs[i]);
 				if(iter != scans_.end())
 				{
 					pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = util3d::transformPointCloud(iter->second, pose);
@@ -190,10 +229,10 @@ public:
 					(int)msg->depthConstants.size(), (int)msg->depthConstantIDs.size());
 		}
 
-		if(msg->depth2DIDs.size() != msg->depths2D.size())
+		if(msg->depth2DIDs.size() != msg->depth2Ds.size())
 		{
 			ROS_WARN("rtabmapviz: receiving map... depths2D and depth2DIDs are not the same size (%d vs %d)!",
-					(int)msg->depths2D.size(), (int)msg->depth2DIDs.size());
+					(int)msg->depth2Ds.size(), (int)msg->depth2DIDs.size());
 		}
 
 		// fill maps
@@ -230,11 +269,11 @@ public:
 			}
 		}
 
-		for(unsigned int i=0; i<msg->depth2DIDs.size() && i < msg->depths2D.size(); ++i)
+		for(unsigned int i=0; i<msg->depth2DIDs.size() && i < msg->depth2Ds.size(); ++i)
 		{
 			if(!uContains(scans_, msg->depth2DIDs[i]))
 			{
-				cv::Mat depth2d = util3d::uncompressData(msg->depths2D[i].bytes);
+				cv::Mat depth2d = util3d::uncompressData(msg->depth2Ds[i].bytes);
 				scans_.insert(std::make_pair(msg->depth2DIDs[i], util3d::depth2DToPointCloud(depth2d)));
 			}
 		}