Merge pull request #5997 from mvieth/radius_search_sorted_results

mvieth · web-flow · commit fa9d6adc81ad · 2024-04-09T09:47:37.000+02:00
Only skip first result of radiusSearch if results are sorted
diff --git a/features/include/pcl/features/impl/cvfh.hpp b/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;
diff --git a/features/include/pcl/features/impl/our_cvfh.hpp b/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;
diff --git a/kdtree/include/pcl/kdtree/kdtree.h b/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_;
diff --git a/recognition/include/pcl/recognition/impl/hv/hv_go.hpp b/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;
diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/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;
diff --git a/segmentation/include/pcl/segmentation/impl/conditional_euclidean_clustering.hpp b/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]])
diff --git a/segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp b/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;
diff --git a/segmentation/include/pcl/segmentation/impl/seeded_hue_segmentation.hpp b/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;

Original file line number	Diff line number	Diff line change
`@@ -96,6 +96,8 @@ pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSmoot`
`96`	`96`	`static_cast<std::size_t>(normals.size()));`
`97`	`97`	`return;`
`98`	`98`	`}`
	`99`	`+ // If tree gives sorted results, we can skip the first one because it is the query point itself`
	`100`	`+ const std::size_t nn_start_idx = tree->getSortedResults () ? 1 : 0;`
`99`	`101`
`100`	`102`	`// Create a bool vector of processed point indices, and initialize it to false`
`101`	`103`	`std::vector<bool> processed (cloud.size (), false);`
`@@ -124,8 +126,7 @@ pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSmoot`
`124`	`126`	`continue;`
`125`	`127`	`}`
`126`	`128`
`127`		`- // skip index 0, since nn_indices[0] == idx, worth it?`
`128`		`- for (std::size_t j = 1; j < nn_indices.size (); ++j)`
	`129`	`+ for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)`
`129`	`130`	`{`
`130`	`131`	`if (processed[nn_indices[j]]) // Has this point been processed before ?`
`131`	`132`	`continue;`
Original file line number	Diff line number	Diff line change
`@@ -97,6 +97,8 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSm`
`97`	`97`	`static_cast<std::size_t>(normals.size()));`
`98`	`98`	`return;`
`99`	`99`	`}`
	`100`	`+ // If tree gives sorted results, we can skip the first one because it is the query point itself`
	`101`	`+ const std::size_t nn_start_idx = tree->getSortedResults () ? 1 : 0;`
`100`	`102`
`101`	`103`	`// Create a bool vector of processed point indices, and initialize it to false`
`102`	`104`	`std::vector<bool> processed (cloud.size (), false);`
`@@ -124,7 +126,7 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSm`
`124`	`126`	`continue;`
`125`	`127`	`}`
`126`	`128`
`127`		`- for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx`
	`129`	`+ for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)`
`128`	`130`	`{`
`129`	`131`	`if (processed[nn_indices[j]]) // Has this point been processed before ?`
`130`	`132`	`continue;`
Original file line number	Diff line number	Diff line change
`@@ -61,6 +61,8 @@ inline void extractEuclideanClustersSmooth(const typename pcl::PointCloud<PointT`
`61`	`61`	`PCL_ERROR("[pcl::extractEuclideanClusters] Number of points in the input point cloud different than normals!\n");`
`62`	`62`	`return;`
`63`	`63`	`}`
	`64`	`+ // If tree gives sorted results, we can skip the first one because it is the query point itself`
	`65`	`+ const std::size_t nn_start_idx = tree->getSortedResults () ? 1 : 0;`
`64`	`66`
`65`	`67`	`// Create a bool vector of processed point indices, and initialize it to false`
`66`	`68`	`std::vector<bool> processed (cloud.size (), false);`
`@@ -96,7 +98,7 @@ inline void extractEuclideanClustersSmooth(const typename pcl::PointCloud<PointT`
`96`	`98`	`continue;`
`97`	`99`	`}`
`98`	`100`
`99`		`- for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx`
	`101`	`+ for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)`
`100`	`102`	`{`
`101`	`103`	`if (processed[nn_indices[j]]) // Has this point been processed before ?`
`102`	`104`	`continue;`
Original file line number	Diff line number	Diff line change
`@@ -123,6 +123,8 @@ namespace pcl`
`123`	`123`	`static_cast<std::size_t>(normals.size()));`
`124`	`124`	`return;`
`125`	`125`	`}`
	`126`	`+ // If tree gives sorted results, we can skip the first one because it is the query point itself`
	`127`	`+ const std::size_t nn_start_idx = tree->getSortedResults () ? 1 : 0;`
`126`	`128`	`const double cos_eps_angle = std::cos (eps_angle); // compute this once instead of acos many times (faster)`
`127`	`129`
`128`	`130`	`// Create a bool vector of processed point indices, and initialize it to false`
`@@ -151,7 +153,7 @@ namespace pcl`
`151`	`153`	`continue;`
`152`	`154`	`}`
`153`	`155`
`154`		`- for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx`
	`156`	`+ for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)`
`155`	`157`	`{`
`156`	`158`	`if (processed[nn_indices[j]]) // Has this point been processed before ?`
`157`	`159`	`continue;`
`@@ -243,6 +245,8 @@ namespace pcl`
`243`	`245`	`static_cast<std::size_t>(normals.size()));`
`244`	`246`	`return;`
`245`	`247`	`}`
	`248`	`+ // If tree gives sorted results, we can skip the first one because it is the query point itself`
	`249`	`+ const std::size_t nn_start_idx = tree->getSortedResults () ? 1 : 0;`
