From cc9bcfcdde078dec86c137b99057e63c7adea44d Mon Sep 17 00:00:00 2001 From: Markus Vieth Date: Tue, 2 Apr 2024 18:28:09 +0200 Subject: [PATCH] Only skip first result of radiusSearch if results are sorted The same was done for `extractEuclideanClusters` years ago: https://github.com/PointCloudLibrary/pcl/pull/109 For ConditionalEuclideanClustering, I tested whether it is faster to require sorted results (and skip first entry), or not (and iterate over all results). The second option is much faster (took roughly 2/3 of the time of the first option in my test) --- features/include/pcl/features/impl/cvfh.hpp | 5 +++-- features/include/pcl/features/impl/our_cvfh.hpp | 4 +++- kdtree/include/pcl/kdtree/kdtree.h | 9 +++++++++ recognition/include/pcl/recognition/impl/hv/hv_go.hpp | 4 +++- segmentation/include/pcl/segmentation/extract_clusters.h | 8 ++++++-- .../impl/conditional_euclidean_clustering.hpp | 8 +++++--- .../pcl/segmentation/impl/extract_labeled_clusters.hpp | 5 +++-- .../pcl/segmentation/impl/seeded_hue_segmentation.hpp | 8 ++++++-- 8 files changed, 38 insertions(+), 13 deletions(-) diff --git a/features/include/pcl/features/impl/cvfh.hpp b/features/include/pcl/features/impl/cvfh.hpp index d5df1e6551b..de4fbc17eb3 100644 --- a/features/include/pcl/features/impl/cvfh.hpp +++ b/features/include/pcl/features/impl/cvfh.hpp @@ -96,6 +96,8 @@ pcl::CVFHEstimation::extractEuclideanClustersSmoot static_cast(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 processed (cloud.size (), false); @@ -124,8 +126,7 @@ pcl::CVFHEstimation::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 index 34caf74c615..b0f0bc01a2e 100644 --- a/features/include/pcl/features/impl/our_cvfh.hpp +++ b/features/include/pcl/features/impl/our_cvfh.hpp @@ -97,6 +97,8 @@ pcl::OURCVFHEstimation::extractEuclideanClustersSm static_cast(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 processed (cloud.size (), false); @@ -124,7 +126,7 @@ pcl::OURCVFHEstimation::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 index fd5e357fb09..5638c76e3e6 100644 --- 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 index e4d6ceee372..3db2df822db 100644 --- 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::PointCloudgetSortedResults () ? 1 : 0; // Create a bool vector of processed point indices, and initialize it to false std::vector processed (cloud.size (), false); @@ -96,7 +98,7 @@ inline void extractEuclideanClustersSmooth(const typename pcl::PointCloud(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(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 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 index 93230268017..5eef5e59919 100644 --- 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::segment (pcl::IndicesClusters &clus if (!searcher_) { if (input_->isOrganized ()) - searcher_.reset (new pcl::search::OrganizedNeighbor ()); + searcher_.reset (new pcl::search::OrganizedNeighbor (false)); // not requiring sorted results is much faster else - searcher_.reset (new pcl::search::KdTree ()); + searcher_.reset (new pcl::search::KdTree (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::segment (pcl::IndicesClusters &clus } // Process the neighbors - for (int nii = 1; nii < static_cast (nn_indices.size ()); ++nii) // nii = neighbor indices iterator + for (int nii = nn_start_idx; nii < static_cast (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 index 1f6ea1d88d1..37ed0d69435 100644 --- 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 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 index 3976c143e2d..1c79547d966 100644 --- 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 &cloud, static_cast(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 processed (cloud.size (), false); @@ -96,7 +98,7 @@ pcl::seededHueSegmentation (const PointCloud &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 &cloud, static_cast(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 processed (cloud.size (), false); @@ -173,7 +177,7 @@ pcl::seededHueSegmentation (const PointCloud &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;