Fixed cloudFromDepth() method when depth image is not the same size as the calibration file. That fixes projection map created from Tango databases (where depth size != rgb size)

corelib/include/rtabmap/core/util3d.h

@@ -74,16 +74,23 @@ pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
 		bool smoothing,
 		float maxZError = 0.02f);
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
+RTABMAP_DEPRECATED (pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
 		const cv::Mat & imageDepth,
 		float cx, float cy,
 		float fx, float fy,
 		int decimation = 1,
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
+		std::vector<int> * validIndices = 0), "Use cloudFromDepth with CameraModel interface.");
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
+		const cv::Mat & imageDepth,
+		const CameraModel & model,
+		int decimation = 1,
+		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0);
 
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
+RTABMAP_DEPRECATED (pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
 		const cv::Mat & imageRgb,
 		const cv::Mat & imageDepth,
 		float cx, float cy,
@@ -91,6 +98,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
 		int decimation = 1,
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
+		std::vector<int> * validIndices = 0), "Use cloudFromDepthRGB with CameraModel interface.");
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
+		const cv::Mat & imageRgb,
+		const cv::Mat & imageDepth,
+		const CameraModel & model,
+		int decimation = 1,
+		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDisparity(

corelib/include/rtabmap/core/util3d_registration.h

@@ -92,18 +92,6 @@ Transform RTABMAP_EXP icpPointToPlane(
 		float epsilon = 0.0f,
 		bool icp2D = false);
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
-		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		int decimation,
-		double maxDepth,
-		float voxel,
-		int samples,
-		const Transform & transform = Transform::getIdentity());
-
 } // namespace util3d
 } // namespace rtabmap
 

corelib/src/util3d.cpp

@@ -248,9 +248,42 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		float minDepth,
 		std::vector<int> * validIndices)
 {
+	CameraModel model(fx, fy, cx, cy);
+	return cloudFromDepth(imageDepth, model, decimation, maxDepth, minDepth, validIndices);
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
+		const cv::Mat & imageDepth,
+		const CameraModel & model,
+		int decimation,
+		float maxDepth,
+		float minDepth,
+		std::vector<int> * validIndices)
+{
+	float rgbToDepthFactorX = 1.0f;
+	float rgbToDepthFactorY = 1.0f;
+
+	UASSERT(model.isValidForProjection());
 	UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
-	UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("rows=%d decimation=%d", imageDepth.rows, decimation).c_str());
+
-	UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("cols=%d decimation=%d", imageDepth.cols, decimation).c_str());
+	int imageRows = imageDepth.rows;
+	int imageCols = imageDepth.cols;
+
+	if(model.imageHeight()>0 && model.imageWidth()>0)
+	{
+		UASSERT(model.imageHeight() % imageDepth.rows == 0 && model.imageWidth() % imageDepth.cols == 0);
+		UASSERT_MSG(model.imageHeight() % decimation == 0, uFormat("model.imageHeight()=%d decimation=%d", model.imageHeight(), decimation).c_str());
+		UASSERT_MSG(model.imageWidth() % decimation == 0, uFormat("model.imageWidth()=%d decimation=%d", model.imageWidth(), decimation).c_str());
+		rgbToDepthFactorX = 1.0f/float((model.imageWidth() / imageDepth.cols));
+		rgbToDepthFactorY = 1.0f/float((model.imageHeight() / imageDepth.rows));
+		imageRows = model.imageHeight();
+		imageCols = model.imageWidth();
+	}
+	else
+	{
+		UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("rows=%d decimation=%d", imageDepth.rows, decimation).c_str());
+		UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("cols=%d decimation=%d", imageDepth.cols, decimation).c_str());
+	}
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 	if(decimation < 1)
@@ -259,8 +292,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 	}
 
 	//cloud.header = cameraInfo.header;
-	cloud->height = imageDepth.rows/decimation;
+	cloud->height = imageRows/decimation;
-	cloud->width  = imageDepth.cols/decimation;
+	cloud->width  = imageCols/decimation;
 	cloud->is_dense = false;
 	cloud->resize(cloud->height * cloud->width);
 	if(validIndices)