`246`	`250`	`const double cos_eps_angle = std::cos (eps_angle); // compute this once instead of acos many times (faster)`
`247`	`251`	`// Create a bool vector of processed point indices, and initialize it to false`
`248`	`252`	`std::vector<bool> processed (cloud.size (), false);`
`@@ -270,7 +274,7 @@ namespace pcl`
`270`	`274`	`continue;`
`271`	`275`	`}`
`272`	`276`
`273`		`- for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx`
	`277`	`+ for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)`
`274`	`278`	`{`
`275`	`279`	`if (processed[nn_indices[j]]) // Has this point been processed before ?`
`276`	`280`	`continue;`
Original file line number	Diff line number	Diff line change
`@@ -60,11 +60,13 @@ pcl::ConditionalEuclideanClustering<PointT>::segment (pcl::IndicesClusters &clus`
`60`	`60`	`if (!searcher_)`
`61`	`61`	`{`
`62`	`62`	`if (input_->isOrganized ())`
`63`		`- searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ());`
	`63`	`+ searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> (false)); // not requiring sorted results is much faster`
`64`	`64`	`else`
`65`		`- searcher_.reset (new pcl::search::KdTree<PointT> ());`
	`65`	`+ searcher_.reset (new pcl::search::KdTree<PointT> (false)); // not requiring sorted results is much faster`
`66`	`66`	`}`
`67`	`67`	`searcher_->setInputCloud (input_, indices_);`
	`68`	`+ // If searcher_ gives sorted results, we can skip the first one because it is the query point itself`
	`69`	`+ const int nn_start_idx = searcher_->getSortedResults () ? 1 : 0;`
`68`	`70`
`69`	`71`	`// Temp variables used by search class`
`70`	`72`	`Indices nn_indices;`
`@@ -100,7 +102,7 @@ pcl::ConditionalEuclideanClustering<PointT>::segment (pcl::IndicesClusters &clus`
`100`	`102`	`}`
`101`	`103`
`102`	`104`	`// Process the neighbors`
`103`		`- for (int nii = 1; nii < static_cast<int> (nn_indices.size ()); ++nii) // nii = neighbor indices iterator`
	`105`	`+ for (int nii = nn_start_idx; nii < static_cast<int> (nn_indices.size ()); ++nii) // nii = neighbor indices iterator`
`104`	`106`	`{`
`105`	`107`	`// Has this point been processed before?`
`106`	`108`	`if (nn_indices[nii] == UNAVAILABLE \|\| processed[nn_indices[nii]])`
Original file line number	Diff line number	Diff line change
`@@ -57,6 +57,8 @@ pcl::extractLabeledEuclideanClusters(`
`57`	`57`	`cloud.size());`
`58`	`58`	`return;`
`59`	`59`	`}`
	`60`	`+ // If tree gives sorted results, we can skip the first one because it is the query point itself`
	`61`	`+ const std::size_t nn_start_idx = tree->getSortedResults () ? 1 : 0;`
`60`	`62`	`// Create a bool vector of processed point indices, and initialize it to false`
`61`	`63`	`std::vector<bool> processed(cloud.size(), false);`
`62`	`64`
`@@ -88,8 +90,7 @@ pcl::extractLabeledEuclideanClusters(`
`88`	`90`	`continue;`
`89`	`91`	`}`
`90`	`92`
`91`		`- for (std::size_t j = 1; j < nn_indices.size();`
`92`		`- ++j) // nn_indices[0] should be sq_idx`
	`93`	`+ for (std::size_t j = nn_start_idx; j < nn_indices.size(); ++j)`
`93`	`94`	`{`
`94`	`95`	`if (processed[nn_indices[j]]) // Has this point been processed before ?`
`95`	`96`	`continue;`
Original file line number	Diff line number	Diff line change
`@@ -61,6 +61,8 @@ pcl::seededHueSegmentation (const PointCloud<PointXYZRGB> &cloud,`
`61`	`61`	`static_cast<std::size_t>(cloud.size()));`
`62`	`62`	`return;`
`63`	`63`	`}`
	`64`	`+ // If tree gives sorted results, we can skip the first one because it is the query point itself`
	`65`	`+ const std::size_t nn_start_idx = tree->getSortedResults () ? 1 : 0;`
`64`	`66`	`// Create a bool vector of processed point indices, and initialize it to false`
`65`	`67`	`std::vector<bool> processed (cloud.size (), false);`
`66`	`68`
`@@ -96,7 +98,7 @@ pcl::seededHueSegmentation (const PointCloud<PointXYZRGB> &cloud,`
`96`	`98`	`continue;`
`97`	`99`	`}`
`98`	`100`
`99`		`- for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx`
	`101`	`+ for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)`
`100`	`102`	`{`
`101`	`103`	`if (processed[nn_indices[j]]) // Has this point been processed before ?`
`102`	`104`	`continue;`
`@@ -139,6 +141,8 @@ pcl::seededHueSegmentation (const PointCloud<PointXYZRGB> &cloud,`
`139`	`141`	`static_cast<std::size_t>(cloud.size()));`
`140`	`142`	`return;`
`141`	`143`	`}`
	`144`	`+ // If tree gives sorted results, we can skip the first one because it is the query point itself`
	`145`	`+ const std::size_t nn_start_idx = tree->getSortedResults () ? 1 : 0;`
`142`	`146`	`// Create a bool vector of processed point indices, and initialize it to false`
`143`	`147`	`std::vector<bool> processed (cloud.size (), false);`
`144`	`148`
`@@ -173,7 +177,7 @@ pcl::seededHueSegmentation (const PointCloud<PointXYZRGB> &cloud,`
`173`	`177`	`sq_idx++;`
`174`	`178`	`continue;`
`175`	`179`	`}`
`176`		`- for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx`
	`180`	`+ for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)`
`177`	`181`	`{`
`178`	`182`	`if (processed[nn_indices[j]]) // Has this point been processed before ?`
`179`	`183`	`continue;`