From 47547865cfce66f75238b6067a5af91e0c033bd8 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Fri, 20 May 2016 18:48:27 -0400
Subject: [PATCH 1/2] obstacles_detection: removed flat obstacles from
 proj_obstacles output

---
 src/nodelets/obstacles_detection.cpp | 52 ++++++++++++++++++++--------
 1 file changed, 37 insertions(+), 15 deletions(-)

diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp
index 66824757..c6e1424d 100644
--- a/src/nodelets/obstacles_detection.cpp
+++ b/src/nodelets/obstacles_detection.cpp
@@ -160,6 +160,7 @@ private:
 		pcl::IndicesPtr ground, obstacles;
 		pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud<pcl::PointXYZ>);
 
 		if(originalCloud->size())
 		{
@@ -174,6 +175,7 @@ private:
 				if(!optimizeForCloseObjects_)
 				{
 					// This is the default strategy
+					pcl::IndicesPtr flatObstacles(new std::vector<int>);
 					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
 							originalCloud,
 							ground,
@@ -183,16 +185,46 @@ private:
 							clusterRadius_,
 							minClusterSize_,
 							segmentFlatObstacles_,
-							maxGroundHeight_);
+							maxGroundHeight_,
+							&flatObstacles);
 
-					if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
+					if(groundPub_.getNumSubscribers() &&
+							ground.get() && ground->size())
 					{
 						pcl::copyPointCloud(*originalCloud, *ground, *groundCloud);
 					}
 
-					if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size())
+					if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) &&
+							obstacles.get() && obstacles->size())
 					{
-						pcl::copyPointCloud(*originalCloud, *obstacles, *obstaclesCloud);
+						// remove flat obstacles from obstacles
+						std::set<int> flatObstaclesSet;
+						if(projObstaclesPub_.getNumSubscribers())
+						{
+							std::set<int> flatObstaclesSet(flatObstacles->begin(), flatObstacles->end());
+						}
+
+						obstaclesCloud->resize(obstacles->size());
+						obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size());
+
+						int oi=0;
+						for(unsigned int i=0; i<obstacles->size(); ++i)
+						{
+							obstaclesCloud->points[i] = originalCloud->at(obstacles->at(i));
+							if(flatObstaclesSet.size() &&
+							   flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
+							{
+								obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
+								obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0;
+								++oi;
+							}
+
+						}
+						obstaclesCloudWithoutFlatSurfaces->resize(oi);
+						if(obstaclesCloudWithoutFlatSurfaces->size() && projVoxelSize_ > 0.0)
+						{
+							obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::voxelize(obstaclesCloudWithoutFlatSurfaces, projVoxelSize_);
+						}
 					}
 				}
 				else
@@ -286,18 +318,8 @@ private:
 
 		if(projObstaclesPub_.getNumSubscribers())
 		{
-			for(unsigned int i=0; i<obstaclesCloud->size(); ++i)
-			{
-				obstaclesCloud->at(i).z = 0;
-			}
-
-			if(obstaclesCloud->size() && projVoxelSize_ > 0.0)
-			{
-				obstaclesCloud = rtabmap::util3d::voxelize(obstaclesCloud, projVoxelSize_);
-			}
-
 			sensor_msgs::PointCloud2 rosCloud;
-			pcl::toROSMsg(*obstaclesCloud, rosCloud);
+			pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, rosCloud);
 			rosCloud.header.stamp = cloudMsg->header.stamp;
 			rosCloud.header.frame_id = frameId_;
 

From 486ecbbf62c49af4b6c610067525a166b57b4c5f Mon Sep 17 00:00:00 2001
From: Mathieu Labbe <matlabbe@gmail.com>
Date: Fri, 20 May 2016 19:14:21 -0400
Subject: [PATCH 2/2] obstacles_detection: Fixed proj_obstacles without flat
 obstacles

---
 src/nodelets/obstacles_detection.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/nodelets/obstacles_detection.cpp b/src/nodelets/obstacles_detection.cpp
index c6e1424d..708a9ad7 100644
--- a/src/nodelets/obstacles_detection.cpp
+++ b/src/nodelets/obstacles_detection.cpp
@@ -201,7 +201,7 @@ private:
 						std::set<int> flatObstaclesSet;
 						if(projObstaclesPub_.getNumSubscribers())
 						{
-							std::set<int> flatObstaclesSet(flatObstacles->begin(), flatObstacles->end());
+							flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end());
 						}
 
 						obstaclesCloud->resize(obstacles->size());
@@ -211,7 +211,7 @@ private:
 						for(unsigned int i=0; i<obstacles->size(); ++i)
 						{
 							obstaclesCloud->points[i] = originalCloud->at(obstacles->at(i));
-							if(flatObstaclesSet.size() &&
+							if(flatObstaclesSet.size() == 0 ||  
 							   flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
 							{
 								obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
@@ -220,6 +220,7 @@ private:
 							}
 
 						}
+
 						obstaclesCloudWithoutFlatSurfaces->resize(oi);
 						if(obstaclesCloudWithoutFlatSurfaces->size() && projVoxelSize_ > 0.0)
 						{