util3d_filtering: refactored implementations using templates. Parameters: Changed Grid/DepthMin|Max to Grid/RangeMin|Max, added Grid/PreVoxelFiltering, added GridBlobal/OctoMapOccupancyThr. OccupancyGrid: supporting input clouds already having normals. Memory: don't save working directory parameter to database.

2026-09-02 17:40:23 +08:00 · 2018-02-13 10:16:48 -05:00
parent 09cae9cbd3
commit 4c0a612ab5
16 changed files with 926 additions and 1203 deletions
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -324,8 +324,9 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			// Memory is empty, save parameters
 			ParametersMap parameters = Parameters::getDefaultParameters();
 			uInsert(parameters, parameters_);
+			parameters.erase(Parameters::kRtabmapWorkingDirectory()); // don't save working directory as it is machine dependent
 			UDEBUG("");
-			_dbDriver->addInfoAfterRun(0, 0,	0, 0, 0, parameters);
+			_dbDriver->addInfoAfterRun(0, 0, 0, 0, 0, parameters);
 		}
 	}
 	else
@@ -1395,6 +1396,7 @@ void Memory::clear()
 		{
 			ParametersMap parameters = Parameters::getDefaultParameters();
 			uInsert(parameters, parameters_);
+			parameters.erase(Parameters::kRtabmapWorkingDirectory()); // don't save working directory as it is machine dependent
 			UDEBUG("");
 			_dbDriver->addInfoAfterRun(memSize,
 					_lastSignature?_lastSignature->id():0,
--- a/corelib/src/OccupancyGrid.cpp
+++ b/corelib/src/OccupancyGrid.cpp
@@ -43,14 +43,15 @@ namespace rtabmap {
 OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	parameters_(parameters),
 	cloudDecimation_(Parameters::defaultGridDepthDecimation()),
-	cloudMaxDepth_(Parameters::defaultGridDepthMax()),
-	cloudMinDepth_(Parameters::defaultGridDepthMin()),
+	cloudMaxDepth_(Parameters::defaultGridRangeMax()),
+	cloudMinDepth_(Parameters::defaultGridRangeMin()),
 	//roiRatios_(Parameters::defaultGridDepthRoiRatios()), // initialized in parseParameters()
 	footprintLength_(Parameters::defaultGridFootprintLength()),
 	footprintWidth_(Parameters::defaultGridFootprintWidth()),
 	footprintHeight_(Parameters::defaultGridFootprintHeight()),
 	scanDecimation_(Parameters::defaultGridScanDecimation()),
 	cellSize_(Parameters::defaultGridCellSize()),
+	preVoxelFiltering_(Parameters::defaultGridPreVoxelFiltering()),
 	occupancyFromCloud_(Parameters::defaultGridFromDepth()),
 	projMapFrame_(Parameters::defaultGridMapFrameProjection()),
 	maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
@@ -92,8 +93,8 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	{
 		cloudDecimation_ = 1;
 	}
-	Parameters::parse(parameters, Parameters::kGridDepthMin(), cloudMinDepth_);
-	Parameters::parse(parameters, Parameters::kGridDepthMax(), cloudMaxDepth_);
+	Parameters::parse(parameters, Parameters::kGridRangeMin(), cloudMinDepth_);
+	Parameters::parse(parameters, Parameters::kGridRangeMax(), cloudMaxDepth_);
 	Parameters::parse(parameters, Parameters::kGridFootprintLength(), footprintLength_);
 	Parameters::parse(parameters, Parameters::kGridFootprintWidth(), footprintWidth_);
 	Parameters::parse(parameters, Parameters::kGridFootprintHeight(), footprintHeight_);
@@ -103,6 +104,8 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	{
 		this->setCellSize(cellSize);
 	}
+
+	Parameters::parse(parameters, Parameters::kGridPreVoxelFiltering(), preVoxelFiltering_);
 	Parameters::parse(parameters, Parameters::kGridMapFrameProjection(), projMapFrame_);
 	Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_);
 	Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
@@ -237,8 +240,14 @@ void OccupancyGrid::createLocalMap(
 				node.sensorData().laserScanInfo().localTransform().y(),
 				node.sensorData().laserScanInfo().localTransform().z());

+		cv::Mat scan = node.sensorData().laserScanRaw();
+		if(cloudMinDepth_ > 0.0f || cloudMaxDepth_ > 0.0f)
+		{
+			scan = util3d::rangeFiltering(scan, cloudMinDepth_, cloudMaxDepth_);
+		}
+
 		util3d::occupancy2DFromLaserScan(
-				util3d::transformLaserScan(node.sensorData().laserScanRaw(), node.sensorData().laserScanInfo().localTransform()),
+				util3d::transformLaserScan(scan, node.sensorData().laserScanInfo().localTransform()),
 				cv::Mat(),
 				viewPoint,
 				emptyCells,
@@ -252,22 +261,52 @@ void OccupancyGrid::createLocalMap(
 	else
 	{
 		// 3D
-		pcl::IndicesPtr indices(new std::vector<int>);
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 		if(!occupancyFromCloud_)
 		{
-			UDEBUG("3D laser scan");
-			const Transform & t = node.sensorData().laserScanInfo().localTransform();
-			cv::Mat scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
-			cloud = util3d::laserScanToPointCloudRGB(
-					scan,
-					t);
+			if(!node.sensorData().laserScanRaw().empty())
+			{
+				UDEBUG("3D laser scan");
+				const Transform & t = node.sensorData().laserScanInfo().localTransform();
+				cv::Mat scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);

-			// update viewpoint
-			viewPoint = cv::Point3f(t.x(), t.y(), t.z());
+				if(cloudMinDepth_ > 0.0f || cloudMaxDepth_ > 0.0f)
+				{
+					scan = util3d::rangeFiltering(scan, cloudMinDepth_, cloudMaxDepth_);
+				}
+
+				// update viewpoint
+				viewPoint = cv::Point3f(t.x(), t.y(), t.z());
+
+				if(scan.channels() == 6)
+				{
+					pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(scan, t);
+					createLocalMap<pcl::PointNormal>(cloud, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
+				}
+				else if(scan.channels() == 7)
+				{
+					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = util3d::laserScanToPointCloudRGBNormal(scan, t);
+					createLocalMap<pcl::PointXYZRGBNormal>(cloud, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
+				}
+				else if(scan.channels() == 4)
+				{
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(scan, t);
+					createLocalMap<pcl::PointXYZRGB>(cloud, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
+				}
+				else
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, t);
+					createLocalMap<pcl::PointXYZ>(cloud, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
+				}
+			}
+			else
+			{
+				UWARN("Cannot create local map, scan is empty (node=%d).", node.id());
+			}
 		}
 		else
 		{
+			pcl::IndicesPtr indices(new std::vector<int>);
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 			UDEBUG("Depth image : decimation=%d max=%f min=%f",
 					cloudDecimation_,
 					cloudMaxDepth_,
@@ -309,152 +348,98 @@ void OccupancyGrid::createLocalMap(
 				const Transform & t = node.sensorData().stereoCameraModel().localTransform();
 				viewPoint = cv::Point3f(t.x(), t.y(), t.z());
 			}
+			createLocalMap<pcl::PointXYZRGB>(cloud, indices, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
 		}
-		createLocalMap(cloud, indices, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
 	}
 }

-void OccupancyGrid::createLocalMap(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, // in base_link frame
+void OccupancyGrid::createLocalMapImpl(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & groundCloud,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstaclesCloud,
 		const Transform & pose,
 		cv::Mat & groundCells,
 		cv::Mat & obstacleCells,
 		cv::Mat & emptyCells,
-		cv::Point3f & viewPointInOut) const
+		const cv::Point3f & viewPoint) const
 {
-	pcl::IndicesPtr indices(new std::vector<int>);
-	UASSERT_MSG(cloud->size() && cloud->is_dense, uFormat("Use interface with indices if cloud is not dense.").c_str());
-	createLocalMap(cloud, indices, pose, groundCells, obstacleCells, emptyCells, viewPointInOut);
-}
-
-void OccupancyGrid::createLocalMap(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, // in base_link frame
-		const pcl::IndicesPtr & indices,
-		const Transform & pose,
-		cv::Mat & groundCells,
-		cv::Mat & obstacleCells,
-		cv::Mat & emptyCells,
-		cv::Point3f & viewPointInOut) const
-{
-	if(projMapFrame_)
+	if(grid3D_)
 	{
-		//we should rotate viewPoint in /map frame
+		UDEBUG("");
+		if(groundIsObstacle_)
+		{
+			*obstaclesCloud += *groundCloud;
+			groundCloud->clear();
+		}
+
+		// transform back in base frame
 		float roll, pitch, yaw;
 		pose.getEulerAngles(roll, pitch, yaw);
-		Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z, 0,0,0);
-		viewPointInOut.x = viewpointRotated.x();
-		viewPointInOut.y = viewpointRotated.y();
-		viewPointInOut.z = viewpointRotated.z();
-	}
+		Transform tinv = Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0).inverse();

-	if((cloud->is_dense && cloud->size()) ||
-		(!cloud->is_dense && indices->size()))
-	{
-		pcl::IndicesPtr groundIndices(new std::vector<int>);
-		pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudSegmented = this->segmentCloud<pcl::PointXYZRGB>(
-				cloud,
-				indices,
-				pose,
-				viewPointInOut,
-				groundIndices,
-				obstaclesIndices);
-
-		if(!groundIndices->empty() || !obstaclesIndices->empty())
+		if(rayTracing_)
 		{
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-
-			if(groundIndices->size())
-			{
-				pcl::copyPointCloud(*cloudSegmented, *groundIndices, *groundCloud);
-			}
-
-			if(obstaclesIndices->size())
-			{
-				pcl::copyPointCloud(*cloudSegmented, *obstaclesIndices, *obstaclesCloud);
-			}
-
-			if(grid3D_)
-			{
-				UDEBUG("");
-				if(groundIsObstacle_)
-				{
-					*obstaclesCloud += *groundCloud;
-					groundCloud->clear();
-				}
-
-				// transform back in base frame
-				float roll, pitch, yaw;
-				pose.getEulerAngles(roll, pitch, yaw);
-				Transform tinv = Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0).inverse();
-
-				if(rayTracing_)
-				{
 #ifdef RTABMAP_OCTOMAP
-					if(!groundCloud->empty() || !obstaclesCloud->empty())
-					{
-						//create local octomap
-						OctoMap octomap(cellSize_);
-						octomap.addToCache(1, groundCloud, obstaclesCloud, pcl::PointXYZ(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z));
-						std::map<int, Transform> poses;
-						poses.insert(std::make_pair(1, Transform::getIdentity()));
-						octomap.update(poses);
-
-						obstaclesIndices->clear();
-						groundIndices->clear();
-						pcl::IndicesPtr emptyIndices(new std::vector<int>);
-						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithRayTracing = octomap.createCloud(0, obstaclesIndices.get(), emptyIndices.get(), groundIndices.get());
-						UDEBUG("ground=%d obstacles=%d empty=%d", (int)groundIndices->size(), (int)obstaclesIndices->size(), (int)emptyIndices->size());
-						groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv);
-						obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv);
-						emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv);
-					}
-				}
-				else
-#else
-					UWARN("RTAB-Map is not built with OctoMap dependency, 3D ray tracing is ignored. Set \"%s\" to false to avoid this warning.", Parameters::kGridRayTracing().c_str());
-				}
-#endif
-				{
-					groundCells = util3d::laserScanFromPointCloud(*groundCloud, tinv);
-					obstacleCells = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv);
-				}
-
-			}
-			else
+			if(!groundCloud->empty() || !obstaclesCloud->empty())
 			{
-				UDEBUG("groundCloud=%d, obstaclesCloud=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
-				// projection on the xy plane
-				util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
-						groundCloud,
-						obstaclesCloud,
-						groundCells,
-						obstacleCells,
-						cellSize_);
+				//create local octomap
+				OctoMap octomap(cellSize_);
+				octomap.addToCache(1, groundCloud, obstaclesCloud, pcl::PointXYZ(viewPoint.x, viewPoint.y, viewPoint.z));
+				std::map<int, Transform> poses;
+				poses.insert(std::make_pair(1, Transform::getIdentity()));
+				octomap.update(poses);

-				if(rayTracing_)
-				{
-					cv::Mat laserScan = obstacleCells;
-					cv::Mat laserScanNoHit = groundCells;
-					obstacleCells = cv::Mat();
-					groundCells = cv::Mat();
-					util3d::occupancy2DFromLaserScan(
-							laserScan,
-							laserScanNoHit,
-							viewPointInOut,
-							emptyCells,
-							obstacleCells,
-							cellSize_,
-							false, // don't fill unknown space
-							0);
-				}
+				pcl::IndicesPtr groundIndices(new std::vector<int>);
+				pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
+				pcl::IndicesPtr emptyIndices(new std::vector<int>);
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithRayTracing = octomap.createCloud(0, obstaclesIndices.get(), emptyIndices.get(), groundIndices.get());
+				UDEBUG("ground=%d obstacles=%d empty=%d", (int)groundIndices->size(), (int)obstaclesIndices->size(), (int)emptyIndices->size());
+				groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv);
+				obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv);
+				emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv);
 			}
 		}
