Fix segfault, added tests for extractLabeledEuclideanClusters

segmentation/include/pcl/segmentation/extract_labeled_clusters.h

@@ -40,6 +40,12 @@
 
 namespace pcl
 {
+
+  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+  /** \brief Map of label and associated clusters
+  */
+  using labeled_cluster_map_t = std::map<std::uint32_t, std::vector<pcl::Indices>>;
+
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
     * \param[in] cloud the point cloud message
@@ -59,6 +65,25 @@ namespace pcl
       unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits<unsigned int>::max (),
       unsigned int max_label = std::numeric_limits<unsigned int>::max ());
 
+  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+  /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
+    * \param[in] cloud the point cloud message
+    * \param[in] tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching
+    * \note the tree has to be created as a spatial locator on \a cloud
+    * \param[in] tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
+    * \param[out] labeled_clusters a map of the resultant clusters, grouped by label
+    * \param[in] min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
+    * \param[in] max_pts_per_cluster maximum number of points that a cluster may contain (default: max uint)
+    * \param[in] max_label the maximum number of labels to extract (default: max uint)
+    * \ingroup segmentation
+    */
+  template <typename PointT> void 
+  extractLabeledEuclideanClusters (
+      const PointCloud<PointT> &cloud, const typename search::Search<PointT>::Ptr &tree,
+      float tolerance, labeled_cluster_map_t &labeled_clusters,
+      unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits<unsigned int>::max (),
+      unsigned int max_labels = std::numeric_limits<unsigned int>::max ());
+
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -148,6 +173,12 @@ namespace pcl
       void 
       extract (std::vector<std::vector<PointIndices> > &labeled_clusters);
 
+      /** \brief Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
+        * \param[out] labeled_clusters the resultant point clusters keyed by label
+        */
+      void 
+      extract ( labeled_cluster_map_t &labeled_clusters);
+
     protected:
       // Members derived from the base class
       using BasePCLBase::input_;

segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp

@@ -39,45 +39,99 @@
 
 #include <pcl/segmentation/extract_labeled_clusters.h>
 
