From 4cc1c66f09516a6d5712969640959c24bd665dc0 Mon Sep 17 00:00:00 2001
From: Mathieu Labbe <matlabbe@gmail.com>
Date: Fri, 15 Apr 2016 17:41:40 -0400
Subject: [PATCH] Segment ground/obstacles: added max ground height parameter

---
 .../rtabmap/core/impl/util3d_mapping.hpp      | 54 +++++++++++++------
 corelib/include/rtabmap/core/util3d_mapping.h | 14 +++--
 2 files changed, 47 insertions(+), 21 deletions(-)

diff --git a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
index 529d79a5..6a332d77 100644
--- a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
+++ b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
@@ -27,7 +27,8 @@ void segmentObstaclesFromGround(
 		float groundNormalAngle,
 		float clusterRadius,
 		int minClusterSize,
-		bool segmentFlatObstacles)
+		bool segmentFlatObstacles,
+		float maxGroundHeight)
 {
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
@@ -56,21 +57,32 @@ void segmentObstaclesFromGround(
 
 
 			// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
-			ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
-			Eigen::Vector4f min,max;
-			pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
-
-			for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
+			if(clusteredFlatSurfaces.size())
 			{
-				if((int)i!=biggestFlatSurfaceIndex)
+				ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
+				Eigen::Vector4f min,max;
+				pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
+
+				if(maxGroundHeight <= 0 || min[2] < maxGroundHeight)
 				{
-					Eigen::Vector4f centroid;
-					pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
-					if(centroid[2] >= min[2] && centroid[2] <= max[2])
+					for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
 					{
-						ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
+						if((int)i!=biggestFlatSurfaceIndex)
+						{
+							Eigen::Vector4f centroid;
+							pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
+							if(centroid[2] >= min[2] && centroid[2] <= max[2])
+							{
+								ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
+							}
+						}
 					}
 				}
+				else
+				{
+					// reject ground!
+					ground.reset(new std::vector<int>);
+				}
 			}
 		}
 		else
@@ -105,7 +117,8 @@ void segmentObstaclesFromGround(
 		float groundNormalAngle,
 		float clusterRadius,
 		int minClusterSize,
-		bool segmentFlatObstacles)
+		bool segmentFlatObstacles,
+		float maxGroundHeight)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -117,7 +130,8 @@ void segmentObstaclesFromGround(
 			groundNormalAngle,
 			clusterRadius,
 			minClusterSize,
-			segmentFlatObstacles);
+			segmentFlatObstacles,
+			maxGroundHeight);
 }
 
 template<typename PointT>
@@ -128,7 +142,9 @@ void occupancy2DFromCloud3D(
 		cv::Mat & obstacles,
 		float cellSize,
 		float groundNormalAngle,
-		int minClusterSize)
+		int minClusterSize,
+		bool segmentFlatObstacles,
+		float maxGroundHeight)
 {
 	if(cloud->size() == 0)
 	{
@@ -144,7 +160,9 @@ void occupancy2DFromCloud3D(
 			20,
 			groundNormalAngle,
 			cellSize*2.0f,
-			minClusterSize);
+			minClusterSize,
+			segmentFlatObstacles,
+			maxGroundHeight);
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -197,10 +215,12 @@ void occupancy2DFromCloud3D(
 		cv::Mat & obstacles,
 		float cellSize,
 		float groundNormalAngle,
-		int minClusterSize)
+		int minClusterSize,
+		bool segmentFlatObstacles,
+		float maxGroundHeight)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
-	occupancy2DFromCloud3D<PointT>(cloud, indices, ground, obstacles, cellSize, groundNormalAngle, minClusterSize);
+	occupancy2DFromCloud3D<PointT>(cloud, indices, ground, obstacles, cellSize, groundNormalAngle, minClusterSize, segmentFlatObstacles, maxGroundHeight);
 }
 
 }
diff --git a/corelib/include/rtabmap/core/util3d_mapping.h b/corelib/include/rtabmap/core/util3d_mapping.h
index 5dbef1ca..53676d65 100644
--- a/corelib/include/rtabmap/core/util3d_mapping.h
+++ b/corelib/include/rtabmap/core/util3d_mapping.h
@@ -90,7 +90,8 @@ void segmentObstaclesFromGround(
 		float groundNormalAngle,
 		float clusterRadius,
 		int minClusterSize,
-		bool segmentFlatObstacles = false);
+		bool segmentFlatObstacles = false,
+		float maxGroundHeight = 0.0f);
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -100,7 +101,8 @@ void segmentObstaclesFromGround(
 		float groundNormalAngle,
 		float clusterRadius,
 		int minClusterSize,
-		bool segmentFlatObstacles = false);
+		bool segmentFlatObstacles = false,
+		float maxGroundHeight = 0.0f);
 
 template<typename PointT>
 void occupancy2DFromCloud3D(
@@ -109,7 +111,9 @@ void occupancy2DFromCloud3D(
 		cv::Mat & obstacles,
 		float cellSize = 0.05f,
 		float groundNormalAngle = M_PI_4,
-		int minClusterSize = 20);
+		int minClusterSize = 20,
+		bool segmentFlatObstacles = false,
+		float maxGroundHeight = 0.0f);
 template<typename PointT>
 void occupancy2DFromCloud3D(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -118,7 +122,9 @@ void occupancy2DFromCloud3D(
 		cv::Mat & obstacles,
 		float cellSize = 0.05f,
 		float groundNormalAngle = M_PI_4,
-		int minClusterSize = 20);
+		int minClusterSize = 20,
+		bool segmentFlatObstacles = false,
+		float maxGroundHeight = 0.0f);
 
 } // namespace util3d
 } // namespace rtabmap