From d54bb58f68b38a07868a2f9150fe377119e878a3 Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Mon, 13 Jul 2020 07:42:47 +0530
Subject: [PATCH 01/19] Modularize extractEuclideanClusters

---
 .../pcl/segmentation/extract_clusters.h       | 152 ++++--------------
 .../segmentation/impl/extract_clusters.hpp    | 144 +++++------------
 2 files changed, 73 insertions(+), 223 deletions(-)

diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index 190f2fd99a8..53454dc9cd3 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -45,6 +45,13 @@
 
 namespace pcl
 {
+  template <typename PointT, typename FunctorT> void
+  extractEuclideanClusters (
+      const PointCloud<PointT> &cloud, const std::vector<int> &indices,
+      FunctorT filter, const typename search::Search<PointT>::Ptr &tree,
+      float tolerance, std::vector<PointIndices> &clusters,
+      unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
+
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
     * \param cloud the point cloud message
@@ -56,7 +63,7 @@ namespace pcl
     * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
     * \ingroup segmentation
     */
-  template <typename PointT> void 
+  template <typename PointT> void
   extractEuclideanClusters (
       const PointCloud<PointT> &cloud, const typename search::Search<PointT>::Ptr &tree,
       float tolerance, std::vector<PointIndices> &clusters,
@@ -112,69 +119,18 @@ namespace pcl
       PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.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;
-    // Process all points in the indices vector
-    for (std::size_t i = 0; i < cloud.points.size (); ++i)
-    {
-      if (processed[i])
-        continue;
-
-      std::vector<unsigned int> seed_queue;
-      int sq_idx = 0;
-      seed_queue.push_back (static_cast<int> (i));
-
-      processed[i] = true;
-
-      while (sq_idx < static_cast<int> (seed_queue.size ()))
-      {
-        // Search for sq_idx
-        if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
-        {
-          sq_idx++;
-          continue;
-        }
-
-        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;
-
-          //processed[nn_indices[j]] = true;
-          // [-1;1]
-          double dot_p = normals[i].normal[0] * normals[nn_indices[j]].normal[0] +
-                         normals[i].normal[1] * normals[nn_indices[j]].normal[1] +
-                         normals[i].normal[2] * normals[nn_indices[j]].normal[2];
-          if ( std::acos (std::abs (dot_p)) < eps_angle )
-          {
-            processed[nn_indices[j]] = true;
-            seed_queue.push_back (nn_indices[j]);
-          }
-        }
-
-        sq_idx++;
-      }
-
-      // 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];
-
-        // These two lines should not be needed: (can anyone confirm?) -FF
-        std::sort (r.indices.begin (), r.indices.end ());
-        r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
-
-        r.header = cloud.header;
-        clusters.push_back (r);   // We could avoid a copy by working directly in the vector
-      }
-    }
+    auto lambda = [&](int &i, int &j, std::vector<int>& nn_indices) -> bool {
+      //processed[nn_indices[j]] = true;
+      // [-1;1]
+      double dot_p = normals.points[i].normal[0] * normals.points[nn_indices[j]].normal[0] +
+                     normals.points[i].normal[1] * normals.points[nn_indices[j]].normal[1] +
+                     normals.points[i].normal[2] * normals.points[nn_indices[j]].normal[2];
+      return std::acos (std::abs (dot_p)) < eps_angle;
+    };
+
+    std::vector<int> indices{-1};
+
+    pcl::extractEuclideanClusters(cloud, indices, lambda, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
   }
 
 
@@ -221,66 +177,18 @@ namespace pcl
     // 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;
-    // Process all points in the indices vector
-    for (std::size_t i = 0; i < indices.size (); ++i)
-    {
-      if (processed[indices[i]])
-        continue;
-
-      std::vector<int> seed_queue;
-      int sq_idx = 0;
-      seed_queue.push_back (indices[i]);
-
-      processed[indices[i]] = true;
-
-      while (sq_idx < static_cast<int> (seed_queue.size ()))
-      {
-        // Search for sq_idx
-        if (!tree->radiusSearch (cloud[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances))
-        {
-          sq_idx++;
-          continue;
-        }
-
-        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;
-
-          //processed[nn_indices[j]] = true;
-          // [-1;1]
-          double dot_p =
-            normals[indices[i]].normal[0] * normals[indices[nn_indices[j]]].normal[0] +
-            normals[indices[i]].normal[1] * normals[indices[nn_indices[j]]].normal[1] +
-            normals[indices[i]].normal[2] * normals[indices[nn_indices[j]]].normal[2];
-          if ( std::acos (std::abs (dot_p)) < eps_angle )
-          {
-            processed[nn_indices[j]] = true;
-            seed_queue.push_back (nn_indices[j]);
-          }
-        }
-
-        sq_idx++;
-      }
-
-      // 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];
+    auto lambda = [&](int &i, int &j, std::vector<int>& nn_indices) -> bool {
+      //processed[nn_indices[j]] = true;
+      // [-1;1]
+      double dot_p =
+          normals.points[indices[i]].normal[0] * normals.points[indices[nn_indices[j]]].normal[0] +
+          normals.points[indices[i]].normal[1] * normals.points[indices[nn_indices[j]]].normal[1] +
+          normals.points[indices[i]].normal[2] * normals.points[indices[nn_indices[j]]].normal[2];
+      return std::acos (std::abs (dot_p)) < eps_angle;
+    };
 
-        // These two lines should not be needed: (can anyone confirm?) -FF
-        std::sort (r.indices.begin (), r.indices.end ());
-        r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
+    pcl::extractEuclideanClusters(cloud, indices, lambda, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
 
-        r.header = cloud.header;
-        clusters.push_back (r);
-      }
-    }
   }
 
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
index 929fd0d7312..3c77b8de2b6 100644
--- a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
+++ b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
@@ -40,14 +40,15 @@
 
 #include <pcl/segmentation/extract_clusters.h>
 
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointT> void
-pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
-                               const typename search::Search<PointT>::Ptr &tree,
-                               float tolerance, std::vector<PointIndices> &clusters,
-                               unsigned int min_pts_per_cluster,
-                               unsigned int max_pts_per_cluster)
-{
+template <typename PointT, typename FunctorT> void
+pcl::extractEuclideanClusters (
+    const PointCloud<PointT> &cloud,
+    const std::vector<int> &indices,
+    FunctorT filter,
+    const typename search::Search<PointT>::Ptr &tree,
+    float tolerance, std::vector<PointIndices> &clusters,
+    unsigned int min_pts_per_cluster,
+    unsigned int max_pts_per_cluster) {
   if (tree->getInputCloud ()->points.size () != cloud.points.size ())
   {
     PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
@@ -61,21 +62,21 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
   std::vector<int> nn_indices;
   std::vector<float> nn_distances;
   // Process all points in the indices vector
-  for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i)
-  {
+  auto lambda = [&](int &i) {
     if (processed[i])
-      continue;
+      return;
 
-    std::vector<int> seed_queue;
     int sq_idx = 0;
-    seed_queue.push_back (i);
+    clusters.emplace_back();
+    auto seed_queue = clusters.back();
+    seed_queue.indices.push_back (i);
 
     processed[i] = true;
 
-    while (sq_idx < static_cast<int> (seed_queue.size ()))
+    while (sq_idx < static_cast<int> (seed_queue.indices.size()))
     {
       // Search for sq_idx
-      if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
+      if (!tree->radiusSearch (seed_queue.indices[sq_idx], tolerance, nn_indices, nn_distances))
       {
         sq_idx++;
         continue;
@@ -86,30 +87,39 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
         if (nn_indices[j] == -1 || processed[nn_indices[j]])        // Has this point been processed before ?
           continue;
 
-        // Perform a simple Euclidean clustering
-        seed_queue.push_back (nn_indices[j]);
-        processed[nn_indices[j]] = true;
+        if (filter(i, j, nn_indices)) {
+          seed_queue.indices.push_back(nn_indices[j]);
+          processed[nn_indices[j]] = true;
+        }
       }
 
       sq_idx++;
     }
 
     // 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];
+    if (seed_queue.indices.size () >= min_pts_per_cluster && seed_queue.indices.size () <= max_pts_per_cluster)
+      seed_queue.header = cloud.header;
+    else
+      clusters.pop_back();
+  };
 
-      // These two lines should not be needed: (can anyone confirm?) -FF
-      std::sort (r.indices.begin (), r.indices.end ());
-      r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
+  if (indices.size() == 1 && indices.front() == - 1)
+    for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i) lambda(i);
+  else
+    for (const int &index : indices) lambda(index);
+}
 
-      r.header = cloud.header;
-      clusters.push_back (r);   // We could avoid a copy by working directly in the vector
-    }
-  }
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
+                               const typename search::Search<PointT>::Ptr &tree,
+                               float tolerance, std::vector<PointIndices> &clusters,
+                               unsigned int min_pts_per_cluster,
+                               unsigned int max_pts_per_cluster)
+{
+  const auto noop = []{};
+  std::vector<int> indices{-1};
+  pcl::extractEuclideanClusters<PointT, decltype(noop)>(cloud, indices, noop, tree, clusters, min_pts_per_cluster, max_pts_per_cluster);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -124,82 +134,14 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
 {
   // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
   //and indices[i]
-  if (tree->getInputCloud ()->points.size () != cloud.points.size ())
-  {
-    PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
-    return;
-  }
   if (tree->getIndices ()->size () != indices.size ())
   {
     PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ());
     return;
   }
-  // Check if the tree is sorted -- if it is we don't need to check the first element
-  int nn_start_idx = tree->getSortedResults () ? 1 : 0;
-
-  // 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;
-  // Process all points in the indices vector
-  for (const int &index : indices)
-  {
-    if (processed[index])
-      continue;
-
-    std::vector<int> seed_queue;
-    int sq_idx = 0;
-    seed_queue.push_back (index);
-
-    processed[index] = true;
-
-    while (sq_idx < static_cast<int> (seed_queue.size ()))
-    {
-      // Search for sq_idx
-      int ret = tree->radiusSearch (cloud[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances);
-      if( ret == -1)
-      {
-        PCL_ERROR("[pcl::extractEuclideanClusters] Received error code -1 from radiusSearch\n");
-        exit(0);
-      }
-      if (!ret)
-      {
-        sq_idx++;
-        continue;
-      }
-
-      for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)             // can't assume sorted (default isn't!)
-      {
-        if (nn_indices[j] == -1 || processed[nn_indices[j]])        // Has this point been processed before ?
-          continue;
-
-        // Perform a simple Euclidean clustering
-        seed_queue.push_back (nn_indices[j]);
-        processed[nn_indices[j]] = true;
-      }
-
-      sq_idx++;
-    }
-
-    // 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)
-        // This is the only place where indices come into play
-        r.indices[j] = seed_queue[j];
-
-      // These two lines should not be needed: (can anyone confirm?) -FF
-      //r.indices.assign(seed_queue.begin(), seed_queue.end());
-      std::sort (r.indices.begin (), r.indices.end ());
-      r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
-
-      r.header = cloud.header;
-      clusters.push_back (r);   // We could avoid a copy by working directly in the vector
-    }
-  }
+  const auto noop = [](){};
+  pcl::extractEuclideanClusters(cloud, indices, noop, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////

From 1b7a763e204d4471683012cf8ab8468ad53ed3b8 Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Mon, 13 Jul 2020 05:47:15 +0530
Subject: [PATCH 02/19] Switch to CloudIterator

---
 .../segmentation/impl/extract_clusters.hpp    | 23 +++++++++----------
 1 file changed, 11 insertions(+), 12 deletions(-)

diff --git a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
index 3c77b8de2b6..26b6a73e28d 100644
--- a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
+++ b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
@@ -62,16 +62,20 @@ pcl::extractEuclideanClusters (
   std::vector<int> nn_indices;
   std::vector<float> nn_distances;
   // Process all points in the indices vector
-  auto lambda = [&](int &i) {
-    if (processed[i])
+
+
+  auto it = indices.empty() ? ConstCloudIterator<PointT>(cloud) : ConstCloudIterator<PointT>(cloud, indices);
+
+  for (; it.isValid(); ++it) {
+    if (processed[it.getCurrentIndex()])
       return;
 
     int sq_idx = 0;
     clusters.emplace_back();
     auto seed_queue = clusters.back();
-    seed_queue.indices.push_back (i);
+    seed_queue.indices.push_back (it.getCurrentIndex());
 
-    processed[i] = true;
+    processed[it.getCurrentIndex()] = true;
 
     while (sq_idx < static_cast<int> (seed_queue.indices.size()))
     {
@@ -84,10 +88,10 @@ pcl::extractEuclideanClusters (
 
       for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)             // can't assume sorted (default isn't!)
       {
-        if (nn_indices[j] == -1 || processed[nn_indices[j]])        // Has this point been processed before ?
+        if (processed[nn_indices[j]])        // Has this point been processed before ?
           continue;
 
-        if (filter(i, j, nn_indices)) {
+        if (filter(it.getCurrentIndex(), j, nn_indices)) {
           seed_queue.indices.push_back(nn_indices[j]);
           processed[nn_indices[j]] = true;
         }
@@ -101,12 +105,7 @@ pcl::extractEuclideanClusters (
       seed_queue.header = cloud.header;
     else
       clusters.pop_back();
-  };
-
-  if (indices.size() == 1 && indices.front() == - 1)
-    for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i) lambda(i);
-  else
-    for (const int &index : indices) lambda(index);
+  }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////

From a985a573e2149b304f23d5cd524320750d20e9da Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Mon, 13 Jul 2020 05:50:06 +0530
Subject: [PATCH 03/19] Switch to Indices & fix noop lambda

---
 .../pcl/segmentation/extract_clusters.h       | 18 +++++++++-------
 .../segmentation/impl/extract_clusters.hpp    | 21 ++++++++++++-------
 2 files changed, 25 insertions(+), 14 deletions(-)

diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index 53454dc9cd3..6d25b2bc37e 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -47,10 +47,10 @@ namespace pcl
 {
   template <typename PointT, typename FunctorT> void
   extractEuclideanClusters (
-      const PointCloud<PointT> &cloud, const std::vector<int> &indices,
+      const PointCloud<PointT> &cloud, const Indices &indices,
       FunctorT filter, const typename search::Search<PointT>::Ptr &tree,
       float tolerance, std::vector<PointIndices> &clusters,
-      unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
+      unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits<int>::max());
 
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
@@ -83,7 +83,7 @@ namespace pcl
     */
   template <typename PointT> void 
   extractEuclideanClusters (
-      const PointCloud<PointT> &cloud, const std::vector<int> &indices,
+      const PointCloud<PointT> &cloud, const Indices &indices,
       const typename search::Search<PointT>::Ptr &tree, float tolerance, std::vector<PointIndices> &clusters,
       unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
 
@@ -119,7 +119,7 @@ namespace pcl
       PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ());
       return;
     }
-    auto lambda = [&](int &i, int &j, std::vector<int>& nn_indices) -> bool {
+    auto lambda = [&](int i, int j, const Indices& nn_indices) -> bool {
       //processed[nn_indices[j]] = true;
       // [-1;1]
       double dot_p = normals.points[i].normal[0] * normals.points[nn_indices[j]].normal[0] +
@@ -128,7 +128,7 @@ namespace pcl
       return std::acos (std::abs (dot_p)) < eps_angle;
     };
 
-    std::vector<int> indices{-1};
+    std::vector<Indices> indices;
 
     pcl::extractEuclideanClusters(cloud, indices, lambda, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
   }
@@ -152,7 +152,7 @@ namespace pcl
   template <typename PointT, typename Normal> 
   void extractEuclideanClusters (
       const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals,
-      const std::vector<int> &indices, const typename KdTree<PointT>::Ptr &tree,
+      const Indices &indices, const typename KdTree<PointT>::Ptr &tree,
       float tolerance, std::vector<PointIndices> &clusters, double eps_angle,
       unsigned int min_pts_per_cluster = 1,
       unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
@@ -174,10 +174,14 @@ namespace pcl
       PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ());
       return;
     }
+
+    if (indices.empty())
+      return;
+
     // Create a bool vector of processed point indices, and initialize it to false
     std::vector<bool> processed (cloud.points.size (), false);
 
-    auto lambda = [&](int &i, int &j, std::vector<int>& nn_indices) -> bool {
+    auto lambda = [&](int &i, int &j, Indices& nn_indices) -> bool {
       //processed[nn_indices[j]] = true;
       // [-1;1]
       double dot_p =
diff --git a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
index 26b6a73e28d..157f086f5ea 100644
--- a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
+++ b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
@@ -43,7 +43,7 @@
 template <typename PointT, typename FunctorT> void
 pcl::extractEuclideanClusters (
     const PointCloud<PointT> &cloud,
-    const std::vector<int> &indices,
+    const Indices &indices,
     FunctorT filter,
     const typename search::Search<PointT>::Ptr &tree,
     float tolerance, std::vector<PointIndices> &clusters,
@@ -59,7 +59,7 @@ pcl::extractEuclideanClusters (
   // 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;
+  Indices nn_indices;
   std::vector<float> nn_distances;
   // Process all points in the indices vector
 
@@ -116,8 +116,10 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
                                unsigned int min_pts_per_cluster,
                                unsigned int max_pts_per_cluster)
 {
-  const auto noop = []{};
-  std::vector<int> indices{-1};
+  auto noop = [&](__attribute__((unused)) int i, __attribute__((unused)) int j, __attribute__((unused)) const Indices& nn_indices) {
+    return true;
+  };
+  Indices indices;
   pcl::extractEuclideanClusters<PointT, decltype(noop)>(cloud, indices, noop, tree, clusters, min_pts_per_cluster, max_pts_per_cluster);
 }
 
@@ -125,7 +127,7 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
 /** @todo: fix the return value, make sure the exit is not needed anymore*/
 template <typename PointT> void
 pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
-                               const std::vector<int> &indices,
+                               const Indices &indices,
                                const typename search::Search<PointT>::Ptr &tree,
                                float tolerance, std::vector<PointIndices> &clusters,
                                unsigned int min_pts_per_cluster,
@@ -139,7 +141,12 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
     return;
   }
 
-  const auto noop = [](){};
+  if (indices.empty())
+    return;
+
+  auto noop = [&](__attribute__((unused)) int i, __attribute__((unused)) int j, __attribute__((unused)) const Indices& nn_indices) {
+    return true;
+  };
   pcl::extractEuclideanClusters(cloud, indices, noop, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
 }
 
@@ -182,6 +189,6 @@ pcl::EuclideanClusterExtraction<PointT>::extract (std::vector<PointIndices> &clu
 
 #define PCL_INSTANTIATE_EuclideanClusterExtraction(T) template class PCL_EXPORTS pcl::EuclideanClusterExtraction<T>;
 #define PCL_INSTANTIATE_extractEuclideanClusters(T) template void PCL_EXPORTS pcl::extractEuclideanClusters<T>(const pcl::PointCloud<T> &, const typename pcl::search::Search<T>::Ptr &, float , std::vector<pcl::PointIndices> &, unsigned int, unsigned int);
-#define PCL_INSTANTIATE_extractEuclideanClusters_indices(T) template void PCL_EXPORTS pcl::extractEuclideanClusters<T>(const pcl::PointCloud<T> &, const std::vector<int> &, const typename pcl::search::Search<T>::Ptr &, float , std::vector<pcl::PointIndices> &, unsigned int, unsigned int);
+#define PCL_INSTANTIATE_extractEuclideanClusters_indices(T) template void PCL_EXPORTS pcl::extractEuclideanClusters<T>(const pcl::PointCloud<T> &, const Indices &, const typename pcl::search::Search<T>::Ptr &, float , std::vector<pcl::PointIndices> &, unsigned int, unsigned int);
 
 #endif        // PCL_EXTRACT_CLUSTERS_IMPL_H_

From 242b8f51d247857352340cbc4159f3fb53ef016e Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Mon, 13 Jul 2020 07:36:08 +0530
Subject: [PATCH 04/19] Fix bugs

---
 .../include/pcl/segmentation/impl/extract_clusters.hpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
index 157f086f5ea..82a91e98a09 100644
--- a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
+++ b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
@@ -55,7 +55,7 @@ pcl::extractEuclideanClusters (
     return;
   }
   // Check if the tree is sorted -- if it is we don't need to check the first element
-  int nn_start_idx = tree->getSortedResults () ? 1 : 0;
+  index_t nn_start_idx = tree->getSortedResults () ? 1 : 0;
   // Create a bool vector of processed point indices, and initialize it to false
   std::vector<bool> processed (cloud.points.size (), false);
 
@@ -68,16 +68,16 @@ pcl::extractEuclideanClusters (
 
   for (; it.isValid(); ++it) {
     if (processed[it.getCurrentIndex()])
-      return;
+      continue;
 
-    int sq_idx = 0;
+    index_t sq_idx = 0;
     clusters.emplace_back();
-    auto seed_queue = clusters.back();
+    auto& seed_queue = clusters.back();
     seed_queue.indices.push_back (it.getCurrentIndex());
 
     processed[it.getCurrentIndex()] = true;
 
-    while (sq_idx < static_cast<int> (seed_queue.indices.size()))
+    while (sq_idx < static_cast<index_t> (seed_queue.indices.size()))
     {
       // Search for sq_idx
       if (!tree->radiusSearch (seed_queue.indices[sq_idx], tolerance, nn_indices, nn_distances))

From 03535065ead058b0ecef097b92dde0e776e38aab Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Mon, 13 Jul 2020 07:38:13 +0530
Subject: [PATCH 05/19] Concentrate checks into main overload

---
 .../include/pcl/segmentation/extract_clusters.h | 17 -----------------
 .../pcl/segmentation/impl/extract_clusters.hpp  | 17 +++++++++--------
 2 files changed, 9 insertions(+), 25 deletions(-)

diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index 6d25b2bc37e..ce396289501 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -109,11 +109,6 @@ namespace pcl
       unsigned int min_pts_per_cluster = 1,
       unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
   {
-    if (tree->getInputCloud ()->points.size () != cloud.points.size ())
-    {
-      PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
-      return;
-    }
     if (cloud.points.size () != normals.points.size ())
     {
       PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ());
@@ -157,18 +152,6 @@ namespace pcl
       unsigned int min_pts_per_cluster = 1,
       unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
   {
-    // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
-    //and indices[i]
-    if (tree->getInputCloud ()->points.size () != cloud.points.size ())
-    {
-      PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
-      return;
-    }
-    if (tree->getIndices ()->size () != indices.size ())
-    {
-      PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ());
-      return;
-    }
     if (cloud.points.size () != normals.points.size ())
     {
       PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ());
diff --git a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
index 82a91e98a09..788d09c4135 100644
--- a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
+++ b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
@@ -54,6 +54,15 @@ pcl::extractEuclideanClusters (
     PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
     return;
   }
+
+  // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
+  //and indices[i]
+  if (!indices.empty() and tree->getIndices ()->size () != indices.size ())
+  {
+    PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ());
+    return;
+  }
+
   // Check if the tree is sorted -- if it is we don't need to check the first element
   index_t nn_start_idx = tree->getSortedResults () ? 1 : 0;
   // Create a bool vector of processed point indices, and initialize it to false
@@ -133,14 +142,6 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
                                unsigned int min_pts_per_cluster,
                                unsigned int max_pts_per_cluster)
 {
-  // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
-  //and indices[i]
-  if (tree->getIndices ()->size () != indices.size ())
-  {
-    PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ());
-    return;
-  }
-
   if (indices.empty())
     return;
 

From d4f96b1ee4f4ef3b625fbfb6b523df4bc7ec9350 Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Mon, 13 Jul 2020 07:38:57 +0530
Subject: [PATCH 06/19]  Replace '.points[' with  '['

---
 .../include/pcl/segmentation/extract_clusters.h    | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index ce396289501..37fcdb2b9cd 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -114,12 +114,13 @@ namespace pcl
       PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ());
       return;
     }
+
     auto lambda = [&](int i, int j, const Indices& nn_indices) -> bool {
       //processed[nn_indices[j]] = true;
       // [-1;1]
-      double dot_p = normals.points[i].normal[0] * normals.points[nn_indices[j]].normal[0] +
-                     normals.points[i].normal[1] * normals.points[nn_indices[j]].normal[1] +
-                     normals.points[i].normal[2] * normals.points[nn_indices[j]].normal[2];
+      double dot_p = normals[i].normal[0] * normals[nn_indices[j]].normal[0] +
+                     normals[i].normal[1] * normals[nn_indices[j]].normal[1] +
+                     normals[i].normal[2] * normals[nn_indices[j]].normal[2];
       return std::acos (std::abs (dot_p)) < eps_angle;
     };
 
@@ -168,14 +169,13 @@ namespace pcl
       //processed[nn_indices[j]] = true;
       // [-1;1]
       double dot_p =
-          normals.points[indices[i]].normal[0] * normals.points[indices[nn_indices[j]]].normal[0] +
-          normals.points[indices[i]].normal[1] * normals.points[indices[nn_indices[j]]].normal[1] +
-          normals.points[indices[i]].normal[2] * normals.points[indices[nn_indices[j]]].normal[2];
+          normals[indices[i]].normal[0] * normals[indices[nn_indices[j]]].normal[0] +
+          normals[indices[i]].normal[1] * normals[indices[nn_indices[j]]].normal[1] +
+          normals[indices[i]].normal[2] * normals[indices[nn_indices[j]]].normal[2];
       return std::acos (std::abs (dot_p)) < eps_angle;
     };
 
     pcl::extractEuclideanClusters(cloud, indices, lambda, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
-
   }
 
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

From c7d683809f5a706702c73236f3a16c919a5ee495 Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Mon, 13 Jul 2020 08:41:52 +0530
Subject: [PATCH 07/19] Fix bugs

---
 segmentation/include/pcl/segmentation/extract_clusters.h        | 2 +-
 segmentation/include/pcl/segmentation/impl/extract_clusters.hpp | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index 37fcdb2b9cd..e876f4c8f49 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -124,7 +124,7 @@ namespace pcl
       return std::acos (std::abs (dot_p)) < eps_angle;
     };
 
-    std::vector<Indices> indices;
+    Indices indices;
 
     pcl::extractEuclideanClusters(cloud, indices, lambda, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
   }
diff --git a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
index 788d09c4135..a8e6e1c5bfb 100644
--- a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
+++ b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
@@ -129,7 +129,7 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
     return true;
   };
   Indices indices;
-  pcl::extractEuclideanClusters<PointT, decltype(noop)>(cloud, indices, noop, tree, clusters, min_pts_per_cluster, max_pts_per_cluster);
+  pcl::extractEuclideanClusters<PointT, decltype(noop)>(cloud, indices, noop, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////

From 4282d2f9efd287820a1443d5ee1c3b297dde7cde Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Mon, 13 Jul 2020 09:02:42 +0530
Subject: [PATCH 08/19] Update filter lambda name

---
 segmentation/include/pcl/segmentation/extract_clusters.h      | 2 +-
 .../include/pcl/segmentation/impl/extract_clusters.hpp        | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index e876f4c8f49..bfe13551a16 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -48,7 +48,7 @@ namespace pcl
   template <typename PointT, typename FunctorT> void
   extractEuclideanClusters (
       const PointCloud<PointT> &cloud, const Indices &indices,
-      FunctorT filter, const typename search::Search<PointT>::Ptr &tree,
+      FunctorT additional_filter_criteria, const typename search::Search<PointT>::Ptr &tree,
       float tolerance, std::vector<PointIndices> &clusters,
       unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits<int>::max());
 
diff --git a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
index a8e6e1c5bfb..be8e69163b7 100644
--- a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
+++ b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
@@ -44,7 +44,7 @@ template <typename PointT, typename FunctorT> void
 pcl::extractEuclideanClusters (
     const PointCloud<PointT> &cloud,
     const Indices &indices,
-    FunctorT filter,
+    FunctorT additional_filter_criteria,
     const typename search::Search<PointT>::Ptr &tree,
     float tolerance, std::vector<PointIndices> &clusters,
     unsigned int min_pts_per_cluster,
@@ -100,7 +100,7 @@ pcl::extractEuclideanClusters (
         if (processed[nn_indices[j]])        // Has this point been processed before ?
           continue;
 
-        if (filter(it.getCurrentIndex(), j, nn_indices)) {
+        if (additional_filter_criteria(it.getCurrentIndex(), j, nn_indices)) {
           seed_queue.indices.push_back(nn_indices[j]);
           processed[nn_indices[j]] = true;
         }

From fbf3b4b4f8dfba98b93a896b872d971a79aff93f Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Mon, 13 Jul 2020 09:43:34 +0530
Subject: [PATCH 09/19] Cleanup

---
 .../pcl/segmentation/extract_clusters.h       | 19 +++++--------------
 .../segmentation/impl/extract_clusters.hpp    | 14 ++++++--------
 2 files changed, 11 insertions(+), 22 deletions(-)

diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index bfe13551a16..6efa4f75296 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -85,7 +85,7 @@ namespace pcl
   extractEuclideanClusters (
       const PointCloud<PointT> &cloud, const Indices &indices,
       const typename search::Search<PointT>::Ptr &tree, float tolerance, std::vector<PointIndices> &clusters,
-      unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
+      unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits<int>::max());
 
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief Decompose a region of space into clusters based on the euclidean distance between points, and the normal
@@ -115,18 +115,14 @@ namespace pcl
       return;
     }
 
-    auto lambda = [&](int i, int j, const Indices& nn_indices) -> bool {
-      //processed[nn_indices[j]] = true;
-      // [-1;1]
+    auto additional_filter_criteria = [&](index_t i, index_t j, const Indices& nn_indices) -> bool {
       double dot_p = normals[i].normal[0] * normals[nn_indices[j]].normal[0] +
                      normals[i].normal[1] * normals[nn_indices[j]].normal[1] +
                      normals[i].normal[2] * normals[nn_indices[j]].normal[2];
       return std::acos (std::abs (dot_p)) < eps_angle;
     };
-
     Indices indices;
-
-    pcl::extractEuclideanClusters(cloud, indices, lambda, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
+    pcl::extractEuclideanClusters(cloud, indices, additional_filter_criteria, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
   }
 
 
@@ -162,12 +158,7 @@ namespace pcl
     if (indices.empty())
       return;
 
-    // Create a bool vector of processed point indices, and initialize it to false
-    std::vector<bool> processed (cloud.points.size (), false);
-
-    auto lambda = [&](int &i, int &j, Indices& nn_indices) -> bool {
-      //processed[nn_indices[j]] = true;
-      // [-1;1]
+    auto additional_filter_criteria = [&](index_t i, index_t j, Indices& nn_indices) -> bool {
       double dot_p =
           normals[indices[i]].normal[0] * normals[indices[nn_indices[j]]].normal[0] +
           normals[indices[i]].normal[1] * normals[indices[nn_indices[j]]].normal[1] +
@@ -175,7 +166,7 @@ namespace pcl
       return std::acos (std::abs (dot_p)) < eps_angle;
     };
 
-    pcl::extractEuclideanClusters(cloud, indices, lambda, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
+    pcl::extractEuclideanClusters(cloud, indices, additional_filter_criteria, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
   }
 
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
index be8e69163b7..46cdd1ce102 100644
--- a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
+++ b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
@@ -68,11 +68,6 @@ pcl::extractEuclideanClusters (
   // Create a bool vector of processed point indices, and initialize it to false
   std::vector<bool> processed (cloud.points.size (), false);
 
-  Indices nn_indices;
-  std::vector<float> nn_distances;
-  // Process all points in the indices vector
-
-
   auto it = indices.empty() ? ConstCloudIterator<PointT>(cloud) : ConstCloudIterator<PointT>(cloud, indices);
 
   for (; it.isValid(); ++it) {
@@ -88,6 +83,9 @@ pcl::extractEuclideanClusters (
 
     while (sq_idx < static_cast<index_t> (seed_queue.indices.size()))
     {
+      Indices nn_indices;
+      std::vector<float> nn_distances;
+
       // Search for sq_idx
       if (!tree->radiusSearch (seed_queue.indices[sq_idx], tolerance, nn_indices, nn_distances))
       {
@@ -95,9 +93,9 @@ pcl::extractEuclideanClusters (
         continue;
       }
 
-      for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)             // can't assume sorted (default isn't!)
+      for (index_t j = nn_start_idx; j < nn_indices.size (); ++j) // can't assume sorted (default isn't!)
       {
-        if (processed[nn_indices[j]])        // Has this point been processed before ?
+        if (processed[nn_indices[j]]) // Has this point been processed before ?
           continue;
 
         if (additional_filter_criteria(it.getCurrentIndex(), j, nn_indices)) {
@@ -145,7 +143,7 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
   if (indices.empty())
     return;
 
-  auto noop = [&](__attribute__((unused)) int i, __attribute__((unused)) int j, __attribute__((unused)) const Indices& nn_indices) {
+  auto noop = [&](__attribute__((unused)) index_t i, __attribute__((unused)) index_t j, __attribute__((unused)) const Indices& nn_indices) {
     return true;
   };
   pcl::extractEuclideanClusters(cloud, indices, noop, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);

From 20ac71cadaf49d5929c9706f740af8552082bcc4 Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Mon, 13 Jul 2020 10:00:35 +0530
Subject: [PATCH 10/19] Use `pcl::utils::ignore` to avoid unused variable
 warnings

---
 common/include/pcl/common/utils.h                           | 4 ++--
 segmentation/include/pcl/segmentation/extract_clusters.h    | 1 +
 .../include/pcl/segmentation/impl/extract_clusters.hpp      | 6 ++++--
 3 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/common/include/pcl/common/utils.h b/common/include/pcl/common/utils.h
index 2390ffeb21f..4a73e76eaa8 100644
--- a/common/include/pcl/common/utils.h
+++ b/common/include/pcl/common/utils.h
@@ -58,8 +58,8 @@ namespace pcl
     }
 
    /** \brief Utility function to eliminate unused variable warnings. */
-    template<typename T> void
-    ignore(const T&)
+    template<typename ...T> void
+    ignore(const T&...)
     {
     }
   } // namespace utils
diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index 6efa4f75296..b8809680c45 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -41,6 +41,7 @@
 
 #include <pcl/pcl_base.h>
 
+#include <pcl/common/utils.h>
 #include <pcl/search/pcl_search.h>
 
 namespace pcl
diff --git a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
index 46cdd1ce102..30b2c1ffc96 100644
--- a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
+++ b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
@@ -123,7 +123,8 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
                                unsigned int min_pts_per_cluster,
                                unsigned int max_pts_per_cluster)
 {
-  auto noop = [&](__attribute__((unused)) int i, __attribute__((unused)) int j, __attribute__((unused)) const Indices& nn_indices) {
+  auto noop = [&](int i, int j, const Indices& nn_indices) {
+    utils::ignore(i, j, nn_indices);
     return true;
   };
   Indices indices;
@@ -143,7 +144,8 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
   if (indices.empty())
     return;
 
-  auto noop = [&](__attribute__((unused)) index_t i, __attribute__((unused)) index_t j, __attribute__((unused)) const Indices& nn_indices) {
+  auto noop = [&](index_t i, index_t j, const Indices& nn_indices) {
+    utils::ignore(i, j, nn_indices);
     return true;
   };
   pcl::extractEuclideanClusters(cloud, indices, noop, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);

From 795b5458f22c68724221f4413c44714017dd8b47 Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Mon, 13 Jul 2020 10:54:10 +0530
Subject: [PATCH 11/19] Move function implementation to `.h` to avoid template
 instantiation

---
 .../pcl/segmentation/extract_clusters.h       | 70 ++++++++++++++++-
 .../segmentation/impl/extract_clusters.hpp    | 75 -------------------
 2 files changed, 68 insertions(+), 77 deletions(-)

diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index b8809680c45..f56c88fc9d8 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -51,7 +51,73 @@ namespace pcl
       const PointCloud<PointT> &cloud, const Indices &indices,
       FunctorT additional_filter_criteria, const typename search::Search<PointT>::Ptr &tree,
       float tolerance, std::vector<PointIndices> &clusters,
-      unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits<int>::max());
+      unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits<int>::max())
+  {
+    if (tree->getInputCloud ()->points.size () != cloud.points.size ())
+    {
+      PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
+      return;
+    }
+
+    // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
+    //and indices[i]
+    if (!indices.empty() and tree->getIndices ()->size () != indices.size ())
+    {
+      PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ());
+      return;
+    }
+
+    // Check if the tree is sorted -- if it is we don't need to check the first element
+    index_t nn_start_idx = tree->getSortedResults () ? 1 : 0;
+    // Create a bool vector of processed point indices, and initialize it to false
+    std::vector<bool> processed (cloud.points.size (), false);
+
+    auto it = indices.empty() ? ConstCloudIterator<PointT>(cloud) : ConstCloudIterator<PointT>(cloud, indices);
+
+    for (; it.isValid(); ++it) {
+      if (processed[it.getCurrentIndex()])
+        continue;
+
+      index_t sq_idx = 0;
+      clusters.emplace_back();
+      auto& seed_queue = clusters.back();
+      seed_queue.indices.push_back (it.getCurrentIndex());
+
+      processed[it.getCurrentIndex()] = true;
+
+      while (sq_idx < static_cast<index_t> (seed_queue.indices.size()))
+      {
+        Indices nn_indices;
+        std::vector<float> nn_distances;
+
+        // Search for sq_idx
+        if (!tree->radiusSearch (seed_queue.indices[sq_idx], tolerance, nn_indices, nn_distances))
+        {
+          sq_idx++;
+          continue;
+        }
+
+        for (index_t j = nn_start_idx; j < nn_indices.size (); ++j) // can't assume sorted (default isn't!)
+        {
+          if (processed[nn_indices[j]]) // Has this point been processed before ?
+            continue;
+
+          if (additional_filter_criteria(it.getCurrentIndex(), j, nn_indices)) {
+            seed_queue.indices.push_back(nn_indices[j]);
+            processed[nn_indices[j]] = true;
+          }
+        }
+
+        sq_idx++;
+      }
+
+      // If this queue is satisfactory, add to the clusters
+      if (seed_queue.indices.size () >= min_pts_per_cluster && seed_queue.indices.size () <= max_pts_per_cluster)
+        seed_queue.header = cloud.header;
+      else
+        clusters.pop_back();
+    }
+  }
 
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
@@ -68,7 +134,7 @@ namespace pcl
   extractEuclideanClusters (
       const PointCloud<PointT> &cloud, const typename search::Search<PointT>::Ptr &tree,
       float tolerance, std::vector<PointIndices> &clusters,
-      unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
+      unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits<int>::max ());
 
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
diff --git a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
index 30b2c1ffc96..564e71a16e2 100644
--- a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
+++ b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
@@ -40,81 +40,6 @@
 
 #include <pcl/segmentation/extract_clusters.h>
 
-template <typename PointT, typename FunctorT> void
-pcl::extractEuclideanClusters (
-    const PointCloud<PointT> &cloud,
-    const Indices &indices,
-    FunctorT additional_filter_criteria,
-    const typename search::Search<PointT>::Ptr &tree,
-    float tolerance, std::vector<PointIndices> &clusters,
-    unsigned int min_pts_per_cluster,
-    unsigned int max_pts_per_cluster) {
-  if (tree->getInputCloud ()->points.size () != cloud.points.size ())
-  {
-    PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
-    return;
-  }
-
-  // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
-  //and indices[i]
-  if (!indices.empty() and tree->getIndices ()->size () != indices.size ())
-  {
-    PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ());
-    return;
-  }
-
-  // Check if the tree is sorted -- if it is we don't need to check the first element
-  index_t nn_start_idx = tree->getSortedResults () ? 1 : 0;
-  // Create a bool vector of processed point indices, and initialize it to false
-  std::vector<bool> processed (cloud.points.size (), false);
-
-  auto it = indices.empty() ? ConstCloudIterator<PointT>(cloud) : ConstCloudIterator<PointT>(cloud, indices);
-
-  for (; it.isValid(); ++it) {
-    if (processed[it.getCurrentIndex()])
-      continue;
-
-    index_t sq_idx = 0;
-    clusters.emplace_back();
-    auto& seed_queue = clusters.back();
-    seed_queue.indices.push_back (it.getCurrentIndex());
-
-    processed[it.getCurrentIndex()] = true;
-
-    while (sq_idx < static_cast<index_t> (seed_queue.indices.size()))
-    {
-      Indices nn_indices;
-      std::vector<float> nn_distances;
-
-      // Search for sq_idx
-      if (!tree->radiusSearch (seed_queue.indices[sq_idx], tolerance, nn_indices, nn_distances))
-      {
-        sq_idx++;
-        continue;
-      }
-
-      for (index_t j = nn_start_idx; j < nn_indices.size (); ++j) // can't assume sorted (default isn't!)
-      {
-        if (processed[nn_indices[j]]) // Has this point been processed before ?
-          continue;
-
-        if (additional_filter_criteria(it.getCurrentIndex(), j, nn_indices)) {
-          seed_queue.indices.push_back(nn_indices[j]);
-          processed[nn_indices[j]] = true;
-        }
-      }
-
-      sq_idx++;
-    }
-
-    // If this queue is satisfactory, add to the clusters
-    if (seed_queue.indices.size () >= min_pts_per_cluster && seed_queue.indices.size () <= max_pts_per_cluster)
-      seed_queue.header = cloud.header;
-    else
-      clusters.pop_back();
-  }
-}
-
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
 pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,

From bd3bc6b4b090f166c5a47d21369c87a5c6dba2e9 Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Mon, 13 Jul 2020 10:55:18 +0530
Subject: [PATCH 12/19] Switch to `search::Kdtree` instead of `Kdtree`

---
 examples/segmentation/example_extract_clusters_normals.cpp | 6 +++---
 segmentation/include/pcl/segmentation/extract_clusters.h   | 4 ++--
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/examples/segmentation/example_extract_clusters_normals.cpp b/examples/segmentation/example_extract_clusters_normals.cpp
index a3e664c9131..8be22efe3f9 100644
--- a/examples/segmentation/example_extract_clusters_normals.cpp
+++ b/examples/segmentation/example_extract_clusters_normals.cpp
@@ -69,8 +69,8 @@ main (int, char **argv)
   std::cout << "Estimated the normals" << std::endl;
 
   // Creating the kdtree object for the search method of the extraction
-  pcl::KdTree<pcl::PointXYZ>::Ptr tree_ec  (new pcl::KdTreeFLANN<pcl::PointXYZ> ());
-  tree_ec->setInputCloud (cloud_ptr);
+  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;
+  tree->setInputCloud (cloud_ptr);
   
   // Extracting Euclidean clusters using cloud and its normals
   std::vector<pcl::PointIndices> cluster_indices;
@@ -78,7 +78,7 @@ main (int, char **argv)
   const double eps_angle = 5 * (M_PI / 180.0); // 5degree tolerance in normals
   const unsigned int min_cluster_size = 50;
  
-  pcl::extractEuclideanClusters (*cloud_ptr, *cloud_normals, tolerance, tree_ec, cluster_indices, eps_angle, min_cluster_size);
+  pcl::extractEuclideanClusters (*cloud_ptr, *cloud_normals, tolerance, tree, cluster_indices, eps_angle, min_cluster_size);
 
   std::cout << "No of clusters formed are " << cluster_indices.size () << std::endl;
 
diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index f56c88fc9d8..fc7b97532f6 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -171,7 +171,7 @@ namespace pcl
   template <typename PointT, typename Normal> void 
   extractEuclideanClusters (
       const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals,
-      float tolerance, const typename KdTree<PointT>::Ptr &tree,
+      float tolerance, const typename search::Search<PointT>::Ptr &tree,
       std::vector<PointIndices> &clusters, double eps_angle,
       unsigned int min_pts_per_cluster = 1,
       unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
@@ -211,7 +211,7 @@ namespace pcl
   template <typename PointT, typename Normal> 
   void extractEuclideanClusters (
       const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals,
-      const Indices &indices, const typename KdTree<PointT>::Ptr &tree,
+      const Indices &indices, const typename search::Search<PointT>::Ptr &tree,
       float tolerance, std::vector<PointIndices> &clusters, double eps_angle,
       unsigned int min_pts_per_cluster = 1,
       unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())

From 08b5155d3291d50d35b5b89a1cdcbf84e1069631 Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Mon, 13 Jul 2020 11:15:37 +0530
Subject: [PATCH 13/19] Use eigen for dot product

---
 .../include/pcl/segmentation/extract_clusters.h    | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index fc7b97532f6..2b6ac65fe04 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -182,11 +182,10 @@ namespace pcl
       return;
     }
 
+    auto cos_eps_angle = std::cos(eps_angle);
     auto additional_filter_criteria = [&](index_t i, index_t j, const Indices& nn_indices) -> bool {
-      double dot_p = normals[i].normal[0] * normals[nn_indices[j]].normal[0] +
-                     normals[i].normal[1] * normals[nn_indices[j]].normal[1] +
-                     normals[i].normal[2] * normals[nn_indices[j]].normal[2];
-      return std::acos (std::abs (dot_p)) < eps_angle;
+      double dot_p = normals[i].getNormalVector3fMap().dot(normals[nn_indices[j]].getNormalVector3fMap());
+      return std::abs(dot_p) < cos_eps_angle;
     };
     Indices indices;
     pcl::extractEuclideanClusters(cloud, indices, additional_filter_criteria, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
@@ -225,12 +224,11 @@ namespace pcl
     if (indices.empty())
       return;
 
+    auto cos_eps_angle = std::cos(eps_angle);
     auto additional_filter_criteria = [&](index_t i, index_t j, Indices& nn_indices) -> bool {
       double dot_p =
-          normals[indices[i]].normal[0] * normals[indices[nn_indices[j]]].normal[0] +
-          normals[indices[i]].normal[1] * normals[indices[nn_indices[j]]].normal[1] +
-          normals[indices[i]].normal[2] * normals[indices[nn_indices[j]]].normal[2];
-      return std::acos (std::abs (dot_p)) < eps_angle;
+          normals[indices[i]].getNormalVector3fMap().dot(normals[indices[nn_indices[j]]].getNormalVector3fMap());
+      return std::abs(dot_p) < cos_eps_angle;
     };
 
     pcl::extractEuclideanClusters(cloud, indices, additional_filter_criteria, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);

From 7d4f5045821bb80079cec7b7bc110c0e4bb7b896 Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Mon, 13 Jul 2020 11:42:49 +0530
Subject: [PATCH 14/19] Rename lambdas to be more descriptive

---
 segmentation/include/pcl/segmentation/extract_clusters.h | 9 ++++-----
 .../include/pcl/segmentation/impl/extract_clusters.hpp   | 9 ++++-----
 2 files changed, 8 insertions(+), 10 deletions(-)

diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index 2b6ac65fe04..06bed119cf0 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -183,12 +183,11 @@ namespace pcl
     }
 
     auto cos_eps_angle = std::cos(eps_angle);
-    auto additional_filter_criteria = [&](index_t i, index_t j, const Indices& nn_indices) -> bool {
+    auto normal_deviation_filter = [&](index_t i, index_t j, const Indices& nn_indices) -> bool {
       double dot_p = normals[i].getNormalVector3fMap().dot(normals[nn_indices[j]].getNormalVector3fMap());
       return std::abs(dot_p) < cos_eps_angle;
     };
-    Indices indices;
-    pcl::extractEuclideanClusters(cloud, indices, additional_filter_criteria, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
+    pcl::extractEuclideanClusters(cloud, normal_deviation_filter, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
   }
 
 
@@ -225,13 +224,13 @@ namespace pcl
       return;
 
     auto cos_eps_angle = std::cos(eps_angle);
-    auto additional_filter_criteria = [&](index_t i, index_t j, Indices& nn_indices) -> bool {
+    auto normal_deviation_filter = [&](index_t i, index_t j, Indices& nn_indices) -> bool {
       double dot_p =
           normals[indices[i]].getNormalVector3fMap().dot(normals[indices[nn_indices[j]]].getNormalVector3fMap());
       return std::abs(dot_p) < cos_eps_angle;
     };
 
-    pcl::extractEuclideanClusters(cloud, indices, additional_filter_criteria, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
+    pcl::extractEuclideanClusters(cloud, indices, normal_deviation_filter, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
   }
 
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
index 564e71a16e2..fefc9a4179e 100644
--- a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
+++ b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
@@ -48,12 +48,11 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
                                unsigned int min_pts_per_cluster,
                                unsigned int max_pts_per_cluster)
 {
-  auto noop = [&](int i, int j, const Indices& nn_indices) {
+  auto extract_all = [&](int i, int j, const Indices& nn_indices) {
     utils::ignore(i, j, nn_indices);
     return true;
   };
-  Indices indices;
-  pcl::extractEuclideanClusters<PointT, decltype(noop)>(cloud, indices, noop, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
+  pcl::extractEuclideanClusters<PointT>(cloud, extract_all, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -69,11 +68,11 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
   if (indices.empty())
     return;
 
-  auto noop = [&](index_t i, index_t j, const Indices& nn_indices) {
+  auto extract_all = [&](index_t i, index_t j, const Indices& nn_indices) {
     utils::ignore(i, j, nn_indices);
     return true;
   };
-  pcl::extractEuclideanClusters(cloud, indices, noop, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
+  pcl::extractEuclideanClusters(cloud, indices, extract_all, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////

From a2bcb28eb41c3453cd3a5972979cc479d84dcb1f Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Mon, 13 Jul 2020 11:43:34 +0530
Subject: [PATCH 15/19] Split clustering into 3 parts for cloud, indices and
 common cloud iterator

---
 .../pcl/segmentation/extract_clusters.h       | 48 +++++++++++++------
 1 file changed, 34 insertions(+), 14 deletions(-)

diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index 06bed119cf0..587c1d675d3 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -46,11 +46,11 @@
 
 namespace pcl
 {
+
   template <typename PointT, typename FunctorT> void
-  extractEuclideanClusters (
-      const PointCloud<PointT> &cloud, const Indices &indices,
-      FunctorT additional_filter_criteria, const typename search::Search<PointT>::Ptr &tree,
-      float tolerance, std::vector<PointIndices> &clusters,
+   extractEuclideanClusters(
+      ConstCloudIterator<PointT> &it, const PointCloud<PointT> &cloud, FunctorT additional_filter_criteria,
+      const typename search::Search<PointT>::Ptr &tree, float tolerance, std::vector<PointIndices> &clusters,
       unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits<int>::max())
   {
     if (tree->getInputCloud ()->points.size () != cloud.points.size ())
@@ -59,21 +59,11 @@ namespace pcl
       return;
     }
 
-    // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
-    //and indices[i]
-    if (!indices.empty() and tree->getIndices ()->size () != indices.size ())
-    {
-      PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ());
-      return;
-    }
-
     // Check if the tree is sorted -- if it is we don't need to check the first element
     index_t nn_start_idx = tree->getSortedResults () ? 1 : 0;
     // Create a bool vector of processed point indices, and initialize it to false
     std::vector<bool> processed (cloud.points.size (), false);
 
-    auto it = indices.empty() ? ConstCloudIterator<PointT>(cloud) : ConstCloudIterator<PointT>(cloud, indices);
-
     for (; it.isValid(); ++it) {
       if (processed[it.getCurrentIndex()])
         continue;
@@ -119,6 +109,36 @@ namespace pcl
     }
   }
 
+  template <typename PointT, typename FunctorT> void
+  extractEuclideanClusters (
+      const PointCloud<PointT> &cloud,
+      FunctorT additional_filter_criteria, const typename search::Search<PointT>::Ptr &tree,
+      float tolerance, std::vector<PointIndices> &clusters,
+      unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits<int>::max())
+  {
+    auto it = ConstCloudIterator<PointT>(cloud);
+    extractEuclideanClusters(it, cloud, additional_filter_criteria, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
+  }
+
+  template <typename PointT, typename FunctorT> void
+  extractEuclideanClusters (
+      const PointCloud<PointT> &cloud, const Indices &indices,
+      FunctorT additional_filter_criteria, const typename search::Search<PointT>::Ptr &tree,
+      float tolerance, std::vector<PointIndices> &clusters,
+      unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits<int>::max())
+  {
+    // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
+    //and indices[i]
+    if (!indices.empty() and tree->getIndices ()->size () != indices.size ())
+    {
+      PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ());
+      return;
+    }
+
+    auto it = ConstCloudIterator<PointT>(cloud, indices);
+    extractEuclideanClusters(it, cloud, additional_filter_criteria, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
+  }
+
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
     * \param cloud the point cloud message

From 95204a31a2861882a91c352d08a3fee61e0915ed Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Mon, 13 Jul 2020 11:48:47 +0530
Subject: [PATCH 16/19] Update error messages to be technically accurate

---
 segmentation/include/pcl/segmentation/extract_clusters.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index 587c1d675d3..e57f69fca66 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -55,7 +55,7 @@ namespace pcl
   {
     if (tree->getInputCloud ()->points.size () != cloud.points.size ())
     {
-      PCL_ERROR ("[pcl::extractEuclideanClusters] 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::extractEuclideanClusters] Tree built with a different point cloud size (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
       return;
     }
 
@@ -129,9 +129,9 @@ namespace pcl
   {
     // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
     //and indices[i]
-    if (!indices.empty() and tree->getIndices ()->size () != indices.size ())
+    if (tree->getIndices ()->size () != indices.size ())
     {
-      PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ());
+      PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built with a different size of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ());
       return;
     }
 

From 0128a1ffea185e88e32e3f26e69f5c26998c8cc8 Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Tue, 14 Jul 2020 09:50:46 +0530
Subject: [PATCH 17/19] Add docstring

---
 .../pcl/segmentation/extract_clusters.h       | 46 +++++++++++++++++++
 1 file changed, 46 insertions(+)

diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index e57f69fca66..a9a1cf7abcb 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -47,6 +47,21 @@
 namespace pcl
 {
 
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/** \brief Decompose a region of space into clusters based on the Euclidean distance between points
+  * \param it cloud iterator for iterating over the points
+  * \param cloud the point cloud message
+  * \param additional_filter_criteria functor which is used as an additonal criteria that all points being added to the cluster must satisfy
+  * \param 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 and \a indices
+  * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
+  * \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
+  * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
+  * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
+  * \warning It is assumed that that the tree built on the same indices and cloud as passed here,
+  * if that is not the case then things can go very wrong. For speed reasons, we only check the sizes and not the content
+  * \ingroup segmentation
+  */
   template <typename PointT, typename FunctorT> void
    extractEuclideanClusters(
       ConstCloudIterator<PointT> &it, const PointCloud<PointT> &cloud, FunctorT additional_filter_criteria,
@@ -109,6 +124,20 @@ namespace pcl
     }
   }
 
+  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+  /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
+    * \param cloud the point cloud message
+    * \param additional_filter_criteria functor which is used as an additonal criteria that all points being added to the cluster must satisfy
+    * \param 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 and \a indices
+    * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
+    * \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
+    * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
+    * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
+    * \warning It is assumed that that the tree built on the same indices and cloud as passed here,
+    * if that is not the case then things can go very wrong. For speed reasons, we only check the sizes and not the content
+    * \ingroup segmentation
+    */
   template <typename PointT, typename FunctorT> void
   extractEuclideanClusters (
       const PointCloud<PointT> &cloud,
@@ -120,6 +149,21 @@ namespace pcl
     extractEuclideanClusters(it, cloud, additional_filter_criteria, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
   }
 
+  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+  /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
+    * \param cloud the point cloud message
+    * \param indices a list of point indices to use from \a cloud
+    * \param additional_filter_criteria functor which is used as an additonal criteria that all points being added to the cluster must satisfy
+    * \param 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 and \a indices
+    * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
+    * \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
+    * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
+    * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
+    * \warning It is assumed that that the tree built on the same indices and cloud as passed here,
+    * if that is not the case then things can go very wrong. For speed reasons, we only check the sizes and not the content
+    * \ingroup segmentation
+    */
   template <typename PointT, typename FunctorT> void
   extractEuclideanClusters (
       const PointCloud<PointT> &cloud, const Indices &indices,
@@ -202,6 +246,7 @@ namespace pcl
       return;
     }
 
+    eps_angle = std::max(std::abs(eps_angle), M_PI);
     auto cos_eps_angle = std::cos(eps_angle);
     auto normal_deviation_filter = [&](index_t i, index_t j, const Indices& nn_indices) -> bool {
       double dot_p = normals[i].getNormalVector3fMap().dot(normals[nn_indices[j]].getNormalVector3fMap());
@@ -243,6 +288,7 @@ namespace pcl
     if (indices.empty())
       return;
 
+    eps_angle = std::max(std::abs(eps_angle), M_PI);
     auto cos_eps_angle = std::cos(eps_angle);
     auto normal_deviation_filter = [&](index_t i, index_t j, Indices& nn_indices) -> bool {
       double dot_p =

From 228fae439671fb610fcc861bf1284ce97b4d98de Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Fri, 17 Jul 2020 04:24:41 +0530
Subject: [PATCH 18/19] Add functor checks and improve docstring

---
 .../pcl/segmentation/extract_clusters.h       | 67 +++++++++++--------
 .../segmentation/impl/extract_clusters.hpp    | 10 +--
 2 files changed, 43 insertions(+), 34 deletions(-)

diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h
index a9a1cf7abcb..9371976f61b 100644
--- a/segmentation/include/pcl/segmentation/extract_clusters.h
+++ b/segmentation/include/pcl/segmentation/extract_clusters.h
@@ -47,19 +47,30 @@
 namespace pcl
 {
 
+namespace detail {
+template <typename PointT, typename Function>
+constexpr static bool is_functor_for_additional_filter_criteria_v =
+    pcl::is_invocable_r_v<bool,
+                          Function,
+                          const pcl::remove_cvref_t<pcl::PointCloud<PointT>>&,
+                          pcl::index_t,
+                          const pcl::remove_cvref_t<Indices>&,
+                          pcl::index_t>;
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
   * \param it cloud iterator for iterating over the points
   * \param cloud the point cloud message
-  * \param additional_filter_criteria functor which is used as an additonal criteria that all points being added to the cluster must satisfy
+  * \param additional_filter_criteria a functor specifying a criterion that must be satisfied by all point added to the cluster
   * \param 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 and \a indices
   * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
   * \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
   * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
   * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
-  * \warning It is assumed that that the tree built on the same indices and cloud as passed here,
-  * if that is not the case then things can go very wrong. For speed reasons, we only check the sizes and not the content
+  * \warning The cloud/indices passed here must be the same ones used to build the tree.
+  *  For performance reasons PCL on warns if the sizes are not equal and no checks are performed to verify if the content is the same
   * \ingroup segmentation
   */
   template <typename PointT, typename FunctorT> void
@@ -68,6 +79,10 @@ namespace pcl
       const typename search::Search<PointT>::Ptr &tree, float tolerance, std::vector<PointIndices> &clusters,
       unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits<int>::max())
   {
+    static_assert(detail::is_functor_for_additional_filter_criteria_v<PointT, FunctorT>,
+                  "Functor signature must be similar to `bool(const PointCloud<PointT>&, "
+                  "index_t, const Indices&, index_t)`");
+
     if (tree->getInputCloud ()->points.size () != cloud.points.size ())
     {
       PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built with a different point cloud size (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
@@ -83,37 +98,31 @@ namespace pcl
       if (processed[it.getCurrentIndex()])
         continue;
 
-      index_t sq_idx = 0;
       clusters.emplace_back();
       auto& seed_queue = clusters.back();
       seed_queue.indices.push_back (it.getCurrentIndex());
 
       processed[it.getCurrentIndex()] = true;
 
-      while (sq_idx < static_cast<index_t> (seed_queue.indices.size()))
+      for (index_t sq_idx = 0; sq_idx < static_cast<index_t> (seed_queue.indices.size()); ++sq_idx)
       {
         Indices nn_indices;
         std::vector<float> nn_distances;
 
         // Search for sq_idx
         if (!tree->radiusSearch (seed_queue.indices[sq_idx], tolerance, nn_indices, nn_distances))
-        {
-          sq_idx++;
           continue;
-        }
 
         for (index_t j = nn_start_idx; j < nn_indices.size (); ++j) // can't assume sorted (default isn't!)
         {
           if (processed[nn_indices[j]]) // Has this point been processed before ?
             continue;
 
-          if (additional_filter_criteria(it.getCurrentIndex(), j, nn_indices)) {
+          if (additional_filter_criteria(cloud, it.getCurrentIndex(), nn_indices, j)) {
             seed_queue.indices.push_back(nn_indices[j]);
             processed[nn_indices[j]] = true;
           }
         }
-
-        sq_idx++;
       }
 
       // If this queue is satisfactory, add to the clusters
@@ -127,15 +136,15 @@ namespace pcl
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
     * \param cloud the point cloud message
-    * \param additional_filter_criteria functor which is used as an additonal criteria that all points being added to the cluster must satisfy
+    * \param additional_filter_criteria a functor specifying a criterion that must be satisfied by all point added to the cluster
     * \param 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 and \a indices
     * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
     * \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
     * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
     * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
-    * \warning It is assumed that that the tree built on the same indices and cloud as passed here,
-    * if that is not the case then things can go very wrong. For speed reasons, we only check the sizes and not the content
+    * \warning The cloud/indices passed here must be the same ones used to build the tree.
+    *  For performance reasons PCL on warns if the sizes are not equal and no checks are performed to verify if the content is the same
     * \ingroup segmentation
     */
   template <typename PointT, typename FunctorT> void
@@ -153,15 +162,15 @@ namespace pcl
   /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
     * \param cloud the point cloud message
     * \param indices a list of point indices to use from \a cloud
-    * \param additional_filter_criteria functor which is used as an additonal criteria that all points being added to the cluster must satisfy
+    * \param additional_filter_criteria a functor specifying a criterion that must be satisfied by all point added to the cluster
     * \param 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 and \a indices
     * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
     * \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
     * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
     * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
-    * \warning It is assumed that that the tree built on the same indices and cloud as passed here,
-    * if that is not the case then things can go very wrong. For speed reasons, we only check the sizes and not the content
+    * \warning The cloud/indices passed here must be the same ones used to build the tree.
+    *  For performance reasons PCL on warns if the sizes are not equal and no checks are performed to verify if the content is the same
     * \ingroup segmentation
     */
   template <typename PointT, typename FunctorT> void
@@ -227,7 +236,7 @@ namespace pcl
     * \note the tree has to be created as a spatial locator on \a cloud
     * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space
     * \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
-    * \param eps_angle the maximum allowed difference between normals in radians for cluster/region growing
+    * \param max_angle the maximum allowed difference between normals in radians for cluster/region growing
     * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
     * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
     * \ingroup segmentation
@@ -236,7 +245,7 @@ namespace pcl
   extractEuclideanClusters (
       const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals,
       float tolerance, const typename search::Search<PointT>::Ptr &tree,
-      std::vector<PointIndices> &clusters, double eps_angle,
+      std::vector<PointIndices> &clusters, double max_angle,
       unsigned int min_pts_per_cluster = 1,
       unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
   {
@@ -246,11 +255,11 @@ namespace pcl
       return;
     }
 
-    eps_angle = std::max(std::abs(eps_angle), M_PI);
-    auto cos_eps_angle = std::cos(eps_angle);
-    auto normal_deviation_filter = [&](index_t i, index_t j, const Indices& nn_indices) -> bool {
+    max_angle = std::min(std::abs(max_angle), M_PI);
+    auto cos_max_angle = std::cos(max_angle);
+    auto normal_deviation_filter = [&](const PointCloud<PointT> &cloud, index_t i, const Indices& nn_indices, index_t j) -> bool {
       double dot_p = normals[i].getNormalVector3fMap().dot(normals[nn_indices[j]].getNormalVector3fMap());
-      return std::abs(dot_p) < cos_eps_angle;
+      return std::abs(dot_p) < cos_max_angle;
     };
     pcl::extractEuclideanClusters(cloud, normal_deviation_filter, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
   }
@@ -266,7 +275,7 @@ namespace pcl
     * \note the tree has to be created as a spatial locator on \a cloud
     * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space
     * \param clusters the resultant clusters containing point indices (as PointIndices)
-    * \param eps_angle the maximum allowed difference between normals in degrees for cluster/region growing
+    * \param max_angle the maximum allowed difference between normals in degrees for cluster/region growing
     * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
     * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
     * \ingroup segmentation
@@ -275,7 +284,7 @@ namespace pcl
   void extractEuclideanClusters (
       const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals,
       const Indices &indices, const typename search::Search<PointT>::Ptr &tree,
-      float tolerance, std::vector<PointIndices> &clusters, double eps_angle,
+      float tolerance, std::vector<PointIndices> &clusters, double max_angle,
       unsigned int min_pts_per_cluster = 1,
       unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
   {
@@ -288,12 +297,12 @@ namespace pcl
     if (indices.empty())
       return;
 
-    eps_angle = std::max(std::abs(eps_angle), M_PI);
-    auto cos_eps_angle = std::cos(eps_angle);
-    auto normal_deviation_filter = [&](index_t i, index_t j, Indices& nn_indices) -> bool {
+    max_angle = std::min(std::abs(max_angle), M_PI);
+    auto cos_max_angle = std::cos(max_angle);
+    auto normal_deviation_filter = [&](const PointCloud<PointT> &cloud, index_t i, const Indices& nn_indices, index_t j) -> bool {
       double dot_p =
           normals[indices[i]].getNormalVector3fMap().dot(normals[indices[nn_indices[j]]].getNormalVector3fMap());
-      return std::abs(dot_p) < cos_eps_angle;
+      return std::abs(dot_p) < cos_max_angle;
     };
 
     pcl::extractEuclideanClusters(cloud, indices, normal_deviation_filter, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
diff --git a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
index fefc9a4179e..be717e0765d 100644
--- a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
+++ b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp
@@ -48,9 +48,9 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
                                unsigned int min_pts_per_cluster,
                                unsigned int max_pts_per_cluster)
 {
-  auto extract_all = [&](int i, int j, const Indices& nn_indices) {
-    utils::ignore(i, j, nn_indices);
-    return true;
+  auto extract_all = [&](const PointCloud<PointT> &cloud, index_t i, const Indices& nn_indices, index_t j) {
+      utils::ignore(cloud, i, nn_indices, j);
+      return true;
   };
   pcl::extractEuclideanClusters<PointT>(cloud, extract_all, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);
 }
@@ -68,8 +68,8 @@ pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud,
   if (indices.empty())
     return;
 
-  auto extract_all = [&](index_t i, index_t j, const Indices& nn_indices) {
-    utils::ignore(i, j, nn_indices);
+  auto extract_all = [&](const PointCloud<PointT> &cloud, index_t i, const Indices& nn_indices, index_t j) {
+    utils::ignore(cloud, i, nn_indices, j);
     return true;
   };
   pcl::extractEuclideanClusters(cloud, indices, extract_all, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster);

From b6283ca2594de7cd6b516a5f4d47b5c7bdba7cbc Mon Sep 17 00:00:00 2001
From: Shrijit Singh <shrijitsingh99@gmail.com>
Date: Fri, 17 Jul 2020 04:24:52 +0530
Subject: [PATCH 19/19] Fix nullptr bug

---
 examples/segmentation/example_extract_clusters_normals.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/segmentation/example_extract_clusters_normals.cpp b/examples/segmentation/example_extract_clusters_normals.cpp
index 8be22efe3f9..055e606a3cb 100644
--- a/examples/segmentation/example_extract_clusters_normals.cpp
+++ b/examples/segmentation/example_extract_clusters_normals.cpp
@@ -69,7 +69,7 @@ main (int, char **argv)
   std::cout << "Estimated the normals" << std::endl;
 
   // Creating the kdtree object for the search method of the extraction
-  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;
+  auto tree = std::make_shared<pcl::search::KdTree<pcl::PointXYZ>>();
   tree->setInputCloud (cloud_ptr);
   
   // Extracting Euclidean clusters using cloud and its normals