ros: updated for RTAB-Map 0.7 (depthConstant->fx,fy,cx,cy) fixed issue 12

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

src/CoreWrapper.cpp

@@ -371,14 +371,20 @@ void CoreWrapper::depthCallback(
 		}
 		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
 
-		float depthConstant = 1.0f/cameraInfoMsg->K[4];
+		float depthFx = cameraInfoMsg->K[0];
+		float depthFy = cameraInfoMsg->K[4];
+		float depthCx = cameraInfoMsg->K[2];
+		float depthCy = cameraInfoMsg->K[5];
 
 		process(ptrImage->header.seq,
 				ptrImage->image,
 				odom,
 				odomMsg->header.frame_id,
 				ptrDepth->image,
-				depthConstant,
+				depthFx,
+				depthFy,
+				depthCx,
+				depthCy,
 				localTransform,
 				cv::Mat());
 	}
@@ -448,6 +454,9 @@ void CoreWrapper::scanCallback(
 				odomMsg->header.frame_id,
 				cv::Mat(),
 				0.0f,
+				0.0f,
+				0.0f,
+				0.0f,
 				Transform(),
 				scan);
 	}
@@ -519,14 +528,20 @@ void CoreWrapper::depthScanCallback(
 		}
 		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
 
-		float depthConstant = 1.0f/cameraInfoMsg->K[4];
+		float depthFx = cameraInfoMsg->K[0];
+		float depthFy = cameraInfoMsg->K[4];
+		float depthCx = cameraInfoMsg->K[2];
+		float depthCy = cameraInfoMsg->K[5];
 
 		process(ptrImage->header.seq,
 				ptrImage->image,
 				odom,
 				odomMsg->header.frame_id,
 				ptrDepth->image,
-				depthConstant,
+				depthFx,
+				depthFy,
+				depthCx,
+				depthCy,
 				localTransform,
 				scan);
 	}
@@ -538,7 +553,10 @@ void CoreWrapper::process(
 		const Transform & odom,
 		const std::string & odomFrameId,
 		const cv::Mat & depth,
-		float depthConstant,
+		float depthFx,
+		float depthFy,
+		float depthCx,
+		float depthCy,
 		const Transform & localTransform,
 		const cv::Mat & scan)
 {
@@ -574,7 +592,10 @@ void CoreWrapper::process(
 		Image data(image,
 				depth16,
 				scan,
-				depthConstant,
+				depthFx,
+				depthFy,
+				depthCx,
+				depthCy,
 				odom,
 				localTransform,
 				id);
@@ -723,7 +744,10 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap:
 	std::map<int, std::vector<unsigned char> > images;
 	std::map<int, std::vector<unsigned char> > depths;
 	std::map<int, std::vector<unsigned char> > depths2d;
-	std::map<int, float> depthConstants;
+	std::map<int, float> depthFxs;
+	std::map<int, float> depthFys;
+	std::map<int, float> depthCxs;
+	std::map<int, float> depthCys;
 	std::map<int, Transform> localTransforms;
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> constraints;
@@ -742,7 +766,10 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap:
 				images,
 				depths,
 				depths2d,
-				depthConstants,
+				depthFxs,
+				depthFys,
+				depthCxs,
+				depthCys,
 				localTransforms,
 				poses,
 				constraints,
@@ -783,13 +810,41 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap:
 		++i;
 	}
 
-	rep.data.depthConstantIDs.resize(depthConstants.size());
-	rep.data.depthConstants.resize(depthConstants.size());
+	// fx,fy,cx,cy parameters
+	rep.data.depthFxIDs.resize(depthFxs.size());
+	rep.data.depthFxs.resize(depthFxs.size());
 	i=0;
-	for(std::map<int, float>::iterator iter = depthConstants.begin(); iter!=depthConstants.end(); ++iter)
+	for(std::map<int, float>::iterator iter = depthFxs.begin(); iter!=depthFxs.end(); ++iter)
 	{
-		rep.data.depthConstantIDs[i] = iter->first;
-		rep.data.depthConstants[i] = iter->second;
+		rep.data.depthFxIDs[i] = iter->first;
+		rep.data.depthFxs[i] = iter->second;
+		++i;
+	}
+	rep.data.depthFyIDs.resize(depthFys.size());
+	rep.data.depthFys.resize(depthFys.size());
+	i=0;
+	for(std::map<int, float>::iterator iter = depthFys.begin(); iter!=depthFys.end(); ++iter)
+	{
+		rep.data.depthFyIDs[i] = iter->first;
+		rep.data.depthFys[i] = iter->second;
+		++i;
+	}
+	rep.data.depthCxIDs.resize(depthCxs.size());
+	rep.data.depthCxs.resize(depthCxs.size());
+	i=0;
+	for(std::map<int, float>::iterator iter = depthCxs.begin(); iter!=depthCxs.end(); ++iter)
+	{
+		rep.data.depthCxIDs[i] = iter->first;
+		rep.data.depthCxs[i] = iter->second;
+		++i;
+	}
+	rep.data.depthCyIDs.resize(depthCys.size());
+	rep.data.depthCys.resize(depthCys.size());
+	i=0;
+	for(std::map<int, float>::iterator iter = depthCys.begin(); iter!=depthCys.end(); ++iter)
+	{
+		rep.data.depthCyIDs[i] = iter->first;
+		rep.data.depthCys[i] = iter->second;
 		++i;
 	}
 
@@ -991,11 +1046,28 @@ void CoreWrapper::publishStats(const Statistics & stats)
 			transformToGeometryMsg(stats.getLocalTransforms().at(stats.refImageId()), msg->localTransforms[0]);
 		}
 
