Added util3d::pasthrough returning indices for convenience. segmentObstaclesFromGound(): filtering obstacles under maxGroundHeight if set

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

@@ -76,7 +76,8 @@ void segmentObstaclesFromGround(
 						{
 							Eigen::Vector4f centroid;
 							pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
-							if(centroid[2] >= min[2]-0.01 && centroid[2] <= max[2]+0.01) // epsilon
+							if(centroid[2] >= min[2]-0.01 &&
+							  (centroid[2] <= max[2]+0.01 || (maxGroundHeight>0 && centroid[2] <= maxGroundHeight+0.01))) // epsilon
 							{
 								ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
 							}
@@ -108,6 +109,12 @@ void segmentObstaclesFromGround(
 			// Remove ground
 			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, ground, true);
 
+			// If ground height is set, remove obstacles under it
+			if(maxGroundHeight > 0.0f)
+			{
+				otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits<float>::max());
+			}
+
 			//Cluster remaining stuff (obstacles)
 			std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
 					cloud,

corelib/include/rtabmap/core/util3d_filtering.h

@@ -108,6 +108,20 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP randomSampling(
 		int samples);
 
 
+pcl::IndicesPtr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZ>::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::PointXYZRGB>::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,

corelib/src/util3d_filtering.cpp

@@ -244,6 +244,46 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr randomSampling(
 	return output;
 }
 
+pcl::IndicesPtr passThrough(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative)
+{
+	UASSERT(max > min);
+	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
+
+	pcl::IndicesPtr output(new std::vector<int>);
+	pcl::PassThrough<pcl::PointXYZ> filter;
+	filter.setNegative(negative);
+	filter.setFilterFieldName(axis);
+	filter.setFilterLimits(min, max);
+	filter.setInputCloud(cloud);
+	filter.filter(*output);
+	return output;
+}
+pcl::IndicesPtr passThrough(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative)
+{
+	UASSERT(max > min);
+	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
+
+	pcl::IndicesPtr output(new std::vector<int>);
+	pcl::PassThrough<pcl::PointXYZRGB> filter;
+	filter.setNegative(negative);
+	filter.setFilterFieldName(axis);
+	filter.setFilterLimits(min, max);
+	filter.setInputCloud(cloud);
+	filter.filter(*output);
+	return output;
+}
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,