@@ -268,14 +301,27 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		validIndices->resize(cloud->size());
 	}
 
+	float depthFx = model.fx() * rgbToDepthFactorX;
+	float depthFy = model.fy() * rgbToDepthFactorY;
+	float depthCx = model.cx() * rgbToDepthFactorX;
+	float depthCy = model.cy() * rgbToDepthFactorY;
+
+	UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
+			imageCols, imageRows,
+			imageDepth.cols, imageDepth.rows,
+			model.fx(), model.fy(), model.cx(), model.cy(),
+			rgbToDepthFactorX,
+			rgbToDepthFactorY,
+			decimation);
+
 	int oi = 0;
-	for(int h = 0; h < imageDepth.rows; h+=decimation)
+	for(int h = 0; h < imageRows && h/decimation < (int)cloud->height; h+=decimation)
 	{
-		for(int w = 0; w < imageDepth.cols; w+=decimation)
+		for(int w = 0; w < imageCols && w/decimation < (int)cloud->width; w+=decimation)
 		{
 			pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
 
-			pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
+			pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w*rgbToDepthFactorX, h*rgbToDepthFactorY, depthCx, depthCy, depthFx, depthFy, false);
 			if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
 			{
 				pt.x = ptXYZ.x;
@@ -310,8 +356,23 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 		float maxDepth,
 		float minDepth,
 		std::vector<int> * validIndices)
+{
+	CameraModel model(fx, fy, cx, cy);
+	return cloudFromDepthRGB(imageRgb, imageDepth, model, decimation, maxDepth, minDepth, validIndices);
+}
+
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
+		const cv::Mat & imageRgb,
+		const cv::Mat & imageDepth,
+		const CameraModel & model,
+		int decimation,
+		float maxDepth,
+		float minDepth,
+		std::vector<int> * validIndices)
 {
 	UDEBUG("");
+	UASSERT(model.isValidForProjection());
+	UASSERT((model.imageHeight() == 0 && model.imageWidth() == 0) || (model.imageHeight() == imageRgb.rows && model.imageWidth() == imageRgb.cols));
 	UASSERT(imageRgb.rows % imageDepth.rows == 0 && imageRgb.cols % imageDepth.cols == 0);
 	UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
 	UASSERT_MSG(imageRgb.rows % decimation == 0, uFormat("imageDepth.rows=%d decimation=%d", imageRgb.rows, decimation).c_str());
@@ -349,15 +410,15 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 
 	float rgbToDepthFactorX = 1.0f/float((imageRgb.cols / imageDepth.cols));
 	float rgbToDepthFactorY = 1.0f/float((imageRgb.rows / imageDepth.rows));
-	float depthFx = fx * rgbToDepthFactorX;
+	float depthFx = model.fx() * rgbToDepthFactorX;
-	float depthFy = fy * rgbToDepthFactorY;
+	float depthFy = model.fy() * rgbToDepthFactorY;
-	float depthCx = cx * rgbToDepthFactorX;
+	float depthCx = model.cx() * rgbToDepthFactorX;
-	float depthCy = cy * rgbToDepthFactorY;
+	float depthCy = model.cy() * rgbToDepthFactorY;
 
 	UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
 			imageRgb.cols, imageRgb.rows,
 			imageDepth.cols, imageDepth.rows,
-			fx, fy, cx, cy,
+			model.fx(), model.fy(), model.cx(), model.cy(),
 			rgbToDepthFactorX,
 			rgbToDepthFactorY,
 			decimation);
@@ -653,10 +714,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 			{
 				pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = util3d::cloudFromDepth(
 						cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
-						sensorData.cameraModels()[i].cx(),
+						sensorData.cameraModels()[i],
-						sensorData.cameraModels()[i].cy(),
-						sensorData.cameraModels()[i].fx(),
-						sensorData.cameraModels()[i].fy(),
 						decimation,
 						maxDepth,
 						minDepth,
@@ -767,10 +825,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
 						cv::Mat(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows)),
 						cv::Mat(sensorData.depthRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthRaw().rows)),