-		if(uContains(stats.getDepthConstants(), stats.refImageId()))
+		// fx,fy,cx,cy parameters
+		if(uContains(stats.getDepthFxs(), stats.refImageId()))
 		{
-			msg->depthConstantIDs.push_back(stats.refImageId());
-			msg->depthConstants.push_back(stats.getDepthConstants().at(stats.refImageId()));
+			msg->depthFxIDs.push_back(stats.refImageId());
+			msg->depthFxs.push_back(stats.getDepthFxs().at(stats.refImageId()));
 		}
+		if(uContains(stats.getDepthFys(), stats.refImageId()))
+		{
+			msg->depthFyIDs.push_back(stats.refImageId());
+			msg->depthFys.push_back(stats.getDepthFys().at(stats.refImageId()));
+		}
+		if(uContains(stats.getDepthCxs(), stats.refImageId()))
+		{
+			msg->depthCxIDs.push_back(stats.refImageId());
+			msg->depthCxs.push_back(stats.getDepthCxs().at(stats.refImageId()));
+		}
+		if(uContains(stats.getDepthCys(), stats.refImageId()))
+		{
+			msg->depthCyIDs.push_back(stats.refImageId());
+			msg->depthCys.push_back(stats.getDepthCys().at(stats.refImageId()));
+		}
+
 
 		mapData_.publish(msg);
 	}

src/CoreWrapper.h

@@ -67,7 +67,10 @@ private:
 			const rtabmap::Transform & odom = rtabmap::Transform(),
 			const std::string & odomFrameId = "",
 			const cv::Mat & depth = cv::Mat(),
-			float depthConstant = 0.0f,
+			float depthFx = 0.0f,
+			float depthFy = 0.0f,
+			float depthCx = 0.0f,
+			float depthCy = 0.0f,
 			const rtabmap::Transform & localTransform = rtabmap::Transform(),
 			const cv::Mat & scan = cv::Mat());
 

src/DataRecorderNode.cpp

@@ -150,6 +150,9 @@ private:
 			cv::Mat(),
 			cv::Mat(),
 			0.0f,
+			0.0f,
+			0.0f,
+			0.0f,
 			Transform(),
 			Transform());
 		recorder_.addData(image);
@@ -177,7 +180,10 @@ private:
 		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
 		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
 
-		float depthConstant = 1.0f/cameraInfoMsg->K[4];
+		float depthFx = cameraInfoMsg->K[0];
+		float depthFy = cameraInfoMsg->K[4];
+		float depthCx = cameraInfoMsg->K[2];
+		float depthCy = cameraInfoMsg->K[5];
 
 		cv::Mat depth16;
 		if(ptrDepth->image.type() != CV_16UC1)
