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.

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -70,15 +70,17 @@ public:
 			cv::Mat & emptyCells,
 			cv::Point3f & viewPoint) const;
 
+	template<typename PointT>
 	void createLocalMap(
-			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, // in base_link frame
+			const typename pcl::PointCloud<PointT>::Ptr cloud, // in base_link frame
 			const Transform & pose,
 			cv::Mat & groundCells,
 			cv::Mat & obstacleCells,
 			cv::Mat & emptyCells,
 			cv::Point3f & viewPointInOut) const;
+	template<typename PointT>
 	void createLocalMap(
-			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, // in base_link frame
+			const typename pcl::PointCloud<PointT>::Ptr cloud, // in base_link frame
 			const pcl::IndicesPtr & indices,
 			const Transform & pose,
 			cv::Mat & groundCells,
@@ -98,6 +100,16 @@ public:
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
 
+private:
+	void 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,
+			const cv::Point3f & viewPoint) const;
+
 private:
 	ParametersMap parameters_;
 	int cloudDecimation_;
@@ -109,6 +121,7 @@ private:
 	float footprintHeight_;
 	int scanDecimation_;
 	float cellSize_;
+	bool preVoxelFiltering_;
 	bool occupancyFromCloud_;
 	bool projMapFrame_;
 	float maxObstacleHeight_;

corelib/include/rtabmap/core/OctoMap.h

@@ -55,14 +55,14 @@ public:
 public:
 	friend class RtabmapColorOcTree; // needs access to node children (inherited)
 
-	RtabmapColorOcTreeNode() : ColorOcTreeNode(), nodeRefId_(0), type_(-1) {}
+	RtabmapColorOcTreeNode() : ColorOcTreeNode(), nodeRefId_(0), type_(kTypeUnknown) {}
 	RtabmapColorOcTreeNode(const RtabmapColorOcTreeNode& rhs) : ColorOcTreeNode(rhs), nodeRefId_(rhs.nodeRefId_), type_(rhs.type_) {}
 
 	void setNodeRefId(int nodeRefId) {nodeRefId_ = nodeRefId;}
 	void setOccupancyType(char type) {type_=type;}
 	void setPointRef(const octomap::point3d & point) {pointRef_ = point;}
 	int getNodeRefId() const {return nodeRefId_;}
-	char getOccupancyType() const {return type_;}
+	int getOccupancyType() const {return type_;}
 	const octomap::point3d & getPointRef() const {return pointRef_;}
 
 	// following methods defined for octomap < 1.8 compatibility
@@ -74,7 +74,7 @@ public:
 
 private:
 	int nodeRefId_;
-	char type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground
+	int type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground
 	octomap::point3d pointRef_;
 };
 
@@ -171,13 +171,13 @@ public:
 	static void HSVtoRGB(float *r, float *g, float *b, float h, float s, float v);
 
 public:
