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,
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
+			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, RangeMin,                float,  0.0,     "Minimum range from sensor.");
-    RTABMAP_PARAM(Grid, DepthMax,                float,  4.0,     uFormat("[%s=true] Maximum cloud's depth from sensor. 0=inf.", kGridFromDepth().c_str()));
+    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 &&
+							if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight) // epsilon
-							  (centroid[2] <= max[2]+0.01 || (maxGroundHeight!=0.0f && centroid[2] <= maxGroundHeight+0.01))) // 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()),
+	cloudMaxDepth_(Parameters::defaultGridRangeMax()),
-	cloudMinDepth_(Parameters::defaultGridDepthMin()),
+	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::kGridRangeMin(), cloudMinDepth_);
-	Parameters::parse(parameters, Parameters::kGridDepthMax(), cloudMaxDepth_);
+	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");
+			if(!node.sensorData().laserScanRaw().empty())
-			const Transform & t = node.sensorData().laserScanInfo().localTransform();
+			{
-			cv::Mat scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
+				UDEBUG("3D laser scan");
-			cloud = util3d::laserScanToPointCloudRGB(
+				const Transform & t = node.sensorData().laserScanInfo().localTransform();
-					scan,
+				cv::Mat scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
-					t);
 
-			// update viewpoint
+				if(cloudMinDepth_ > 0.0f || cloudMaxDepth_ > 0.0f)
-			viewPoint = cv::Point3f(t.x(), t.y(), t.z());
+				{
+					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(
+void OccupancyGrid::createLocalMapImpl(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, // in base_link frame
+		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>);
+	if(grid3D_)
-	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_)
 	{
-		//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);
+		Transform tinv = Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0).inverse();
-		viewPointInOut.x = viewpointRotated.x();
-		viewPointInOut.y = viewpointRotated.y();
-		viewPointInOut.z = viewpointRotated.z();
-	}
 
-	if((cloud->is_dense && cloud->size()) ||
+		if(rayTracing_)
-		(!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())
 		{
-			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())
+			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
 			{
-				UDEBUG("groundCloud=%d, obstaclesCloud=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
+				//create local octomap
-				// projection on the xy plane
+				OctoMap octomap(cellSize_);
-				util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
+				octomap.addToCache(1, groundCloud, obstaclesCloud, pcl::PointXYZ(viewPoint.x, viewPoint.y, viewPoint.z));
-						groundCloud,
+				std::map<int, Transform> poses;
-						obstaclesCloud,
+				poses.insert(std::make_pair(1, Transform::getIdentity()));
-						groundCells,
+				octomap.update(poses);
-						obstacleCells,
-						cellSize_);
 
-				if(rayTracing_)
+				pcl::IndicesPtr groundIndices(new std::vector<int>);
-				{
+				pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
-					cv::Mat laserScan = obstacleCells;
+				pcl::IndicesPtr emptyIndices(new std::vector<int>);
-					cv::Mat laserScanNoHit = groundCells;
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithRayTracing = octomap.createCloud(0, obstaclesIndices.get(), emptyIndices.get(), groundIndices.get());
-					obstacleCells = cv::Mat();
+				UDEBUG("ground=%d obstacles=%d empty=%d", (int)groundIndices->size(), (int)obstaclesIndices->size(), (int)emptyIndices->size());
-					groundCells = cv::Mat();
+				groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv);
-					util3d::occupancy2DFromLaserScan(
+				obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv);
-							laserScan,
+				emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv);
-							laserScanNoHit,
-							viewPointInOut,
-							emptyCells,
-							obstacleCells,
-							cellSize_,
-							false, // don't fill unknown space
-							0);
-				}
 			}
 		}
+		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,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
+		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, "")));

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -176,7 +176,6 @@ public:
 	int getOctomapRenderingType() const;
 	bool isOctomap2dGrid() const;
 	int getOctomapTreeDepth() const;
-	double getOctomapOccupancyThr() const;
 	int getOctomapPointSize() const;
 	int getCloudDecimation(int index) const;   // 0=map, 1=odom
 	double getCloudMaxDepth(int index) const;  // 0=map, 1=odom

guilib/src/DatabaseViewer.cpp

@@ -327,7 +327,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->checkBox_grid_2d, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
 	connect(ui_->comboBox_octomap_rendering_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateOctomapView()));
 	connect(ui_->spinBox_grid_depth, SIGNAL(valueChanged(int)), this, SLOT(updateOctomapView()));
-	connect(ui_->checkBox_grid_empty, SIGNAL(stateChanged(int)), this, SLOT(updateOctomapView()));
+	connect(ui_->checkBox_grid_empty, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
 	connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
 	connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
 	connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(update3dView()));