@@ -209,7 +215,10 @@ private:
 			ptrImage->image.clone(),
 			depth16,
 			cv::Mat(),
-			depthConstant,
+			depthFx,
+			depthFy,
+			depthCx,
+			depthCy,
 			Transform(),
 			localTransform);
 		recorder_.addData(image);
@@ -240,7 +249,10 @@ private:
 		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
 		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
 
-		float depthConstant = 1.0f/cameraInfoMsg->K[4];
+		float depthFx = cameraInfoMsg->K[0];
+		float depthFy = cameraInfoMsg->K[4];
+		float depthCx = cameraInfoMsg->K[2];
+		float depthCy = cameraInfoMsg->K[5];
 
 		cv::Mat depth16;
 		if(ptrDepth->image.type() != CV_16UC1)
@@ -272,7 +284,10 @@ private:
 			ptrImage->image.clone(),
 			depth16,
 			cv::Mat(),
-			depthConstant,
+			depthFx,
+			depthFy,
+			depthCx,
+			depthCy,
 			odom,
 			localTransform);
 		recorder_.addData(image);
@@ -312,6 +327,9 @@ private:
 			cv::Mat(),
 			scan,
 			0.0f,
+			0.0f,
+			0.0f,
+			0.0f,
 			odom,
 			Transform());
 		recorder_.addData(image);
@@ -353,7 +371,10 @@ private:
 		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
 		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
 
-		float depthConstant = 1.0f/cameraInfoMsg->K[4];
+		float depthFx = cameraInfoMsg->K[0];
+		float depthFy = cameraInfoMsg->K[4];
+		float depthCx = cameraInfoMsg->K[2];
+		float depthCy = cameraInfoMsg->K[5];
 
 		cv::Mat depth16;
 		if(ptrDepth->image.type() != CV_16UC1)
@@ -385,7 +406,10 @@ private:
 			ptrImage->image.clone(),
 			depth16,
 			scan,
-			depthConstant,
+			depthFx,
+			depthFy,
+			depthCx,
+			depthCy,
 			odom,
 			localTransform);
 		recorder_.addData(image);

src/GuiWrapper.cpp

@@ -203,12 +203,33 @@ void GuiWrapper::infoMapCallback(
 	}
 	stat.setDepth2ds(depth2ds);
 
-	std::map<int, float> depthConstants;
-	for(unsigned int i=0; i<mapMsg->depthConstantIDs.size() && i<mapMsg->depthConstants.size(); ++i)
+	std::map<int, float> depthFxs;
+	for(unsigned int i=0; i<mapMsg->depthFxIDs.size() && i<mapMsg->depthFxs.size(); ++i)
 	{
-		depthConstants.insert(std::make_pair(mapMsg->depthConstantIDs[i], mapMsg->depthConstants[i]));
+		depthFxs.insert(std::make_pair(mapMsg->depthFxIDs[i], mapMsg->depthFxs[i]));
 	}
-	stat.setDepthConstants(depthConstants);
+	stat.setDepthFxs(depthFxs);
+
+	std::map<int, float> depthFys;
+	for(unsigned int i=0; i<mapMsg->depthFyIDs.size() && i<mapMsg->depthFys.size(); ++i)
+	{
+		depthFys.insert(std::make_pair(mapMsg->depthFyIDs[i], mapMsg->depthFys[i]));
+	}
+	stat.setDepthFys(depthFys);
+
+	std::map<int, float> depthCxs;
+	for(unsigned int i=0; i<mapMsg->depthCxIDs.size() && i<mapMsg->depthCxs.size(); ++i)
+	{
+		depthCxs.insert(std::make_pair(mapMsg->depthCxIDs[i], mapMsg->depthCxs[i]));
+	}
+	stat.setDepthCxs(depthCxs);
+
+	std::map<int, float> depthCys;
+	for(unsigned int i=0; i<mapMsg->depthCyIDs.size() && i<mapMsg->depthCys.size(); ++i)
+	{
+		depthCys.insert(std::make_pair(mapMsg->depthCyIDs[i], mapMsg->depthCys[i]));
+	}
+	stat.setDepthCys(depthCys);
 
 	std::map<int, Transform> localTransforms;
 	for(unsigned int i=0; i<mapMsg->localTransformIDs.size() && i<mapMsg->localTransforms.size(); ++i)