-	OctoMap(const ParametersMap & parameters, float occupancyThr = 0.5f);
+	OctoMap(const ParametersMap & parameters);
 	OctoMap(float cellSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false, float updateError=0.01f);
 
 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 	void 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);
 	void addToCache(int nodeId,
 			const cv::Mat & ground,
@@ -215,7 +215,7 @@ private:
 
 private:
 	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; // [id: < <ground, obstacles>, empty>]
-	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cacheClouds_; // [id: <ground, obstacles>]
+	std::map<int, std::pair<const pcl::PointCloud<pcl::PointXYZRGB>::Ptr, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cacheClouds_; // [id: <ground, obstacles>]
 	std::map<int, cv::Point3f> cacheViewPoints_;
 	RtabmapColorOcTree * octree_;
 	std::map<int, Transform> addedNodes_;

corelib/include/rtabmap/core/Parameters.h

@@ -589,14 +589,15 @@ class RTABMAP_EXP Parameters
     // Occupancy Grid
     RTABMAP_PARAM(Grid, FromDepth,               bool,   true,    "Create occupancy grid from depth image(s), otherwise it is created from laser scan.");
     RTABMAP_PARAM(Grid, DepthDecimation,         int,    4,       uFormat("[%s=true] Decimation of the depth image before creating cloud. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).", kGridDepthDecimation().c_str()));
-    RTABMAP_PARAM(Grid, DepthMin,                float,  0.0,     uFormat("[%s=true] Minimum cloud's depth from sensor.", kGridFromDepth().c_str()));
-    RTABMAP_PARAM(Grid, DepthMax,                float,  4.0,     uFormat("[%s=true] Maximum cloud's depth from sensor. 0=inf.", kGridFromDepth().c_str()));
+    RTABMAP_PARAM(Grid, RangeMin,                float,  0.0,     "Minimum range from sensor.");
+    RTABMAP_PARAM(Grid, RangeMax,                float,  5.0,     "Maximum range from sensor. 0=inf.");
     RTABMAP_PARAM_STR(Grid, DepthRoiRatios,      "0.0 0.0 0.0 0.0", uFormat("[%s=true] Region of interest ratios [left, right, top, bottom].", kGridFromDepth().c_str()));
     RTABMAP_PARAM(Grid, FootprintLength,         float,  0.0,     "Footprint length used to filter points over the footprint of the robot.");
     RTABMAP_PARAM(Grid, FootprintWidth,          float,  0.0,     "Footprint width used to filter points over the footprint of the robot. Footprint length should be set.");
     RTABMAP_PARAM(Grid, FootprintHeight,         float,  0.0,     "Footprint height used to filter points over the footprint of the robot. Footprint length and width should be set.");
     RTABMAP_PARAM(Grid, ScanDecimation,          int,    1,       uFormat("[%s=false] Decimation of the laser scan before creating cloud.", kGridFromDepth().c_str()));
     RTABMAP_PARAM(Grid, CellSize,                float,  0.05,    "Resolution of the occupancy grid.");
+    RTABMAP_PARAM(Grid, PreVoxelFiltering,       bool,   true,    uFormat("Input cloud is downsampled by voxel filter (voxel size is \"%s\") before doing segmentation of obstacles and ground.", kGridCellSize().c_str()));
     RTABMAP_PARAM(Grid, MapFrameProjection,      bool,   false,   "Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead.");
     RTABMAP_PARAM(Grid, NormalsSegmentation,     bool,   true,    "Segment ground from obstacles using point normals, otherwise a fast passthrough is used.");
     RTABMAP_PARAM(Grid, MaxObstacleHeight,       float,  0.0,     "Maximum obstacles height (0=disabled).");
@@ -625,6 +626,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(GridGlobal, MinSize,              float,  0.0,     "Minimum map size (m).");
     RTABMAP_PARAM(GridGlobal, Eroded,               bool,   false,   "Erode obstacle cells.");
     RTABMAP_PARAM(GridGlobal, MaxNodes,             int,    0,       "Maximum nodes assembled in the map starting from the last node (0=unlimited).");
+    RTABMAP_PARAM(GridGlobal, OctoMapOccupancyThr,  float,  0.5,     "OctoMap occupancy threshold (value between 0 and 1).");
 
 public:
     virtual ~Parameters();

corelib/include/rtabmap/core/impl/OccupancyGrid.hpp

@@ -45,14 +45,34 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 		pcl::IndicesPtr * flatObstacles) const
 {
 	typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
-
-	// voxelize to grid cell size
-	cloud = util3d::voxelize(cloudIn, indicesIn, cellSize_);
 	pcl::IndicesPtr indices(new std::vector<int>);
-	indices->resize(cloud->size());
-	for(unsigned int i=0; i<indices->size(); ++i)
+
+	if(preVoxelFiltering_)
 	{
-		indices->at(i) = i;
+		// voxelize to grid cell size
+		cloud = util3d::voxelize(cloudIn, indicesIn, cellSize_);
+
+		indices->resize(cloud->size());
+		for(unsigned int i=0; i<indices->size(); ++i)
+		{
+			indices->at(i) = i;
+		}
+	}
+	else
+	{
+		cloud = cloudIn;
+		if(indicesIn->empty() && cloud->is_dense)
+		{
+			indices->resize(cloud->size());
+			for(unsigned int i=0; i<indices->size(); ++i)
+			{
+				indices->at(i) = i;
+			}
+		}
+		else
+		{
+			indices = indicesIn;
+		}
 	}
 
 	// add pose rotation without yaw
@@ -166,6 +186,74 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 	return cloud;
 }
 