@@ -2411,7 +2411,7 @@ void DatabaseViewer::regenerateLocalMaps()
 						viewpoint = cv::Point3f(t.x(), t.y(), t.z());
 					}
 
-					grid.createLocalMap(cloud, s.getPose(), ground, obstacles, empty, viewpoint);
+					grid.createLocalMap<pcl::PointXYZRGB>(cloud, s.getPose(), ground, obstacles, empty, viewpoint);
 				}
 			}
 			else
@@ -2533,7 +2533,7 @@ void DatabaseViewer::regenerateCurrentLocalMaps()
 						viewpoint = cv::Point3f(t.x(), t.y(), t.z());
 					}
 
-					grid.createLocalMap(cloud, s.getPose(), ground, obstacles, empty, viewpoint);
+					grid.createLocalMap<pcl::PointXYZRGB>(cloud, s.getPose(), ground, obstacles, empty, viewpoint);
 				}
 			}
 			else
@@ -4575,7 +4575,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 #ifdef RTABMAP_OCTOMAP
 			if(ui_->checkBox_octomap->isChecked())
 			{
-				octomap_ = new OctoMap(cellSize);
+				octomap_ = new OctoMap(parameters);
 				bool updateAborted = false;
 				for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
 				{

guilib/src/MainWindow.cpp

@@ -251,13 +251,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_preferencesDialog->loadWindowGeometry(_aboutDialog);
 	setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
 
-	_occupancyGrid = new OccupancyGrid(_preferencesDialog->getAllParameters());
+	ParametersMap parameters = _preferencesDialog->getAllParameters();
+	_occupancyGrid = new OccupancyGrid();
 #ifdef RTABMAP_OCTOMAP
-	_octomap = new OctoMap(
+	_octomap = new OctoMap(parameters);
-			_occupancyGrid->getCellSize(),
-			_preferencesDialog->getOctomapOccupancyThr(),
-			_occupancyGrid->isFullUpdate(),
-			_occupancyGrid->getUpdateError());
 #endif
 
 	// Timer
@@ -4733,11 +4730,7 @@ void MainWindow::startDetection()
 #ifdef RTABMAP_OCTOMAP
 	UASSERT(_octomap != 0);
 	delete _octomap;
-	_octomap = new OctoMap(
+	_octomap = new OctoMap(parameters);
-			_occupancyGrid->getCellSize(),
-			_preferencesDialog->getOctomapOccupancyThr(),
-			_occupancyGrid->isFullUpdate(),
-			_occupancyGrid->getUpdateError());
 #endif
 
 	// clear odometry visual stuff
@@ -6051,11 +6044,7 @@ void MainWindow::clearTheCache()
 	// re-create one if the resolution has changed
 	UASSERT(_octomap != 0);
 	delete _octomap;
-	_octomap = new OctoMap(
+	_octomap = new OctoMap(_preferencesDialog->getAllParameters());
-			_occupancyGrid->getCellSize(),
-			_preferencesDialog->getOctomapOccupancyThr(),
-			_occupancyGrid->isFullUpdate(),
-			_occupancyGrid->getUpdateError());
 #endif
 	_occupancyGrid->clear();
 }

guilib/src/PreferencesDialog.cpp

@@ -455,7 +455,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_octomap_show3dMap, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->comboBox_octomap_renderingType, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->spinBox_octomap_pointSize, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->doubleSpinBox_octomap_occupancyThr, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	connect(_ui->groupBox_organized, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_mesh_angleTolerance, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -912,9 +911,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->groupBox_grid_3d->setObjectName(Parameters::kGrid3D().c_str());
 	_ui->checkBox_grid_groundObstacle->setObjectName(Parameters::kGridGroundIsObstacle().c_str());
 	_ui->doubleSpinBox_grid_resolution->setObjectName(Parameters::kGridCellSize().c_str());
+	_ui->checkBox_grid_preVoxelFiltering->setObjectName(Parameters::kGridPreVoxelFiltering().c_str());
 	_ui->spinBox_grid_decimation->setObjectName(Parameters::kGridDepthDecimation().c_str());
-	_ui->doubleSpinBox_grid_maxDepth->setObjectName(Parameters::kGridDepthMax().c_str());
+	_ui->doubleSpinBox_grid_maxDepth->setObjectName(Parameters::kGridRangeMax().c_str());
-	_ui->doubleSpinBox_grid_minDepth->setObjectName(Parameters::kGridDepthMin().c_str());
+	_ui->doubleSpinBox_grid_minDepth->setObjectName(Parameters::kGridRangeMin().c_str());
 	_ui->lineEdit_grid_roi->setObjectName(Parameters::kGridDepthRoiRatios().c_str());
 	_ui->checkBox_grid_projRayTracing->setObjectName(Parameters::kGridRayTracing().c_str());
 	_ui->doubleSpinBox_grid_footprintLength->setObjectName(Parameters::kGridFootprintLength().c_str());
@@ -942,6 +942,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->doubleSpinBox_grid_minMapSize->setObjectName(Parameters::kGridGlobalMinSize().c_str());
 	_ui->spinBox_grid_maxNodes->setObjectName(Parameters::kGridGlobalMaxNodes().c_str());
 	_ui->doubleSpinBox_grid_footprintRadius->setObjectName(Parameters::kGridGlobalFootprintRadius().c_str());
+	_ui->doubleSpinBox_grid_octomapOccThr->setObjectName(Parameters::kGridGlobalOctoMapOccupancyThr().c_str());
 	_ui->checkBox_grid_erode->setObjectName(Parameters::kGridGlobalEroded().c_str());
 
 	//Odometry
@@ -1469,7 +1470,6 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkBox_octomap_show3dMap->setChecked(true);
 		_ui->comboBox_octomap_renderingType->setCurrentIndex(0);
 		_ui->spinBox_octomap_pointSize->setValue(5);
-		_ui->doubleSpinBox_octomap_occupancyThr->setValue(0.5);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
 	{
@@ -1857,7 +1857,6 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->checkBox_octomap_2dgrid->setChecked(settings.value("octomap_2dgrid", _ui->checkBox_octomap_2dgrid->isChecked()).toBool());
 	_ui->checkBox_octomap_show3dMap->setChecked(settings.value("octomap_3dmap", _ui->checkBox_octomap_show3dMap->isChecked()).toBool());
 	_ui->comboBox_octomap_renderingType->setCurrentIndex(settings.value("octomap_rendering_type", _ui->comboBox_octomap_renderingType->currentIndex()).toInt());
-	_ui->doubleSpinBox_octomap_occupancyThr->setValue(settings.value("octomap_occupancy_thr", _ui->doubleSpinBox_octomap_occupancyThr->value()).toDouble());
 	_ui->spinBox_octomap_pointSize->setValue(settings.value("octomap_point_size", _ui->spinBox_octomap_pointSize->value()).toInt());
 
 	_ui->groupBox_organized->setChecked(settings.value("meshing", _ui->groupBox_organized->isChecked()).toBool());
@@ -2261,7 +2260,6 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("octomap_2dgrid",              _ui->checkBox_octomap_2dgrid->isChecked());
 	settings.setValue("octomap_3dmap",               _ui->checkBox_octomap_show3dMap->isChecked());
 	settings.setValue("octomap_rendering_type",      _ui->comboBox_octomap_renderingType->currentIndex());
-	settings.setValue("octomap_occupancy_thr",       _ui->doubleSpinBox_octomap_occupancyThr->value());
 	settings.setValue("octomap_point_size",          _ui->spinBox_octomap_pointSize->value());
 
 
@@ -4322,10 +4320,6 @@ int PreferencesDialog::getOctomapTreeDepth() const
 {
 	return _ui->spinBox_octomap_treeDepth->value();
 }
-double PreferencesDialog::getOctomapOccupancyThr() const
-{
-	return _ui->doubleSpinBox_octomap_occupancyThr->value();
-}
 int PreferencesDialog::getOctomapPointSize() const
 {
 	return _ui->spinBox_octomap_pointSize->value();

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-284</y>
+            <y>0</y>
             <width>678</width>
-            <height>2778</height>
+            <height>2811</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>5</number>
+                <number>16</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -2042,32 +2042,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="1">
-                        <widget class="QLabel" name="label_octomap_treeDepth_4">
-                         <property name="text">
-                          <string>Occupancy threshold.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="5" column="0">
-                        <widget class="QDoubleSpinBox" name="doubleSpinBox_octomap_occupancyThr">
-                         <property name="maximum">
-                          <double>1.000000000000000</double>
-                         </property>
-                         <property name="singleStep">
-                          <double>0.050000000000000</double>
-                         </property>
-                         <property name="value">
-                          <double>0.500000000000000</double>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="4" column="1">
                         <widget class="QLabel" name="label_octomap_treeDepth">
                          <property name="text">
@@ -2120,6 +2094,16 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
+                       <item row="1" column="0">
+                        <widget class="QCheckBox" name="checkBox_octomap_2dgrid">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>false</bool>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="0" column="1">
                         <widget class="QLabel" name="label_octomap_treeDepth_3">
                          <property name="text">
@@ -2146,16 +2130,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="1" column="0">
-                        <widget class="QCheckBox" name="checkBox_octomap_2dgrid">
-                         <property name="text">
-                          <string/>
-                         </property>
-                         <property name="checked">
-                          <bool>false</bool>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="1" column="1">
                         <widget class="QLabel" name="label_octomap_treeDepth_8">
                          <property name="text">
@@ -9314,6 +9288,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="0" column="0">
+                       <widget class="QCheckBox" name="checkbox_rgbd_createOccupancyGrid">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>false</bool>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="8" column="1">
                        <widget class="QLabel" name="label_325">
                         <property name="text">
@@ -9346,8 +9330,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="0" column="0">
+                      <item row="1" column="0">
-                       <widget class="QCheckBox" name="checkbox_rgbd_createOccupancyGrid">
+                       <widget class="QCheckBox" name="checkBox_grid_projMapFrame">
                         <property name="text">
                          <string/>
                         </property>
@@ -9382,8 +9366,85 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="1" column="0">
+                      <item row="3" column="1">
-                       <widget class="QCheckBox" name="checkBox_grid_projMapFrame">
+                       <widget class="QLabel" name="label_323">
+                        <property name="text">
+                         <string>Minimum range from the sensor.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="3" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_minDepth">
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="decimals">
+                         <number>1</number>
+                        </property>
+                        <property name="maximum">
+                         <double>100.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.000000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="4" column="1">
+                       <widget class="QLabel" name="label_320">
+                        <property name="text">
+                         <string>Maximum range from the sensor (0 means no limit).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="4" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_maxDepth">
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="decimals">
+                         <number>1</number>
+                        </property>
+                        <property name="maximum">
+                         <double>100.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>4.000000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="14" column="1">
+                       <widget class="QLabel" name="label_456">
+                        <property name="text">
+                         <string>Input cloud is downsampled by voxel filter (voxel size is cell size) before doing segmentation of obstacles and ground.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="14" column="0">
+                       <widget class="QCheckBox" name="checkBox_grid_preVoxelFiltering">
                         <property name="text">
                          <string/>
                         </property>
@@ -9444,70 +9505,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="1" column="0">
-                          <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_minDepth">
-                           <property name="suffix">
-                            <string> m</string>
-                           </property>
-                           <property name="decimals">
-                            <number>1</number>
-                           </property>
-                           <property name="maximum">
-                            <double>100.000000000000000</double>
-                           </property>
-                           <property name="singleStep">
-                            <double>0.100000000000000</double>
-                           </property>
-                           <property name="value">
-                            <double>0.000000000000000</double>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="1" column="1">
-                          <widget class="QLabel" name="label_323">
-                           <property name="text">
-                            <string>Minimum depth.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="2" column="0">
-                          <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_maxDepth">
-                           <property name="suffix">
-                            <string> m</string>
-                           </property>
-                           <property name="decimals">
-                            <number>1</number>
-                           </property>
-                           <property name="maximum">
-                            <double>100.000000000000000</double>
-                           </property>
-                           <property name="singleStep">
-                            <double>0.100000000000000</double>
-                           </property>
-                           <property name="value">
-                            <double>4.000000000000000</double>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="2" column="1">
-                          <widget class="QLabel" name="label_320">
-                           <property name="text">
-                            <string>Maximum depth (0 means no limit).</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="3" column="0">
                           <widget class="QLineEdit" name="lineEdit_grid_roi">
                            <property name="readOnly">
@@ -9908,7 +9905,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="5" column="1">
+                      <item row="6" column="1">
                        <widget class="QLabel" name="label_224">
                         <property name="text">
                          <string>Erode obstacle cells.</string>
@@ -9934,7 +9931,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="5" column="0">
+                      <item row="6" column="0">
                        <widget class="QCheckBox" name="checkBox_grid_erode">
                         <property name="text">
                          <string/>
@@ -9944,6 +9941,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="3" column="0">
+                       <widget class="QSpinBox" name="spinBox_grid_maxNodes">
+                        <property name="maximum">
+                         <number>9999</number>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="4" column="0">
                        <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_footprintRadius">
                         <property name="suffix">
@@ -9976,13 +9980,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="3" column="0">
-                       <widget class="QSpinBox" name="spinBox_grid_maxNodes">
-                        <property name="maximum">
-                         <number>9999</number>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="2" column="0">
                        <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_minMapSize">
                         <property name="suffix">
@@ -10053,6 +10050,38 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="5" column="1">
+                       <widget class="QLabel" name="label_457">
+                        <property name="text">
+                         <string>OctoMap occupancy threshold.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="5" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_octomapOccThr">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.000000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                    </layout>