@@ -247,7 +268,10 @@ void GuiWrapper::processRequestedMap(const rtabmap::MapData & map)
 	std::map<int, std::vector<unsigned char> > images;
 	std::map<int, std::vector<unsigned char> > depths;
 	std::map<int, std::vector<unsigned char> > depths2d;
-	std::map<int, float> depthConstants;
+	std::map<int, float> depthFxs;
+	std::map<int, float> depthFys;
+	std::map<int, float> depthCxs;
+	std::map<int, float> depthCys;
 	std::map<int, Transform> localTransforms;
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> constraints;
@@ -270,10 +294,25 @@ void GuiWrapper::processRequestedMap(const rtabmap::MapData & map)
 				(int)map.depth2Ds.size(), (int)map.depth2DIDs.size());
 	}
 
-	if(map.depthConstantIDs.size() != map.depthConstants.size())
+	if(map.depthFxIDs.size() != map.depthFxs.size())
 	{
-		ROS_WARN("rtabmapviz: receiving map... depthConstants and IDs are not the same size (%d vs %d)!",
-				(int)map.depthConstants.size(), (int)map.depthConstantIDs.size());
+		ROS_WARN("rtabmapviz: receiving map... depthFxs and IDs are not the same size (%d vs %d)!",
+				(int)map.depthFxs.size(), (int)map.depthFxIDs.size());
+	}
+	if(map.depthFyIDs.size() != map.depthFys.size())
+	{
+		ROS_WARN("rtabmapviz: receiving map... depthFys and IDs are not the same size (%d vs %d)!",
+				(int)map.depthFys.size(), (int)map.depthFyIDs.size());
+	}
+	if(map.depthCxIDs.size() != map.depthCxs.size())
+	{
+		ROS_WARN("rtabmapviz: receiving map... depthCxs and IDs are not the same size (%d vs %d)!",
+				(int)map.depthCxs.size(), (int)map.depthCxIDs.size());
+	}
+	if(map.depthCyIDs.size() != map.depthCys.size())
+	{
+		ROS_WARN("rtabmapviz: receiving map... depthCys and IDs are not the same size (%d vs %d)!",
+				(int)map.depthCys.size(), (int)map.depthCyIDs.size());
 	}
 
 	if(map.poseIDs.size() != map.poses.size())
@@ -305,10 +344,23 @@ void GuiWrapper::processRequestedMap(const rtabmap::MapData & map)
 		depths2d.insert(std::make_pair(map.depth2DIDs[i], map.depth2Ds[i].bytes));
 	}
 
-	for(unsigned int i=0; i<map.depthConstantIDs.size() && i < map.depthConstants.size(); ++i)
+	for(unsigned int i=0; i<map.depthFxIDs.size() && i < map.depthFxs.size(); ++i)
 	{
-		depthConstants.insert(std::make_pair(map.depthConstantIDs[i], map.depthConstants[i]));
+		depthFxs.insert(std::make_pair(map.depthFxIDs[i], map.depthFxs[i]));
 	}
+	for(unsigned int i=0; i<map.depthFyIDs.size() && i < map.depthFys.size(); ++i)
+	{
+		depthFys.insert(std::make_pair(map.depthFyIDs[i], map.depthFys[i]));
+	}
+	for(unsigned int i=0; i<map.depthCxIDs.size() && i < map.depthCxs.size(); ++i)
+	{
+		depthCxs.insert(std::make_pair(map.depthCxIDs[i], map.depthCxs[i]));
+	}
+	for(unsigned int i=0; i<map.depthCyIDs.size() && i < map.depthCys.size(); ++i)
+	{
+		depthCys.insert(std::make_pair(map.depthCyIDs[i], map.depthCys[i]));
+	}
+
 
 	for(unsigned int i=0; i<map.localTransformIDs.size() && i < map.localTransforms.size(); ++i)
 	{
@@ -331,7 +383,10 @@ void GuiWrapper::processRequestedMap(const rtabmap::MapData & map)
 	this->post(new RtabmapEvent3DMap(images,
 			depths,
 			depths2d,
-			depthConstants,
+			depthFxs,
+			depthFys,
+			depthCxs,
+			depthCys,
 			localTransforms,
 			poses,
 			constraints));
@@ -467,6 +522,9 @@ void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
 			cv::Mat(),
 			cv::Mat(),
 			0.0f,
+			0.0f,
+			0.0f,
+			0.0f,
 			odom,
 			Transform());
 	this->post(new OdometryEvent(image));
