From abd376a44ccc6d05325e3dac2f8234e431ef61a8 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Tue, 14 Jun 2016 11:55:58 -0400
Subject: [PATCH] segmentObstaclesFromGround() fixed identical ground and
 obstacles indices

---
 .../rtabmap/core/impl/util3d_mapping.hpp        | 17 ++++++++++-------
 1 file changed, 10 insertions(+), 7 deletions(-)

diff --git a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
index 4e5e07f8..837e6924 100644
--- a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
+++ b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
@@ -116,14 +116,17 @@ void segmentObstaclesFromGround(
 			}
 
 			//Cluster remaining stuff (obstacles)
-			std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
-					cloud,
-					otherStuffIndices,
-					clusterRadius,
-					minClusterSize);
+			if(otherStuffIndices->size())
+			{
+				std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
+						cloud,
+						otherStuffIndices,
+						clusterRadius,
+						minClusterSize);
 
-			// merge indices
-			obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+				// merge indices
+				obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+			}
 		}
 	}
 }