+		else
+#else
+			UWARN("RTAB-Map is not built with OctoMap dependency, 3D ray tracing is ignored. Set \"%s\" to false to avoid this warning.", Parameters::kGridRayTracing().c_str());
+		}
+#endif
+		{
+			groundCells = util3d::laserScanFromPointCloud(*groundCloud, tinv);
+			obstacleCells = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv);
+		}
+
+	}
+	else
+	{
+		UDEBUG("groundCloud=%d, obstaclesCloud=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
+		// projection on the xy plane
+		util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
+				groundCloud,
+				obstaclesCloud,
+				groundCells,
+				obstacleCells,
+				cellSize_);
+
+		if(rayTracing_)
+		{
+			cv::Mat laserScan = obstacleCells;
+			cv::Mat laserScanNoHit = groundCells;
+			obstacleCells = cv::Mat();
+			groundCells = cv::Mat();
+			util3d::occupancy2DFromLaserScan(
+					laserScan,
+					laserScanNoHit,
+					viewPoint,
+					emptyCells,
+					obstacleCells,
+					cellSize_,
+					false, // don't fill unknown space
+					0);
+		}
 	}
-	UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());
 }

+
 void OccupancyGrid::clear()
 {
 	cache_.clear();
--- a/corelib/src/OctoMap.cpp
+++ b/corelib/src/OctoMap.cpp
@@ -262,7 +262,7 @@ RtabmapColorOcTree::StaticMemberInitializer::StaticMemberInitializer() {
 // OctoMap
 //////////////////////////////////////

-OctoMap::OctoMap(const ParametersMap & parameters, float occupancyThr) :
+OctoMap::OctoMap(const ParametersMap & parameters) :
 		hasColor_(false),
 		fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
 		updateError_(Parameters::defaultGridGlobalUpdateError())
@@ -274,6 +274,9 @@ OctoMap::OctoMap(const ParametersMap & parameters, float occupancyThr) :
 	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
 	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;

+	float occupancyThr = Parameters::defaultGridGlobalOctoMapOccupancyThr();
+	Parameters::parse(parameters, Parameters::kGridGlobalOctoMapOccupancyThr(), occupancyThr);
+
 	octree_ = new RtabmapColorOcTree(cellSize);
 	octree_->setOccupancyThres(occupancyThr);
 	Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
@@ -313,12 +316,13 @@ void OctoMap::clear()
 }

 void OctoMap::addToCache(int nodeId,
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
 		const pcl::PointXYZ & viewPoint)
 {
 	UDEBUG("nodeId=%d", nodeId);
-	uInsert(cacheClouds_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+	cacheClouds_.erase(nodeId);
+	cacheClouds_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
 	uInsert(cacheViewPoints_, std::make_pair(nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
 }
 void OctoMap::addToCache(int nodeId,
@@ -509,7 +513,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 	UDEBUG("orderedPoses = %d", (int)orderedPoses.size());
 	for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
 	{
-		std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
+		std::map<int, std::pair<const pcl::PointCloud<pcl::PointXYZRGB>::Ptr, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
 		std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator occupancyIter;
 		std::map<int, cv::Point3f>::iterator viewPointIter;
 		cloudIter = cacheClouds_.find(iter->first);
@@ -575,7 +579,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 					}

 					updateMinMax(point);
-					RtabmapColorOcTreeNode * n = octree_->updateNode(key, false);
+					RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);

 					if(n)
 					{
@@ -659,11 +663,24 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 			// mark free cells only if not seen occupied in this cloud
 			for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it)
 			{
+				if(iter->first > 0)
+				{
+					RtabmapColorOcTreeNode * n = octree_->search(*it);
+					if(n && n->getNodeRefId() > 0 && n->getNodeRefId() >= iter->first)
+					{
+						// The cell has been updated from current node or more recent node, don't update the cell
+						continue;
+					}
+				}
+
 				RtabmapColorOcTreeNode * n = octree_->updateNode(*it, false);
 				if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
 				{
 					n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
-					n->setNodeRefId(iter->first);
+					if(iter->first > 0)
+					{
+						n->setNodeRefId(iter->first);
+					}
 				}
 			}

@@ -683,23 +700,27 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 					octomap::OcTreeKey key;
 					if (octree_->coordToKeyChecked(point, key))
 					{
-						updateMinMax(point);

-						if(iter->first >0 && iter->first<lastId)
+						if(iter->first >0)
 						{
 							RtabmapColorOcTreeNode * n = octree_->search(key);
-							if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
+							if(n && n->getNodeRefId() > 0 && n->getNodeRefId() >= iter->first)
 							{
-								// The cell has been updated from more recent node, don't update the cell
+								// The cell has been updated from current node or more recent node, don't update the cell
 								continue;
 							}
 						}

+						updateMinMax(point);
+
 						RtabmapColorOcTreeNode * n = octree_->updateNode(key, false);
 						if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
 						{
 							n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
-							n->setNodeRefId(iter->first);
+							if(iter->first > 0)
+							{
+								n->setNodeRefId(iter->first);
+							}
 						}
 					}
 				}
@@ -847,7 +868,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 	float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f;
 	for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
 	{
-		if(octree_->isNodeOccupied(*it) && (obstacleIndices != 0 || addAllPoints))
+		if(octree_->isNodeOccupied(*it) && (obstacleIndices != 0 || groundIndices != 0 || addAllPoints))
 		{
 			octomap::point3d pt = octree_->keyToCoord(it.getKey());
 			if(octree_->getTreeDepth() == it.getDepth() && hasColor_)
@@ -879,20 +900,6 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 				(*cloud)[oi].z = pt.z();
 			}

-			if(obstacleIndices)
-			{
-				obstacleIndices->at(si++) = oi;
-			}
-
-			++oi;
-		}
-		else if(!octree_->isNodeOccupied(*it) && (emptyIndices != 0 || groundIndices != 0 || addAllPoints))
-		{
-			octomap::point3d pt = octree_->keyToCoord(it.getKey());
-			(*cloud)[oi]  = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
-			(*cloud)[oi].x = pt.x()-halfCellSize;
-			(*cloud)[oi].y = pt.y()-halfCellSize;
-			(*cloud)[oi].z = pt.z();
 			if(it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeGround)
 			{
 				if(groundIndices)
@@ -900,7 +907,21 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 					groundIndices->at(gi++) = oi;
 				}
 			}
-			else if(emptyIndices)
+			else if(obstacleIndices)
+			{
+				obstacleIndices->at(si++) = oi;
+			}
+
+			++oi;
+		}
+		else if(!octree_->isNodeOccupied(*it) && (emptyIndices != 0 || addAllPoints))
+		{
+			octomap::point3d pt = octree_->keyToCoord(it.getKey());
+			(*cloud)[oi]  = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
+			(*cloud)[oi].x = pt.x()-halfCellSize;
+			(*cloud)[oi].y = pt.y()-halfCellSize;
+			(*cloud)[oi].z = pt.z();
+			if(emptyIndices)
 			{
 				emptyIndices->at(ei++) = oi;
 			}
@@ -950,7 +971,7 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
 	for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
 	{
 		octomap::point3d pt = octree_->keyToCoord(it.getKey());
-		if(octree_->isNodeOccupied(*it))
+		if(octree_->isNodeOccupied(*it) && it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeObstacle)
 		{
 			(*obstacles)[oi++]  = pcl::PointXYZ(pt.x()-halfCellSize, pt.y()-halfCellSize, 0); // projected on ground
 		}
--- a/corelib/src/Parameters.cpp
+++ b/corelib/src/Parameters.cpp
@@ -228,6 +228,8 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo

 		// 0.16.0
 		removedParameters_.insert(std::make_pair("Grid/ProjRayTracing",           std::make_pair(true,  Parameters::kGridRayTracing())));
+		removedParameters_.insert(std::make_pair("Grid/DepthMin",                 std::make_pair(true,  Parameters::kGridRangeMin())));
+		removedParameters_.insert(std::make_pair("Grid/DepthMax",                 std::make_pair(true,  Parameters::kGridRangeMax())));

 		// 0.15.1
 		removedParameters_.insert(std::make_pair("Reg/VarianceFromInliersCount",  std::make_pair(false, "")));
--- a/corelib/src/util3d_filtering.cpp
+++ b/corelib/src/util3d_filtering.cpp