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

2026-09-03 01:50:24 +08:00 · 2018-02-13 10:16:48 -05:00
parent 09cae9cbd3
commit 4c0a612ab5
16 changed files with 926 additions and 1203 deletions
--- a/corelib/include/rtabmap/core/OccupancyGrid.h
+++ b/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_;
--- a/corelib/include/rtabmap/core/OctoMap.h
+++ b/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_;
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/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();
--- a/corelib/include/rtabmap/core/impl/OccupancyGrid.hpp
+++ b/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());
+}
+
 }


--- a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
+++ b/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));
 							}
--- a/corelib/include/rtabmap/core/util3d_filtering.h
+++ b/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,