-						sensorData.cameraModels()[i].cx(),
+						sensorData.cameraModels()[i],
-						sensorData.cameraModels()[i].cy(),
-						sensorData.cameraModels()[i].fx(),
-						sensorData.cameraModels()[i].fy(),
 						decimation,
 						maxDepth,
 						minDepth,

corelib/src/util3d_registration.cpp

@@ -380,60 +380,6 @@ Transform icpPointToPlane(
 	return Transform::fromEigen4f(icp.getFinalTransformation());
 }
 
-// If "voxel" > 0, "samples" is ignored
-pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
-		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		int decimation,
-		double maxDepth,
-		float voxel,
-		int samples,
-		const Transform & transform)
-{
-	UASSERT(!depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1));
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-	cloud = cloudFromDepth(
-			depth,
-			cx,
-			cy,
-			fx,
-			fy,
-			decimation);
-
-	if(cloud->size())
-	{
-		if(maxDepth>0.0)
-		{
-			cloud = passThrough(cloud, "z", 0, maxDepth);
-		}
-
-		if(cloud->size())
-		{
-			if(voxel>0)
-			{
-				cloud = voxelize(cloud, voxel);
-			}
-			else if(samples>0 && (int)cloud->size() > samples)
-			{
-				cloud = randomSampling(cloud, samples);
-			}
-
-			if(cloud->size())
-			{
-				if(!transform.isNull() && !transform.isIdentity())
-				{
-					cloud = transformPointCloud(cloud, transform);
-				}
-			}
-		}
-	}
-
-	return cloud;
-}
-
 }
 
 }

guilib/include/rtabmap/gui/MainWindow.h

@@ -204,6 +204,7 @@ private slots:
 	void dataRecorder();
 	void dataRecorderDestroyed();
 	void updateNodeVisibility(int, bool);
+	void updateGraphView();
 
 signals:
 	void statsReceived(const rtabmap::Statistics &);
@@ -233,7 +234,12 @@ private:
 			const std::map<int, std::string> & labels,
 			const std::map<int, Transform> & groundTruths,
 			bool verboseProgress = false);
-	void createAndAddCloudToMap(int nodeId,	const Transform & pose, int mapId);
+	std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createAndAddCloudToMap(int nodeId,	const Transform & pose, int mapId);
+	void createAndAddProjectionMap(
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+			const pcl::IndicesPtr & indices,
+			int nodeId,
+			const Transform & pose);
 	void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId);
 	void createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId);
 	Transform alignPosesToGroundTruth(std::map<int, Transform> & poses, const std::map<int, Transform> & groundTruth);

guilib/src/DatabaseViewer.cpp

@@ -2328,7 +2328,10 @@ void DatabaseViewer::update(int value,
 								{
 									depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
 								}
-								cloud = util3d::cloudFromDepthRGB(data.imageRaw(), depth, data.cameraModels()[0].cx(), data.cameraModels()[0].cy(), data.cameraModels()[0].fx(), data.cameraModels()[0].fy());
+								cloud = util3d::cloudFromDepthRGB(
+										data.imageRaw(),
+										depth,
+										data.cameraModels()[0]);
 							}
 							else
 							{
@@ -2388,15 +2391,6 @@ void DatabaseViewer::update(int value,
 				}
 			}
 