@@ -497,12 +555,18 @@ void GuiWrapper::depthCallback(
 	cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
 	cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
 
-	float depthConstant = 1.0f/cameraInfoMsg->K[4];
+	float depthFx = cameraInfoMsg->K[0];
+	float depthFy = cameraInfoMsg->K[4];
+	float depthCx = cameraInfoMsg->K[2];
+	float depthCy = cameraInfoMsg->K[5];
 
 	rtabmap::Image image(
 			ptrImage->image.clone(),
 			ptrDepth->image.clone(),
-			depthConstant,
+			depthFx,
+			depthFy,
+			depthCx,
+			depthCy,
 			odom,
 			localTransform);
 	this->post(new OdometryEvent(image));
@@ -542,6 +606,9 @@ void GuiWrapper::scanCallback(
 			cv::Mat(),
 			scan,
 			0.0f,
+			0.0f,
+			0.0f,
+			0.0f,
 			odom,
 			Transform());
 	this->post(new OdometryEvent(image));
@@ -582,13 +649,19 @@ void GuiWrapper::depthScanCallback(
 	cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
 	cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
 
-	float depthConstant = 1.0f/cameraInfoMsg->K[4];
+	float depthFx = cameraInfoMsg->K[0];
+	float depthFy = cameraInfoMsg->K[4];
+	float depthCx = cameraInfoMsg->K[2];
+	float depthCy = cameraInfoMsg->K[5];
 
 	rtabmap::Image image(
 			ptrImage->image.clone(),
 			ptrDepth->image.clone(),
 			scan,
-			depthConstant,
+			depthFx,
+			depthFy,
+			depthCx,
+			depthCy,
 			odom,
 			localTransform);
 	this->post(new OdometryEvent(image));

src/MapAssemblerNode.cpp

@@ -74,7 +74,10 @@ public:
 				if(!localTransform.isNull())
 				{
 					cv::Mat image, depth;
-					float depthConstant = 0.0f;
+					float depthFx = 0.0f;
+					float depthFy = 0.0f;
+					float depthCx = 0.0f;
+					float depthCy = 0.0f;
 
 					for(unsigned int i=0; i<msg->imageIDs.size() && i<msg->images.size(); ++i)
 					{
@@ -92,18 +95,42 @@ public:
 							break;
 						}
 					}
-					for(unsigned int i=0; i<msg->depthConstantIDs.size() && i<msg->depthConstants.size(); ++i)
+					for(unsigned int i=0; i<msg->depthFxIDs.size() && i<msg->depthFxs.size(); ++i)
 					{
-						if(msg->depthConstantIDs[i] == id)
+						if(msg->depthFxIDs[i] == id)
 						{
-							depthConstant = msg->depthConstants[i];
+							depthFx = msg->depthFxs[i];
+							break;
+						}
+					}
+					for(unsigned int i=0; i<msg->depthFyIDs.size() && i<msg->depthFys.size(); ++i)
+					{
+						if(msg->depthFyIDs[i] == id)
+						{
+							depthFy = msg->depthFys[i];
+							break;
+						}
+					}
+					for(unsigned int i=0; i<msg->depthCxIDs.size() && i<msg->depthCxs.size(); ++i)
+					{
+						if(msg->depthCxIDs[i] == id)
+						{
+							depthCx = msg->depthCxs[i];
+							break;
+						}
+					}
+					for(unsigned int i=0; i<msg->depthCyIDs.size() && i<msg->depthCys.size(); ++i)
+					{
+						if(msg->depthCyIDs[i] == id)
+						{
+							depthCy = msg->depthCys[i];
 							break;
 						}
 					}
 
-					if(!image.empty() && !depth.empty() && depthConstant > 0.0f)
+					if(!image.empty() && !depth.empty() && depthFx > 0.0f && depthFy > 0.0f && depthCx >= 0.0f && depthCy >= 0.0f)
 					{
-						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, depthConstant, cloudDecimation_);
+						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, depthCx, depthCy, depthFx, depthFy, cloudDecimation_);
 
 						if(cloudMaxDepth_ > 0)
 						{

src/VisualOdometryNode.cpp

@@ -188,13 +188,19 @@ public:
 
 			if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
 			{
-				float depthConstant = 1.0f/cameraInfo->K[4];
+				float depthFx = cameraInfo->K[0];
+				float depthFy = cameraInfo->K[4];
+				float depthCx = cameraInfo->K[2];
+				float depthCy = cameraInfo->K[5];
 				cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, "mono8");
 				cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);
 
 				rtabmap::Image data(ptrImage->image,
 						ptrDepth->image.type() == CV_32FC1?util3d::cvtDepthFromFloat(ptrDepth->image):ptrDepth->image,
-						depthConstant,
+						depthFx,
+						depthFy,
+						depthCx,
+						depthCy,
 						rtabmap::Transform(),
 						rtabmap::transformFromTF(localTransform));
 				rtabmap::Transform pose = odometry_->process(data);

src/rviz/MapCloudDisplay.cpp

@@ -219,7 +219,10 @@ void MapCloudDisplay::processMapData(const rtabmap::MapData& map)
 			if(!localTransform.isNull())
 			{
 				cv::Mat image, depth;
-				float depthConstant = 0.0f;
+				float depthFx = 0.0f;
+				float depthFy = 0.0f;
+				float depthCx = 0.0f;
+				float depthCy = 0.0f;
 				rtabmap::Transform pose;
 
 				for(unsigned int i=0; i<map.imageIDs.size() && i<map.images.size(); ++i)
@@ -238,11 +241,35 @@ void MapCloudDisplay::processMapData(const rtabmap::MapData& map)
 						break;
 					}
 				}
