Only skip first result of radiusSearch if results are sorted

features/include/pcl/features/impl/cvfh.hpp

@@ -96,6 +96,8 @@ pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSmoot
               static_cast<std::size_t>(normals.size()));
     return;
   }
+  // If tree gives sorted results, we can skip the first one because it is the query point itself
+  const std::size_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.size (), false);
@@ -124,8 +126,7 @@ pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSmoot
         continue;
       }
 
-      // skip index 0, since nn_indices[0] == idx, worth it?
-      for (std::size_t j = 1; j < nn_indices.size (); ++j)
+      for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)
       {
         if (processed[nn_indices[j]]) // Has this point been processed before ?
           continue;

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

@@ -97,6 +97,8 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSm
               static_cast<std::size_t>(normals.size()));
     return;
   }
+  // If tree gives sorted results, we can skip the first one because it is the query point itself
+  const std::size_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.size (), false);
@@ -124,7 +126,7 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSm
         continue;
       }
 
-      for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
+      for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)
       {
         if (processed[nn_indices[j]]) // Has this point been processed before ?
           continue;

kdtree/include/pcl/kdtree/kdtree.h

@@ -332,6 +332,15 @@ namespace pcl
         return (min_pts_);
       }
 
+      /** \brief Gets whether the results should be sorted (ascending in the distance) or not
+        * Otherwise the results may be returned in any order.
+        */
+      inline bool
+      getSortedResults () const
+      {
+        return (sorted_);
+      }
+
     protected:
       /** \brief The input point cloud dataset containing the points we need to use. */
       PointCloudConstPtr input_;

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

@@ -61,6 +61,8 @@ inline void extractEuclideanClustersSmooth(const typename pcl::PointCloud<PointT
     PCL_ERROR("[pcl::extractEuclideanClusters] Number of points in the input point cloud different than normals!\n");
     return;
   }
+  // If tree gives sorted results, we can skip the first one because it is the query point itself
+  const std::size_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.size (), false);
@@ -96,7 +98,7 @@ 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 = nn_start_idx; j < nn_indices.size (); ++j)
       {
         if (processed[nn_indices[j]]) // Has this point been processed before ?
           continue;

segmentation/include/pcl/segmentation/extract_clusters.h

@@ -123,6 +123,8 @@ namespace pcl
                 static_cast<std::size_t>(normals.size()));
       return;
     }
+    // If tree gives sorted results, we can skip the first one because it is the query point itself
+    const std::size_t nn_start_idx = tree->getSortedResults () ? 1 : 0;
     const double cos_eps_angle = std::cos (eps_angle); // compute this once instead of acos many times (faster)
 
     // Create a bool vector of processed point indices, and initialize it to false
@@ -151,7 +153,7 @@ 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 = nn_start_idx; j < nn_indices.size (); ++j)
         {
           if (processed[nn_indices[j]])                         // Has this point been processed before ?
             continue;
@@ -243,6 +245,8 @@ namespace pcl
                 static_cast<std::size_t>(normals.size()));
       return;
     }
+    // If tree gives sorted results, we can skip the first one because it is the query point itself
+    const std::size_t nn_start_idx = tree->getSortedResults () ? 1 : 0;
     const double cos_eps_angle = std::cos (eps_angle); // compute this once instead of acos many times (faster)
     // Create a bool vector of processed point indices, and initialize it to false
     std::vector<bool> processed (cloud.size (), false);
@@ -270,7 +274,7 @@ 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 = nn_start_idx; j < nn_indices.size (); ++j)
         {
           if (processed[nn_indices[j]])                             // Has this point been processed before ?
             continue;

segmentation/include/pcl/segmentation/impl/conditional_euclidean_clustering.hpp

@@ -60,11 +60,13 @@ pcl::ConditionalEuclideanClustering<PointT>::segment (pcl::IndicesClusters &clus
   if (!searcher_)
   {
     if (input_->isOrganized ())
-      searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
+      searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> (false)); // not requiring sorted results is much faster
     else
-      searcher_.reset (new pcl::search::KdTree<PointT> ());
+      searcher_.reset (new pcl::search::KdTree<PointT> (false)); // not requiring sorted results is much faster
   }
   searcher_->setInputCloud (input_, indices_);
+  // If searcher_ gives sorted results, we can skip the first one because it is the query point itself
+  const int nn_start_idx = searcher_->getSortedResults () ? 1 : 0;
 
   // Temp variables used by search class
   Indices nn_indices;
@@ -100,7 +102,7 @@ pcl::ConditionalEuclideanClustering<PointT>::segment (pcl::IndicesClusters &clus
       }
 
       // Process the neighbors
-      for (int nii = 1; nii < static_cast<int> (nn_indices.size ()); ++nii)  // nii = neighbor indices iterator
+      for (int nii = nn_start_idx; nii < static_cast<int> (nn_indices.size ()); ++nii)  // nii = neighbor indices iterator
       {
         // Has this point been processed before?
         if (nn_indices[nii] == UNAVAILABLE || processed[nn_indices[nii]])

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

@@ -57,6 +57,8 @@ pcl::extractLabeledEuclideanClusters(
               cloud.size());
     return;
   }
+  // If tree gives sorted results, we can skip the first one because it is the query point itself
+  const std::size_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.size(), false);
 
@@ -88,8 +90,7 @@ 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 = nn_start_idx; j < nn_indices.size(); ++j)
       {
         if (processed[nn_indices[j]]) // Has this point been processed before ?
           continue;

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

@@ -61,6 +61,8 @@ pcl::seededHueSegmentation (const PointCloud<PointXYZRGB>          &cloud,
               static_cast<std::size_t>(cloud.size()));
     return;
   }
+  // If tree gives sorted results, we can skip the first one because it is the query point itself
+  const std::size_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.size (), false);
 
@@ -96,7 +98,7 @@ 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 = nn_start_idx; j < nn_indices.size (); ++j)
       {
         if (processed[nn_indices[j]])                             // Has this point been processed before ?
           continue;
@@ -139,6 +141,8 @@ pcl::seededHueSegmentation (const PointCloud<PointXYZRGB>            &cloud,
               static_cast<std::size_t>(cloud.size()));
     return;
   }
+  // If tree gives sorted results, we can skip the first one because it is the query point itself
+  const std::size_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.size (), false);
 
@@ -173,7 +177,7 @@ 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 = nn_start_idx; j < nn_indices.size (); ++j)
       {
         if (processed[nn_indices[j]])                             // Has this point been processed before ?
           continue;