-			if(!imgDepth.isNull())
-			{
-				view->setImageDepth(imgDepth);
-				rect = imgDepth.rect();
-			}
-			else
-			{
-				ULOGGER_DEBUG("Image depth is empty");
-			}
 			if(!img.isNull())
 			{
 				view->setImage(img);
@@ -2407,6 +2401,19 @@ void DatabaseViewer::update(int value,
 				ULOGGER_DEBUG("Image is empty");
 			}
 
+			if(!imgDepth.isNull())
+			{
+				view->setImageDepth(imgDepth);
+				if(!img.isNull())
+				{
+					rect = imgDepth.rect();
+				}
+			}
+			else
+			{
+				ULOGGER_DEBUG("Image depth is empty");
+			}
+
 			// loops
 			std::map<int, rtabmap::Link> links;
 			dbDriver_->loadLinks(id, links);

guilib/src/MainWindow.cpp

@@ -453,6 +453,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 		connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(configGUIModified()));
 		connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
 	}
+	connect(_ui->dockWidget_graphViewer->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
 	// catch resize events
 	_ui->dockWidget_posterior->installEventFilter(this);
 	_ui->dockWidget_likelihood->installEventFilter(this);
@@ -1912,9 +1913,16 @@ void MainWindow::updateMapCloud(
 			std::string cloudName = uFormat("cloud%d", iter->first);
 
 			// 3d point cloud
-			if((_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)) ||
+			bool update3dCloud = _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0);
-				(_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible() && _preferencesDialog->isGridMapFrom3DCloud()))
+			bool updateProjMap =
+					_ui->graphicsView_graphView->isVisible() &&
+					_ui->graphicsView_graphView->isGridMapVisible() &&
+					_preferencesDialog->isGridMapFrom3DCloud() &&
+					_projectionLocalMaps.find(iter->first) == _projectionLocalMaps.end();
+			if(update3dCloud ||	updateProjMap)
 			{
+				// update cloud
+				std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createdCloud;
 				if(viewerClouds.contains(cloudName))
 				{
 					// Update only if the pose has changed
@@ -1933,14 +1941,24 @@ void MainWindow::updateMapCloud(
 				}
 				else if(_cachedClouds.find(iter->first) == _cachedClouds.end() && _cachedSignatures.contains(iter->first))
 				{
-					if((_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)) ||
+					createdCloud = this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
-					    _projectionLocalMaps.find(iter->first) == _projectionLocalMaps.end())
+					if(viewerClouds.contains(cloudName))
 					{
-						this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
+						_cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0));
-						if(viewerClouds.contains(cloudName))
+					}
-						{
+				}
-							_cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0));
+
-						}
+				//Update projection map
+				if(updateProjMap)
+				{
+					std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >::iterator cloudIter = _cachedClouds.find(iter->first);
+					if(cloudIter != _cachedClouds.end())
+					{
+						createAndAddProjectionMap(cloudIter->second.first, cloudIter->second.second, iter->first, iter->second);
+					}
+					else if(createdCloud.first->size() && createdCloud.second->size())
+					{
+						createAndAddProjectionMap(createdCloud.first, createdCloud.second, iter->first, iter->second);
 					}
 				}
 			}
@@ -2247,22 +2265,23 @@ void MainWindow::updateMapCloud(
 	UDEBUG("");
 }
 
-void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId)
+std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId)
 {
 	UDEBUG("");
 	UASSERT(!pose.isNull());
 	std::string cloudName = uFormat("cloud%d", nodeId);
+	std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> outputPair;
 	if(_cloudViewer->getAddedClouds().contains(cloudName))
 	{
 		UERROR("Cloud %d already added to map.", nodeId);
-		return;
+		return outputPair;
 	}
 
 	QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
 	if(iter == _cachedSignatures.end())
 	{
 		UERROR("Node %d is not in the cache.", nodeId);
-		return;
+		return outputPair;
 	}
 
 	UASSERT(_cachedClouds.find(nodeId) == _cachedClouds.end());
@@ -2286,7 +2305,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 					_preferencesDialog->getCloudDecimation(0),
 					image.cols,
 					image.rows);
-			return;
+			return outputPair;
 		}
 
 		// Create organized cloud
@@ -2336,49 +2355,6 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 					_preferencesDialog->getMapNoiseMinNeighbors());
 		}
 