-				for(unsigned int i=0; i<map.depthConstantIDs.size() && i<map.depthConstants.size(); ++i)
+				for(unsigned int i=0; i<map.depthFxIDs.size() && i<map.depthFxs.size(); ++i)
 				{
-					if(map.depthConstantIDs[i] == id)
+					if(map.depthFxIDs[i] == id)
 					{
-						depthConstant = map.depthConstants[i];
+						depthFx = map.depthFxs[i];
+						break;
+					}
+				}
+				for(unsigned int i=0; i<map.depthFyIDs.size() && i<map.depthFys.size(); ++i)
+				{
+					if(map.depthFyIDs[i] == id)
+					{
+						depthFy = map.depthFys[i];
+						break;
+					}
+				}
+				for(unsigned int i=0; i<map.depthCxIDs.size() && i<map.depthCxs.size(); ++i)
+				{
+					if(map.depthCxIDs[i] == id)
+					{
+						depthCx = map.depthCxs[i];
+						break;
+					}
+				}
+				for(unsigned int i=0; i<map.depthCyIDs.size() && i<map.depthCys.size(); ++i)
+				{
+					if(map.depthCyIDs[i] == id)
+					{
+						depthCy = map.depthCys[i];
 						break;
 					}
 				}
@@ -255,9 +282,9 @@ void MapCloudDisplay::processMapData(const rtabmap::MapData& map)
 					}
 				}
 
-				if(!image.empty() && !depth.empty() && depthConstant > 0.0f && !pose.isNull())
+				if(!image.empty() && !depth.empty() && depthFx > 0.0f && depthFy > 0.0f && depthCx >= 0.0f && depthCy >= 0.0f && !pose.isNull())
 				{
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, depthConstant, cloud_decimation_->getInt());
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, depthCx, depthCy, depthFx, depthFy, cloud_decimation_->getInt());
 
 					if(cloud_max_depth_->getFloat() > 0.0f)
 					{