Correct usage of radiusSearch results when unsorted search trees are provided as input

features/include/pcl/features/impl/our_cvfh.hpp

@@ -109,37 +109,50 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSm
     if (processed[i])
       continue;
 
-    std::vector<std::size_t> seed_queue;
+    pcl::Indices seed_queue;
     std::size_t sq_idx = 0;
-    seed_queue.push_back (i);
+    seed_queue.push_back(i);
 
     processed[i] = true;
 
-    while (sq_idx < seed_queue.size ())
-    {
+    while (sq_idx < seed_queue.size()) {
       // Search for sq_idx
-      if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
-      {
+      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;
+      const auto process_neighbor = [&](const auto& neighbor) {
+        if (processed[neighbor]) // Has this point been processed before ?
+          return;
 
         //processed[nn_indices[j]] = true;
         // [-1;1]
 
-        double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0]
-            + normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] + normals[seed_queue[sq_idx]].normal[2]
-            * normals[nn_indices[j]].normal[2];
+        double dot_p =
+            normals[seed_queue[sq_idx]].normal[0] * normals[neighbor].normal[0] +
+            normals[seed_queue[sq_idx]].normal[1] * normals[neighbor].normal[1] +
+            normals[seed_queue[sq_idx]].normal[2] * normals[neighbor].normal[2];
 
-        if (std::abs (std::acos (dot_p)) < eps_angle)
-        {
-          processed[nn_indices[j]] = true;
-          seed_queue.push_back (nn_indices[j]);
+        if (std::abs(std::acos(dot_p)) < eps_angle) {
+          processed[neighbor] = true;
+          seed_queue.push_back(neighbor);
+        }
+      };
+
+      if (tree->getSortedResults()) {
+        // we can skip the 1st result, since it would be the same
+        for (std::size_t j = 1; j < nn_indices.size(); ++j) {
+          process_neighbor(nn_indices[j]);
+        }
+      }
+      else {
+        for (const auto& nn_idx : nn_indices) {
+          if (seed_queue[sq_idx] == nn_idx) { // Don't process the same point
+            continue;
+          }
+          process_neighbor(nn_idx);
         }
       }
 

recognition/include/pcl/recognition/impl/hv/hv_go.hpp

@@ -68,19 +68,18 @@ inline void extractEuclideanClustersSmooth(const typename pcl::PointCloud<PointT
   pcl::Indices nn_indices;
   std::vector<float> nn_distances;
   // Process all points in the indices vector
-  int size = static_cast<int> (cloud.size ());
-  for (int i = 0; i < size; ++i)
+  for (std::size_t i = 0; i < cloud.size(); ++i)
   {
     if (processed[i])
       continue;
 
-    std::vector<unsigned int> seed_queue;
-    int sq_idx = 0;
+    pcl::Indices seed_queue;
+    pcl::index_t sq_idx = 0;
     seed_queue.push_back (i);
 
     processed[i] = true;
 
-    while (sq_idx < static_cast<int> (seed_queue.size ()))
+    while (static_cast<std::size_t>(sq_idx) < seed_queue.size ())
     {
 
       if (normals[seed_queue[sq_idx]].curvature > curvature_threshold)
@@ -96,8 +95,10 @@ inline void extractEuclideanClustersSmooth(const typename pcl::PointCloud<PointT
         continue;
       }
 
-      for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
-      {
+      for (std::size_t j = 0; j < nn_indices.size(); ++j) {
+        if (seed_queue[sq_idx] == nn_indices[j]) { // Don't process the same point
+          continue;
+        }
         if (processed[nn_indices[j]]) // Has this point been processed before ?
           continue;
 

segmentation/include/pcl/segmentation/extract_clusters.h

@@ -151,8 +151,10 @@ namespace pcl
           continue;
         }
 
-        for (std::size_t j = 1; j < nn_indices.size (); ++j)             // nn_indices[0] should be sq_idx
-        {
+        for (std::size_t j = 0; j < nn_indices.size(); ++j) {
+          if (seed_queue[sq_idx] == nn_indices[j]) { // Don't process the same point
+            continue;
+          }
           if (processed[nn_indices[j]])                         // Has this point been processed before ?
             continue;
 
@@ -271,8 +273,10 @@ namespace pcl
           continue;
         }
 
-        for (std::size_t j = 1; j < nn_indices.size (); ++j)             // nn_indices[0] should be sq_idx
-        {
+        for (std::size_t j = 0; j < nn_indices.size(); ++j) {
+          if (seed_queue[sq_idx] == nn_indices[j]) { // Don't process the same point
+            continue;
+          }
           if (processed[nn_indices[j]])                             // Has this point been processed before ?
             continue;
 

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

@@ -107,9 +107,10 @@ pcl::extractLabeledEuclideanClusters(
         continue;
       }
 
-      for (std::size_t j = 1; j < nn_indices.size();
-           ++j) // nn_indices[0] should be sq_idx
-      {
+      for (std::size_t j = 0; j < nn_indices.size(); ++j) {
+        if (seed_queue[sq_idx] == nn_indices[j]) { // Don't process the same point
+          continue;
+        }
         if (processed[nn_indices[j]]) // Has this point been processed before ?
           continue;
         if (cloud[i].label == cloud[nn_indices[j]].label) {

segmentation/include/pcl/segmentation/impl/seeded_hue_segmentation.hpp

@@ -96,8 +96,10 @@ pcl::seededHueSegmentation (const PointCloud<PointXYZRGB>          &cloud,
         continue;
       }
 
-      for (std::size_t j = 1; j < nn_indices.size (); ++j)             // nn_indices[0] should be sq_idx
-      {
+      for (std::size_t j = 0; j < nn_indices.size(); ++j) {
+        if (seed_queue[sq_idx] == nn_indices[j]) { // Don't process the same point
+          continue;
+        }
         if (processed[nn_indices[j]])                             // Has this point been processed before ?
           continue;
 
@@ -173,8 +175,10 @@ pcl::seededHueSegmentation (const PointCloud<PointXYZRGB>            &cloud,
         sq_idx++;
         continue;
       }
-      for (std::size_t j = 1; j < nn_indices.size (); ++j)             // nn_indices[0] should be sq_idx
-      {
+      for (std::size_t j = 0; j < nn_indices.size(); ++j) {
+        if (seed_queue[sq_idx] == nn_indices[j]) { // Don't process the same point
+          continue;
+        }
         if (processed[nn_indices[j]])                             // Has this point been processed before ?
           continue;