-		if(indices->size() &&
-		  _preferencesDialog->isGridMapFrom3DCloud() &&
-		  _projectionLocalMaps.find(nodeId) == _projectionLocalMaps.end())
-		{
-			UTimer timer;
-			cv::Mat ground, obstacles;
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelCloud = cloud;
-
-			// voxelize to grid cell size
-			if(_preferencesDialog->getMapVoxel() < _preferencesDialog->getGridMapResolution())
-			{
-				voxelCloud = util3d::voxelize(voxelCloud, indices, _preferencesDialog->getGridMapResolution());
-			}
-
-			// add pose rotation without yaw
-			if(_preferencesDialog->projMapFrame())
-			{
-				float roll, pitch, yaw;
-				pose.getEulerAngles(roll, pitch, yaw);
-				voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0, pose.z(), roll, pitch, 0));
-			}
-
-			if(_preferencesDialog->projMaxObstaclesHeight())
-			{
-				voxelCloud = util3d::passThrough(voxelCloud, "z", std::numeric_limits<int>::min(), _preferencesDialog->projMaxObstaclesHeight());
-			}
-
-			util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(
-					voxelCloud,
-					ground,
-					obstacles,
-					_preferencesDialog->getGridMapResolution(),
-					_preferencesDialog->projMaxGroundAngle(),
-					_preferencesDialog->projMinClusterSize(),
-					_preferencesDialog->projFlatObstaclesDetected(),
-					_preferencesDialog->projMaxGroundHeight());
-			if(!ground.empty() || !obstacles.empty())
-			{
-				_projectionLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
-			}
-			UDEBUG("time gridMapFrom3DCloud = %f s", timer.ticks());
-		}
-
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 		if(_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0))
 		{
@@ -2443,7 +2419,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 					}
 
 
-					UWARN("Time subtract filtering %d from %d -> %d (%fs)",
+					UINFO("Time subtract filtering %d from %d -> %d (%fs)",
 							(int)_previousCloud.second.second->size(),
 							(int)beforeFiltering->size(),
 							(int)indices->size(),
@@ -2458,10 +2434,11 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 
 			if(indices->size())
 			{
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr output;
+				bool added = false;
 				if(_preferencesDialog->isCloudMeshing() && cloud->isOrganized())
 				{
 					// Fast organized mesh
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr output;
 					// we need to extract indices as pcl::OrganizedFastMesh doesn't take indices
 					output = util3d::extractIndices(cloud, indices, false, true);
 					std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
@@ -2480,10 +2457,9 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 						{
 							UERROR("Adding mesh cloud %d to viewer failed!", nodeId);
 						}
-						else if(_preferencesDialog->isCloudsKept())
+						else
 						{
-							_cachedClouds.insert(std::make_pair(nodeId, std::make_pair(output, indices)));
+							added = true;
-							_createdCloudsMemoryUsage += output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int);
 						}
 					}
 				}
@@ -2508,7 +2484,6 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 						color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
 					}
 
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr output;
 					output = util3d::extractIndices(cloud, indices, false, true);
 
 					if(cloudWithNormals->size())
@@ -2520,37 +2495,95 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 						{
 							UERROR("Adding cloud %d to viewer failed!", nodeId);
 						}
-						else if(_preferencesDialog->isCloudsKept())
+						else
 						{
-							_cachedClouds.insert(std::make_pair(nodeId, std::make_pair(output, indices)));
+							added = true;
-							_createdCloudsMemoryUsage += output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int);
 						}
 					}
 					else
 					{
-
 						if(!_cloudViewer->addCloud(cloudName, output, pose, color))
 						{
 							UERROR("Adding cloud %d to viewer failed!", nodeId);
 						}
-						else if(_preferencesDialog->isCloudsKept())
+						else
 						{
-							_cachedClouds.insert(std::make_pair(nodeId, std::make_pair(output, indices)));
+							added = true;
-							_createdCloudsMemoryUsage += output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int);
 						}
 					}
 				}
