Added some empty cloud verifications

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

@@ -152,6 +152,8 @@ void segmentObstaclesFromGround(
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
 
+	if(cloud->size())
+	{
 		// Find the ground
 		pcl::IndicesPtr flatSurfaces = util3d::normalFiltering<PointT>(
 				cloud,
@@ -211,6 +213,7 @@ void segmentObstaclesFromGround(
 			obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
 		}
 	}
+}
 
 template<typename PointT>
 void projectCloudOnXYPlane(
@@ -301,6 +304,10 @@ pcl::IndicesPtr normalFiltering(
 		const Eigen::Vector4f & normal,
 		float radiusSearch,
 		const Eigen::Vector4f & viewpoint)
+{
+	pcl::IndicesPtr output(new std::vector<int>());
+
+	if(cloud->size())
 	{
 		typedef typename pcl::search::KdTree<PointT> KdTree;
 		typedef typename KdTree::Ptr KdTreePtr;
@@ -334,7 +341,7 @@ pcl::IndicesPtr normalFiltering(
 
 		ne.compute (*cloud_normals);
 
-	pcl::IndicesPtr output(new std::vector<int>(cloud_normals->size()));
+		output->resize(cloud_normals->size());
 		int oi = 0; // output iterator
 		Eigen::Vector3f n(normal[0], normal[1], normal[2]);
 		for(unsigned int i=0; i<cloud_normals->size(); ++i)
@@ -347,6 +354,7 @@ pcl::IndicesPtr normalFiltering(
 			}
 		}
 		output->resize(oi);
+	}
 
 	return output;
 }