From 7aa9c929714517cec05eee5cb0503fe5438c7b97 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sat, 21 May 2016 15:54:37 -0400
Subject: [PATCH] Added util3d::pasthrough returning indices for convenience.
 segmentObstaclesFromGound(): filtering obstacles under maxGroundHeight if set

---
 .../rtabmap/core/impl/util3d_mapping.hpp      |  9 ++++-
 .../include/rtabmap/core/util3d_filtering.h   | 14 +++++++
 corelib/src/util3d_filtering.cpp              | 40 +++++++++++++++++++
 3 files changed, 62 insertions(+), 1 deletion(-)

diff --git a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
index a05e858e..4e5e07f8 100644
--- a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
+++ b/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,
diff --git a/corelib/include/rtabmap/core/util3d_filtering.h b/corelib/include/rtabmap/core/util3d_filtering.h
index 9426aec4..57392c6a 100644
--- a/corelib/include/rtabmap/core/util3d_filtering.h
+++ b/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,
diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp
index 9d71fb97..85abff37 100644
--- a/corelib/src/util3d_filtering.cpp
+++ b/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,