+				if(added)
+				{
+					outputPair.first = output;
+					outputPair.second = indices;
+					if(_preferencesDialog->isCloudsKept())
+					{
+						_cachedClouds.insert(std::make_pair(nodeId, outputPair));
+						_createdCloudsMemoryUsage += output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int);
+					}
+				}
 			}
 			_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
 			_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
 		}
 	}
-	else
+
+	return outputPair;
+	UDEBUG("");
+}
+
+void MainWindow::createAndAddProjectionMap(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		int nodeId,
+		const Transform & pose)
+{
+	UASSERT(!pose.isNull());
+
+	if(_projectionLocalMaps.find(nodeId) != _projectionLocalMaps.end())
 	{
+		UERROR("Projection map %d already added.", nodeId);
 		return;
 	}
 
-	UDEBUG("");
+	if(indices->size())
+	{
+		UTimer timer;
+		cv::Mat ground, obstacles;
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelCloud = cloud;
+
+		// voxelize to grid cell size
+		if(_preferencesDialog->getMapVoxel() < _preferencesDialog->getGridMapResolution())
+		{
+			voxelCloud = util3d::voxelize(voxelCloud, indices, _preferencesDialog->getGridMapResolution());
+		}
+
+		// add pose rotation without yaw
+		if(_preferencesDialog->projMapFrame())
+		{
+			float roll, pitch, yaw;
+			pose.getEulerAngles(roll, pitch, yaw);
+			voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0, pose.z(), roll, pitch, 0));
+		}
+
+		if(_preferencesDialog->projMaxObstaclesHeight())
+		{
+			voxelCloud = util3d::passThrough(voxelCloud, "z", std::numeric_limits<int>::min(), _preferencesDialog->projMaxObstaclesHeight());
+		}
+
+		util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(
+				voxelCloud,
+				ground,
+				obstacles,
+				_preferencesDialog->getGridMapResolution(),
+				_preferencesDialog->projMaxGroundAngle(),
+				_preferencesDialog->projMinClusterSize(),
+				_preferencesDialog->projFlatObstaclesDetected(),
+				_preferencesDialog->projMaxGroundHeight());
+
+		_projectionLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+		UDEBUG("time gridMapFrom3DCloud = %f s", timer.ticks());
+	}
 }
 
 void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int mapId)
@@ -2838,6 +2871,23 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible)
 	_cloudViewer->update();
 }
 
+void MainWindow::updateGraphView()
+{
+	if(_ui->dockWidget_graphViewer->isVisible())
+	{
+		UDEBUG("Graph visible!");
+		if(_currentPosesMap.size())
+		{
+			this->updateMapCloud(
+					std::map<int, Transform>(_currentPosesMap),
+					std::multimap<int, Link>(_currentLinksMap),
+					std::map<int, int>(_currentMapIds),
+					std::map<int, std::string>(_currentLabels),
+					std::map<int, Transform>(_currentGTPosesMap));
+		}
+	}
+}
+
 void MainWindow::processRtabmapEventInit(int status, const QString & info)
 {
 	if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitializing)

tools/CameraRGBD/main.cpp

@@ -342,10 +342,7 @@ int main(int argc, char * argv[])
 			{
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(
 						rgb, depth,
-						data.cameraModels()[0].cx(),
+						data.cameraModels()[0]);
-						data.cameraModels()[0].cy(),
-						data.cameraModels()[0].fx(),
-						data.cameraModels()[0].fy());
 				cloud = rtabmap::util3d::transformPointCloud(cloud, t);
 				if(viewer)
 					viewer->showCloud(cloud, "cloud");
@@ -356,10 +353,7 @@ int main(int argc, char * argv[])
 			{
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(
 						depth,
-						data.cameraModels()[0].cx(),
+						data.cameraModels()[0]);
-						data.cameraModels()[0].cy(),
-						data.cameraModels()[0].fx(),
-						data.cameraModels()[0].fy());
 				cloud = rtabmap::util3d::transformPointCloud(cloud, t);
 				viewer->showCloud(cloud, "cloud");
 			}