+namespace impl
+{
+  // convert labeled_cluster_map_t
+  void
+  labeled_cluster_map_to_labeled_cluster_vector( const pcl::PCLHeader& header, pcl::labeled_cluster_map_t labeled_map, std::vector<std::vector<pcl::PointIndices>>& vec )
+  {
+    if ( labeled_map.empty() )
+      return;
+
+    // get max label
+    const auto extracted_max_label = std::max_element( labeled_map.begin(), labeled_map.end(), 
+      []( const pcl::labeled_cluster_map_t::value_type& lhs, const pcl::labeled_cluster_map_t::value_type& rhs  ) { return lhs.first < rhs.first; }  
+      )
+      ->first;
+
+    // if the user didn't provide a preallocated vector, create it for them based on max label
+    if ( vec.empty() )
+      vec.resize(extracted_max_label+1);
+    else if ( vec.size() <= extracted_max_label )
+    {
+      PCL_ERROR ("[pcl::extractLabeledEuclideanClusters] labeled_clusters size (%lu) less than extracted max_label (%lu)!\n", vec.size (), (unsigned int)extracted_max_label );
+      return;
+    }
+
+    // move clusters to output vector
+    //  convert pcl::Indices to pcl:PointIndices
+    for ( auto& label_group : labeled_map )
+    {
+      for ( auto& cluster : label_group.second )  // cluster is pcl::Indices
+      {
+        pcl::PointIndices pi = {};
+        pi.header = header;
+        pi.indices = std::move(cluster);
+        vec[label_group.first].emplace_back(std::move(pi));
+      }
+    }
+
+  }
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
 pcl::extractLabeledEuclideanClusters (const PointCloud<PointT> &cloud,
                                       const typename search::Search<PointT>::Ptr &tree,
                                       float tolerance,
-                                      std::vector<std::vector<PointIndices> > &labeled_clusters,
+                                      labeled_cluster_map_t &labeled_clusters,
                                       unsigned int min_pts_per_cluster,
                                       unsigned int max_pts_per_cluster,
-                                      unsigned int)
+                                      unsigned int max_labels )
 {
-  if (tree->getInputCloud ()->points.size () != cloud.points.size ())
+  assert(tree); // ensure we have a tree
+
+  if (tree->getInputCloud ()->size () != cloud.size ())
   {
-    PCL_ERROR ("[pcl::extractLabeledEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
+    PCL_ERROR ("[pcl::extractLabeledEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->size (), cloud.size ());
     return;
   }
-  // Create a bool vector of processed point indices, and initialize it to false
-  std::vector<bool> processed (cloud.points.size (), false);
 
-  std::vector<int> nn_indices;
-  std::vector<float> nn_distances;
+  // Create a bool vector of processed point indices, and initialize it to false
+  std::vector<bool> processed (cloud.size (), false);
 
-  // Process all points in the indices vector
-  for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i)
+  // Process all points in the cloud
+  for ( index_t i = 0; i < static_cast<index_t>(cloud.size()); ++i)
   {
     if (processed[i])
       continue;
 
-    std::vector<int> seed_queue;
-    int sq_idx = 0;
-    seed_queue.push_back (i);
-
     processed[i] = true;
 
-    while (sq_idx < static_cast<int> (seed_queue.size ()))
+    // label for the current point
+    const auto current_label = cloud[i].label;
+
+    // iterator in labeled_clusters for the current label
+    const auto current_label_cluster_it = labeled_clusters.find( current_label );
+
+    // max labels check
+    //  if we would have to add a new label and we're already at max, no need to proceed
+    if ( current_label_cluster_it == labeled_clusters.end() && labeled_clusters.size() >= max_labels )
+      continue;
+
+    pcl::Indices seed_queue = {i};
+
+    index_t sq_idx = 0;
+    while (sq_idx < static_cast<index_t> (seed_queue.size ()))
     {
       // Search for sq_idx
-      int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
+      pcl::Indices nn_indices = {};
+      std::vector<float> nn_distances = {};
+
+      const int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, max_pts_per_cluster );
       if(ret == -1)
         PCL_ERROR("radiusSearch on tree came back with error -1");
+
       if (!ret)
       {
         sq_idx++;
@@ -86,12 +140,10 @@ pcl::extractLabeledEuclideanClusters (const PointCloud<PointT> &cloud,
 
       for (std::size_t j = 1; j < nn_indices.size (); ++j)             // nn_indices[0] should be sq_idx
       {
-        if (processed[nn_indices[j]])                             // Has this point been processed before ?
-          continue;
-        if (cloud.points[i].label == cloud.points[nn_indices[j]].label)
+        // labels match, and nn point not processed before
+        if ( !processed[nn_indices[j]] && current_label == cloud[nn_indices[j]].label )
         {
-          // Perform a simple Euclidean clustering
-          seed_queue.push_back (nn_indices[j]);
+          seed_queue.push_back ( nn_indices[j] ) ;
           processed[nn_indices[j]] = true;
         }
       }
@@ -101,26 +153,54 @@ pcl::extractLabeledEuclideanClusters (const PointCloud<PointT> &cloud,
 
     // If this queue is satisfactory, add to the clusters
     if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
-    {
-      pcl::PointIndices r;
-      r.indices.resize (seed_queue.size ());
-      for (std::size_t j = 0; j < seed_queue.size (); ++j)
-        r.indices[j] = seed_queue[j];
-
-      std::sort (r.indices.begin (), r.indices.end ());
-      r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
+    { 
+      if ( current_label_cluster_it == labeled_clusters.end() )  // label not found in map, add
+      {
+        std::vector<pcl::Indices> v = {};
+        v.emplace_back(std::move(seed_queue));
+        labeled_clusters.emplace(current_label, std::move(v));
+      }
+      else  // label already exists in map; append seed_queue to label vector
+        current_label_cluster_it->second.emplace_back(std::move(seed_queue));
 
-      r.header = cloud.header;
-      labeled_clusters[cloud.points[i].label].push_back (r);   // We could avoid a copy by working directly in the vector
     }
   }
 }
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::extractLabeledEuclideanClusters (const PointCloud<PointT> &cloud,
+                                      const typename search::Search<PointT>::Ptr &tree,
+                                      float tolerance,
+                                      std::vector<std::vector<PointIndices> > &labeled_clusters,
+                                      unsigned int min_pts_per_cluster,
+                                      unsigned int max_pts_per_cluster,
+                                      unsigned int max_label )
+{
+  labeled_cluster_map_t label_map = {};
+  extractLabeledEuclideanClusters( cloud, tree, tolerance, label_map, min_pts_per_cluster, max_pts_per_cluster, max_label );
+
+  impl::labeled_cluster_map_to_labeled_cluster_vector( cloud.header, std::move(label_map), labeled_clusters );
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////////////////
 
 template <typename PointT> void 
 pcl::LabeledEuclideanClusterExtraction<PointT>::extract (std::vector<std::vector<PointIndices> > &labeled_clusters)
+{
+  labeled_cluster_map_t label_map = {};
+  extract( label_map );
+  impl::labeled_cluster_map_to_labeled_cluster_vector( input_->header, std::move(label_map), labeled_clusters );
+
+  // Sort the clusters based on their size (largest one first)
+  for (auto &labeled_cluster : labeled_clusters)
+    std::sort (labeled_cluster.rbegin (), labeled_cluster.rend (), comparePointClusters);
+}
+
+template <typename PointT> void 
+pcl::LabeledEuclideanClusterExtraction<PointT>::extract ( pcl::labeled_cluster_map_t &labeled_clusters)
 {
   if (!initCompute () || 
       (input_   && input_->points.empty ()) ||
@@ -143,14 +223,11 @@ pcl::LabeledEuclideanClusterExtraction<PointT>::extract (std::vector<std::vector
   tree_->setInputCloud (input_);
   extractLabeledEuclideanClusters (*input_, tree_, static_cast<float> (cluster_tolerance_), labeled_clusters, min_pts_per_cluster_, max_pts_per_cluster_, max_label_);
 
-  // Sort the clusters based on their size (largest one first)
-  for (auto &labeled_cluster : labeled_clusters)
-    std::sort (labeled_cluster.rbegin (), labeled_cluster.rend (), comparePointClusters);
-
   deinitCompute ();
 }
 
 #define PCL_INSTANTIATE_LabeledEuclideanClusterExtraction(T) template class PCL_EXPORTS pcl::LabeledEuclideanClusterExtraction<T>;
 #define PCL_INSTANTIATE_extractLabeledEuclideanClusters(T) template void PCL_EXPORTS pcl::extractLabeledEuclideanClusters<T>(const pcl::PointCloud<T> &, const typename pcl::search::Search<T>::Ptr &, float , std::vector<std::vector<pcl::PointIndices> > &, unsigned int, unsigned int, unsigned int);
+#define PCL_INSTANTIATE_extractLabeledEuclideanClustersMap(T) template void PCL_EXPORTS pcl::extractLabeledEuclideanClusters<T>(const pcl::PointCloud<T> &, const typename pcl::search::Search<T>::Ptr &, float , pcl::labeled_cluster_map_t &, unsigned int, unsigned int, unsigned int);
 
 #endif        // PCL_EXTRACT_CLUSTERS_IMPL_H_

test/segmentation/test_segmentation.cpp

@@ -49,6 +49,7 @@
 #include <pcl/segmentation/region_growing.h>
 #include <pcl/segmentation/region_growing_rgb.h>
 #include <pcl/segmentation/min_cut_segmentation.h>
+#include <pcl/segmentation/extract_labeled_clusters.h>
 
 using namespace pcl;
 using namespace pcl::io;
@@ -62,6 +63,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr another_cloud_;
 pcl::PointCloud<pcl::Normal>::Ptr normals_;
 pcl::PointCloud<pcl::Normal>::Ptr another_normals_;
 
+pcl::PointCloud<pcl::PointXYZL>::Ptr labeled_cloud_;
+static const std::size_t LABELED_CLOUD_NUM_LABELS_ = 5;
+static const std::size_t LABELED_CLOUD_NUM_PTS_PER_CLUSTER_ = 5;
+
 ////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (RegionGrowingRGBTest, Segment)
 {
@@ -385,6 +390,79 @@ TEST (ExtractPolygonalPrism, Segmentation)
   EXPECT_EQ (static_cast<int> (output.indices.size ()), 0);
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////
+TEST (ExtractLabeledEuclideanClusters, SegmentEmptyCloud)
+{
+  pcl::PointCloud<pcl::PointXYZL>::Ptr empty_cloud (new pcl::PointCloud<pcl::PointXYZL>);
+
+  pcl::LabeledEuclideanClusterExtraction<pcl::PointXYZL> ec;
+  ec.setInputCloud (empty_cloud);
+
+  std::vector < std::vector< pcl::PointIndices > > labeled_clusters;
+  ec.extract (labeled_clusters);
+  int num_of_labels = static_cast<int> (labeled_clusters.size ());
+  EXPECT_EQ (0, num_of_labels);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+TEST (ExtractLabeledEuclideanClusters, SegmentFromPointsVec)
+{
+  pcl::LabeledEuclideanClusterExtraction<pcl::PointXYZL> ec;
+
+  ec.setInputCloud (labeled_cloud_);
+  ec.setClusterTolerance( std::numeric_limits<double>::max() ); // no distance limit
+
+  std::vector < std::vector< pcl::PointIndices > > labeled_clusters;
+  ec.extract (labeled_clusters);
+
+  const auto num_of_labels = labeled_clusters.size ();
+  EXPECT_EQ (LABELED_CLOUD_NUM_LABELS_, num_of_labels);
+
+  for ( std::size_t i = 0; i < labeled_clusters.size(); ++i ) 
+  {
+    const auto& label = labeled_clusters[i];
+
+    EXPECT_EQ( 1, label.size() );          // 1 cluster per label
+
+    const auto& cluster = label.front();
+    EXPECT_EQ( LABELED_CLOUD_NUM_PTS_PER_CLUSTER_, cluster.indices.size() );  // n points per cluster
+
+    for ( const auto idx : cluster.indices ) // check all labels in the cluster
+      EXPECT_EQ( i, (*labeled_cloud_)[idx].label ); // get point from cloud, check its label
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+TEST (ExtractLabeledEuclideanClusters, SegmentFromPointsMap)
+{
+  pcl::LabeledEuclideanClusterExtraction<pcl::PointXYZL> ec;
+
+  ec.setInputCloud (labeled_cloud_);
+  ec.setClusterTolerance( std::numeric_limits<double>::max() ); // no distance limit
+
+  pcl::labeled_cluster_map_t labeled_clusters = {};
+  ec.extract (labeled_clusters);
+
+  const auto num_of_labels = labeled_clusters.size ();
+  EXPECT_EQ (LABELED_CLOUD_NUM_LABELS_, num_of_labels);
+
+  for ( const auto& label : labeled_clusters )
+  {
+    const auto& clusters = label.second;
+    EXPECT_EQ( 1, clusters.size() );          // 1 cluster per label
+
+    for ( const auto& cluster : clusters )
+    {
+      EXPECT_EQ( LABELED_CLOUD_NUM_PTS_PER_CLUSTER_, cluster.size() );  // n points per cluster
+
+      for ( const auto idx : cluster ) // check all labels in the cluster
+        EXPECT_EQ( label.first, (*labeled_cloud_)[idx].label ); // get point from cloud, check its label
+    }
+
+  }
+}
+
+
 /* ---[ */
 int
 main (int argc, char** argv)
@@ -438,6 +516,20 @@ main (int argc, char** argv)
   normal_estimator.setKSearch(30);
   normal_estimator.compute(*another_normals_);
 
+  // load labeled cloud
+  labeled_cloud_ = (new pcl::PointCloud<pcl::PointXYZL>)->makeShared();
+
+  // create LABELED_CLOUD_NUM_LABELS_ labels, each with 1 cluster of LABELED_CLOUD_NUM_PTS_PER_CLUSTER_ points
+  for (std::size_t i = 0; i < LABELED_CLOUD_NUM_LABELS_; ++i)
+  {
+    for (std::size_t j = 0; j < LABELED_CLOUD_NUM_PTS_PER_CLUSTER_; ++j) {
+      auto pt = pcl::PointXYZL();
+      pt.x = pt.y = pt.z = static_cast<float>( j );
+      pt.label = static_cast<std::uint32_t> (i);
+      labeled_cloud_->points.emplace_back(std::move(pt));
+    }
+  }
+
   testing::InitGoogleTest (&argc, argv);
   return (RUN_ALL_TESTS ());
 }