+template<typename PointT>
+void OccupancyGrid::createLocalMap(
+		const typename pcl::PointCloud<PointT>::Ptr cloud, // in base_link frame
+		const Transform & pose,
+		cv::Mat & groundCells,
+		cv::Mat & obstacleCells,
+		cv::Mat & emptyCells,
+		cv::Point3f & viewPointInOut) const
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	createLocalMap<PointT>(cloud, indices, pose, groundCells, obstacleCells, emptyCells, viewPointInOut);
+}
+
+template<typename PointT>
+void OccupancyGrid::createLocalMap(
+		const typename pcl::PointCloud<PointT>::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_)
+	{
+		//we should rotate viewPoint in /map 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();
+	}
+
+	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>);
+		typename pcl::PointCloud<PointT>::Ptr cloudSegmented = segmentCloud<PointT>(
+				cloud,
+				indices,
+				pose,
+				viewPointInOut,
+				groundIndices,
+				obstaclesIndices);
+
+		if(!groundIndices->empty() || !obstaclesIndices->empty())
+		{
+			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);
+			}
+
+			createLocalMapImpl(groundCloud, obstaclesCloud, pose, groundCells, obstacleCells, emptyCells, viewPointInOut);
+		}
+	}
+	UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());
+}
+
 }
 
 

corelib/include/rtabmap/core/impl/util3d_mapping.hpp

@@ -112,8 +112,7 @@ void segmentObstaclesFromGround(
 						{
 							Eigen::Vector4f centroid(0,0,0,1);
 							pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
-							if(centroid[2] >= min[2]-0.01 &&
-							  (centroid[2] <= max[2]+0.01 || (maxGroundHeight!=0.0f && centroid[2] <= maxGroundHeight+0.01))) // epsilon
+							if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight) // epsilon
 							{
 								ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
 							}

corelib/include/rtabmap/core/util3d_filtering.h

@@ -42,6 +42,11 @@ namespace rtabmap
 namespace util3d
 {
 
+cv::Mat RTABMAP_EXP rangeFiltering(
+		const cv::Mat & scan,
+		float rangeMin,
+		float rangeMax);
+
 cv::Mat RTABMAP_EXP downsample(
 		const cv::Mat & cloud,
 		int step);
@@ -129,6 +134,20 @@ pcl::IndicesPtr RTABMAP_EXP passThrough(
 		float min,
 		float max,
 		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP passThrough(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const std::string & axis,
@@ -161,6 +180,13 @@ pcl::IndicesPtr RTABMAP_EXP cropBox(
 		const Eigen::Vector4f & max,
 		const Transform & transform = Transform::getIdentity(),
 		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
 pcl::IndicesPtr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
@@ -168,18 +194,37 @@ pcl::IndicesPtr RTABMAP_EXP cropBox(
 		const Eigen::Vector4f & max,
 		const Transform & transform = Transform::getIdentity(),
 		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const Eigen::Vector4f & min,
 		const Eigen::Vector4f & max,
 		const Transform & transform = Transform::getIdentity(),
 		bool negative = false);
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const Eigen::Vector4f & min,
 		const Eigen::Vector4f & max,
 		const Transform & transform = Transform::getIdentity(),
 		bool negative = false);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
 
 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
 pcl::IndicesPtr RTABMAP_EXP frustumFiltering(
@@ -443,6 +488,13 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
 		const Eigen::Vector4f & viewpoint);
+pcl::IndicesPtr RTABMAP_EXP normalFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		int normalKSearch,
+		const Eigen::Vector4f & viewpoint);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
@@ -486,6 +538,13 @@ std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
 		int minClusterSize,
 		int maxClusterSize = std::numeric_limits<int>::max(),
 		int * biggestClusterIndex = 0);
+std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize = std::numeric_limits<int>::max(),
+		int * biggestClusterIndex = 0);
 std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
@@ -505,6 +564,10 @@ pcl::IndicesPtr RTABMAP_EXP extractIndices(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		bool negative);
+pcl::IndicesPtr RTABMAP_EXP extractIndices(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		bool negative);
 pcl::IndicesPtr RTABMAP_EXP extractIndices(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
@@ -524,6 +587,11 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP extractIndices(
 		const pcl::IndicesPtr & indices,
 		bool negative,
 		bool keepOrganized);
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP extractIndices(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		bool negative,
+		bool keepOrganized);
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP extractIndices(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,

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,

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();

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
 		}

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, "")));