From 1a469fc9ac2944cd5f8209aec2739de58f04389a Mon Sep 17 00:00:00 2001 From: Markus Vieth Date: Sun, 12 Oct 2025 21:18:49 +0200 Subject: [PATCH 1/3] More further towards KdTreeNanoflann Extend `autoSelectMethod` to types that do not have xyz-coordinates (i.e. features types) --- common/include/pcl/impl/point_types.hpp | 1 + common/include/pcl/point_representation.h | 42 ++++++++++++++----- .../ground_based_people_detection_app.hpp | 3 -- .../include/pcl/registration/ia_fpcs.h | 2 +- .../include/pcl/registration/ia_ransac.h | 7 +--- .../include/pcl/registration/impl/ia_fpcs.hpp | 7 +++- .../pcl/registration/impl/ia_ransac.hpp | 4 +- .../impl/sample_consensus_prerejective.hpp | 5 ++- .../sample_consensus_prerejective.h | 3 +- search/include/pcl/search/impl/auto.hpp | 21 ++++++---- search/src/auto.cpp | 2 +- test/common/test_common.cpp | 10 +++++ tools/boundary_estimation.cpp | 1 - tools/compute_hausdorff.cpp | 12 +++--- tools/pcd_viewer.cpp | 8 ++-- 15 files changed, 82 insertions(+), 46 deletions(-) diff --git a/common/include/pcl/impl/point_types.hpp b/common/include/pcl/impl/point_types.hpp index c12552f8463..0f626f03652 100644 --- a/common/include/pcl/impl/point_types.hpp +++ b/common/include/pcl/impl/point_types.hpp @@ -173,6 +173,7 @@ namespace pcl template<> struct descriptorSize { static constexpr const int value = 7992; }; template<> struct descriptorSize { static constexpr const int value = 16; }; template<> struct descriptorSize { static constexpr const int value = 36; }; + template<> struct descriptorSize { static constexpr const int value = 12; }; template struct descriptorSize> { static constexpr const int value = N; }; diff --git a/common/include/pcl/point_representation.h b/common/include/pcl/point_representation.h index 17b2a0d422c..bff480386cf 100644 --- a/common/include/pcl/point_representation.h +++ b/common/include/pcl/point_representation.h @@ -204,6 +204,8 @@ namespace pcl using Ptr = shared_ptr >; using ConstPtr = shared_ptr >; + static constexpr const std::int32_t NR_DIMS = std::min(sizeof (PointDefault) / sizeof (float), 3); + DefaultPointRepresentation () { // If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions @@ -312,6 +314,8 @@ namespace pcl using ConstPtr = shared_ptr>; using FieldList = typename pcl::traits::fieldList::type; + static constexpr const std::int32_t NR_DIMS = pcl::detail::traits::descriptorSize_v; + DefaultFeatureRepresentation () { nr_dimensions_ = 0; // zero-out the nr_dimensions_ before it gets incremented @@ -336,9 +340,11 @@ namespace pcl class DefaultPointRepresentation : public PointRepresentation { public: + static constexpr const std::int32_t NR_DIMS = 3; + DefaultPointRepresentation () { - nr_dimensions_ = 3; + nr_dimensions_ = NR_DIMS; trivial_ = true; } @@ -356,9 +362,11 @@ namespace pcl class DefaultPointRepresentation : public PointRepresentation { public: + static constexpr const std::int32_t NR_DIMS = 3; + DefaultPointRepresentation () { - nr_dimensions_ = 3; + nr_dimensions_ = NR_DIMS; trivial_ = true; } @@ -377,9 +385,11 @@ namespace pcl class DefaultPointRepresentation : public PointRepresentation { public: + static constexpr const std::int32_t NR_DIMS = 3; + DefaultPointRepresentation () { - nr_dimensions_ = 3; + nr_dimensions_ = NR_DIMS; trivial_ = true; } @@ -404,12 +414,14 @@ namespace pcl ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template <> - class DefaultPointRepresentation : public DefaultFeatureRepresentation + class DefaultPointRepresentation : public PointRepresentation { public: + static constexpr const std::int32_t NR_DIMS = 4; + DefaultPointRepresentation () { - nr_dimensions_ = 4; + nr_dimensions_ = NR_DIMS; trivial_ = true; } @@ -453,9 +465,11 @@ namespace pcl class DefaultPointRepresentation : public PointRepresentation { public: + static constexpr const std::int32_t NR_DIMS = 36; + DefaultPointRepresentation () { - nr_dimensions_ = 36; + nr_dimensions_ = NR_DIMS; trivial_=false; } @@ -476,9 +490,11 @@ namespace pcl class DefaultPointRepresentation : public PointRepresentation { public: + static constexpr const std::int32_t NR_DIMS = 1980; + DefaultPointRepresentation () { - nr_dimensions_ = 1980; + nr_dimensions_ = NR_DIMS; } void @@ -494,9 +510,11 @@ namespace pcl class DefaultPointRepresentation : public PointRepresentation { public: + static constexpr const std::int32_t NR_DIMS = 1960; + DefaultPointRepresentation () { - nr_dimensions_ = 1960; + nr_dimensions_ = NR_DIMS; } void @@ -512,9 +530,11 @@ namespace pcl class DefaultPointRepresentation : public PointRepresentation { public: + static constexpr const std::int32_t NR_DIMS = 352; + DefaultPointRepresentation () { - nr_dimensions_ = 352; + nr_dimensions_ = NR_DIMS; } void @@ -530,9 +550,11 @@ namespace pcl class DefaultPointRepresentation : public PointRepresentation { public: + static constexpr const std::int32_t NR_DIMS = 1344; + DefaultPointRepresentation () { - nr_dimensions_ = 1344; + nr_dimensions_ = NR_DIMS; } void diff --git a/people/include/pcl/people/impl/ground_based_people_detection_app.hpp b/people/include/pcl/people/impl/ground_based_people_detection_app.hpp index 3fa4962955c..0cde6b81295 100644 --- a/people/include/pcl/people/impl/ground_based_people_detection_app.hpp +++ b/people/include/pcl/people/impl/ground_based_people_detection_app.hpp @@ -375,13 +375,10 @@ pcl::people::GroundBasedPeopleDetectionApp::compute (std::vector cluster_indices; - typename pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); - tree->setInputCloud(no_ground_cloud_); pcl::EuclideanClusterExtraction ec; ec.setClusterTolerance(2 * voxel_size_); ec.setMinClusterSize(min_points_); ec.setMaxClusterSize(max_points_); - ec.setSearchMethod(tree); ec.setInputCloud(no_ground_cloud_); ec.extract(cluster_indices); diff --git a/registration/include/pcl/registration/ia_fpcs.h b/registration/include/pcl/registration/ia_fpcs.h index afb9f252805..a7cae129ddf 100644 --- a/registration/include/pcl/registration/ia_fpcs.h +++ b/registration/include/pcl/registration/ia_fpcs.h @@ -86,7 +86,7 @@ class FPCSInitialAlignment : public Registration>; - using KdTreeReciprocal = pcl::search::KdTree; + using KdTreeReciprocal = pcl::search::Search; using KdTreeReciprocalPtr = typename KdTreeReciprocal::Ptr; using PointCloudTarget = pcl::PointCloud; diff --git a/registration/include/pcl/registration/ia_ransac.h b/registration/include/pcl/registration/ia_ransac.h index 10fc7ec8b43..9c3e13a4591 100644 --- a/registration/include/pcl/registration/ia_ransac.h +++ b/registration/include/pcl/registration/ia_ransac.h @@ -137,13 +137,10 @@ class SampleConsensusInitialAlignment : public Registration::Ptr; + using FeatureKdTreePtr = typename pcl::search::Search::Ptr; /** \brief Constructor. */ SampleConsensusInitialAlignment() - : input_features_() - , target_features_() - , feature_tree_(new pcl::KdTreeFLANN) - , error_functor_() + : input_features_(), target_features_(), error_functor_() { reg_name_ = "SampleConsensusInitialAlignment"; max_iterations_ = 1000; diff --git a/registration/include/pcl/registration/impl/ia_fpcs.hpp b/registration/include/pcl/registration/impl/ia_fpcs.hpp index 395a7f102e7..0b01484e0c9 100644 --- a/registration/include/pcl/registration/impl/ia_fpcs.hpp +++ b/registration/include/pcl/registration/impl/ia_fpcs.hpp @@ -671,8 +671,11 @@ pcl::registration::FPCSInitialAlignmentsetInputCloud(cloud_e); + KdTreeReciprocalPtr tree_e(pcl::search::autoSelectMethod( + cloud_e, + true, + pcl::search::Purpose::radius_search)); // maybe check again if results do not have + // to be sorted pcl::Indices ids; std::vector dists_sqr; diff --git a/registration/include/pcl/registration/impl/ia_ransac.hpp b/registration/include/pcl/registration/impl/ia_ransac.hpp index 77f3cdc52dc..a4d7bee6658 100644 --- a/registration/include/pcl/registration/impl/ia_ransac.hpp +++ b/registration/include/pcl/registration/impl/ia_ransac.hpp @@ -42,6 +42,7 @@ #define IA_RANSAC_HPP_ #include +#include // for autoSelectMethod namespace pcl { @@ -71,7 +72,8 @@ SampleConsensusInitialAlignment::setTargetFe return; } target_features_ = features; - feature_tree_->setInputCloud(target_features_); + feature_tree_.reset(pcl::search::autoSelectMethod( + target_features_, false, pcl::search::Purpose::many_knn_search)); } template diff --git a/registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp b/registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp index 5a84f870489..a26cef95cb3 100644 --- a/registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp +++ b/registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp @@ -41,6 +41,8 @@ #ifndef PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_ #define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_ +#include // for autoSelectMethod + namespace pcl { template @@ -69,7 +71,8 @@ SampleConsensusPrerejective::setTargetFeatur return; } target_features_ = features; - feature_tree_->setInputCloud(target_features_); + feature_tree_.reset(pcl::search::autoSelectMethod( + target_features_, false, pcl::search::Purpose::many_knn_search)); } template diff --git a/registration/include/pcl/registration/sample_consensus_prerejective.h b/registration/include/pcl/registration/sample_consensus_prerejective.h index 3d67b293092..e8fcb95c414 100644 --- a/registration/include/pcl/registration/sample_consensus_prerejective.h +++ b/registration/include/pcl/registration/sample_consensus_prerejective.h @@ -109,7 +109,7 @@ class SampleConsensusPrerejective : public Registration>; - using FeatureKdTreePtr = typename KdTreeFLANN::Ptr; + using FeatureKdTreePtr = typename pcl::search::Search::Ptr; using CorrespondenceRejectorPoly = pcl::registration::CorrespondenceRejectorPoly; @@ -121,7 +121,6 @@ class SampleConsensusPrerejective : public Registration) , correspondence_rejector_poly_(new CorrespondenceRejectorPoly) { reg_name_ = "SampleConsensusPrerejective"; diff --git a/search/include/pcl/search/impl/auto.hpp b/search/include/pcl/search/impl/auto.hpp index 62ef2313060..ace5aafc3e9 100644 --- a/search/include/pcl/search/impl/auto.hpp +++ b/search/include/pcl/search/impl/auto.hpp @@ -20,14 +20,17 @@ template pcl::search::Search * pcl::search::autoSelectMethod(const typename pcl::PointCloud::ConstPtr& cloud, const pcl::IndicesConstPtr& indices, bool sorted_results, pcl::search::Purpose purpose) { pcl::search::Search * searcher = nullptr; - if (cloud->isOrganized ()) { - searcher = new pcl::search::OrganizedNeighbor (sorted_results); - if(searcher->setInputCloud (cloud, indices)) { // may return false if OrganizedNeighbor cannot work with the cloud, then use another search method instead - return searcher; + if constexpr (pcl::traits::has_xyz_v) { + if (cloud->isOrganized ()) { + searcher = new pcl::search::OrganizedNeighbor (sorted_results); + if(searcher->setInputCloud (cloud, indices)) { // may return false if OrganizedNeighbor cannot work with the cloud, then use another search method instead + return searcher; + } } } #if PCL_HAS_NANOFLANN - searcher = new pcl::search::KdTreeNanoflann (sorted_results, (purpose == pcl::search::Purpose::one_knn_search ? 10 : 20)); + // we get the number of search dimensions as a compile-time-constant via NR_DIMS. NR_DIMS may be -1 if it is not possible to determine the dimensions at compile-time (only at run-time), however then searching may be slower. If NR_DIMS is not -1, it must be the same as the return value of getNumberOfDimensions(). + searcher = new pcl::search::KdTreeNanoflann::NR_DIMS> (sorted_results, (purpose == pcl::search::Purpose::one_knn_search ? 10 : 20)); if(searcher->setInputCloud (cloud, indices)) { return searcher; } @@ -40,9 +43,11 @@ pcl::search::Search * pcl::search::autoSelectMethod(const typename pcl:: return searcher; } #endif - // If nothing else works, use brute force method - searcher = new pcl::search::BruteForce (sorted_results); - searcher->setInputCloud (cloud, indices); + // If nothing else works, and the point type has xyz coordinates, use brute force method + if constexpr (pcl::traits::has_xyz_v) { + searcher = new pcl::search::BruteForce (sorted_results); + searcher->setInputCloud (cloud, indices); + } return searcher; } diff --git a/search/src/auto.cpp b/search/src/auto.cpp index ed11c929067..4b1380f20e2 100644 --- a/search/src/auto.cpp +++ b/search/src/auto.cpp @@ -12,6 +12,6 @@ #ifndef PCL_NO_PRECOMPILE #include #include -PCL_INSTANTIATE(AutoSelectMethod, PCL_XYZ_POINT_TYPES) +PCL_INSTANTIATE(AutoSelectMethod, PCL_POINT_TYPES) #endif // PCL_NO_PRECOMPILE diff --git a/test/common/test_common.cpp b/test/common/test_common.cpp index c8ff786a119..6138756c0d0 100644 --- a/test/common/test_common.cpp +++ b/test/common/test_common.cpp @@ -50,6 +50,7 @@ #include #include #include // for isFinite +#include using namespace pcl; @@ -520,6 +521,15 @@ TEST (PCL, computeMedian) EXPECT_EQ(median5, 5.5); } +template class PointRepresentationTest : public ::testing::Test { }; +using PointRepresentationTestTypes = ::testing::Types; +TYPED_TEST_SUITE(PointRepresentationTest, PointRepresentationTestTypes); +TYPED_TEST(PointRepresentationTest, GetNormalVectorXfMap) +{ + pcl::DefaultPointRepresentation point_representation; + EXPECT_EQ (pcl::DefaultPointRepresentation::NR_DIMS, point_representation.getNumberOfDimensions()); +} + /* ---[ */ int main (int argc, char** argv) diff --git a/tools/boundary_estimation.cpp b/tools/boundary_estimation.cpp index 1bf275e65e5..581aeed78c9 100644 --- a/tools/boundary_estimation.cpp +++ b/tools/boundary_estimation.cpp @@ -107,7 +107,6 @@ compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output BoundaryEstimation ne; ne.setInputCloud (xyznormals); ne.setInputNormals (xyznormals); - //ne.setSearchMethod (pcl::KdTreeFLANN::Ptr (new pcl::KdTreeFLANN)); ne.setKSearch (k); ne.setAngleThreshold (static_cast (angle)); ne.setRadiusSearch (radius); diff --git a/tools/compute_hausdorff.cpp b/tools/compute_hausdorff.cpp index e9712a6f825..186f6d839b0 100644 --- a/tools/compute_hausdorff.cpp +++ b/tools/compute_hausdorff.cpp @@ -43,7 +43,7 @@ #include #include #include -#include +#include // for autoSelectMethod using namespace pcl; using namespace pcl::io; @@ -84,29 +84,27 @@ compute (const Cloud::ConstPtr &cloud_a, const Cloud::ConstPtr &cloud_b) print_highlight (stderr, "Computing "); // compare A to B - pcl::search::KdTree tree_b; - tree_b.setInputCloud (cloud_b); + pcl::search::Search::Ptr tree_b(pcl::search::autoSelectMethod(cloud_b, false, pcl::search::Purpose::one_knn_search)); float max_dist_a = -std::numeric_limits::max (); for (const auto &point : (*cloud_a)) { pcl::Indices indices (1); std::vector sqr_distances (1); - tree_b.nearestKSearch (point, 1, indices, sqr_distances); + tree_b->nearestKSearch (point, 1, indices, sqr_distances); if (sqr_distances[0] > max_dist_a) max_dist_a = sqr_distances[0]; } // compare B to A - pcl::search::KdTree tree_a; - tree_a.setInputCloud (cloud_a); + pcl::search::Search::Ptr tree_a(pcl::search::autoSelectMethod(cloud_a, false, pcl::search::Purpose::one_knn_search)); float max_dist_b = -std::numeric_limits::max (); for (const auto &point : (*cloud_b)) { pcl::Indices indices (1); std::vector sqr_distances (1); - tree_a.nearestKSearch (point, 1, indices, sqr_distances); + tree_a->nearestKSearch (point, 1, indices, sqr_distances); if (sqr_distances[0] > max_dist_b) max_dist_b = sqr_distances[0]; } diff --git a/tools/pcd_viewer.cpp b/tools/pcd_viewer.cpp index f2096c4672d..00060c10f35 100644 --- a/tools/pcd_viewer.cpp +++ b/tools/pcd_viewer.cpp @@ -52,7 +52,7 @@ #include #include #include -#include +#include // for autoSelectMethod #include using namespace std::chrono_literals; @@ -150,7 +150,7 @@ printHelp (int, char **argv) pcl::visualization::PCLPlotter ph_global; pcl::visualization::PCLVisualizer::Ptr p; std::vector imgs; -pcl::search::KdTree search; +pcl::search::Search::Ptr search; pcl::PCLPointCloud2::Ptr cloud; pcl::PointCloud::Ptr xyzcloud; @@ -178,7 +178,7 @@ pp_callback (const pcl::visualization::PointPickingEvent& event, void* cookie) cloud = *reinterpret_cast (cookie); xyzcloud.reset (new pcl::PointCloud); pcl::fromPCLPointCloud2 (*cloud, *xyzcloud); - search.setInputCloud (xyzcloud); + search.reset(pcl::search::autoSelectMethod(xyzcloud, false, pcl::search::Purpose::one_knn_search)); } // Return the correct index in the cloud instead of the index on the screen pcl::Indices indices (1); @@ -187,7 +187,7 @@ pp_callback (const pcl::visualization::PointPickingEvent& event, void* cookie) // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point pcl::PointXYZ picked_pt; event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z); - search.nearestKSearch (picked_pt, 1, indices, distances); + search->nearestKSearch (picked_pt, 1, indices, distances); PCL_INFO ("Point index picked: %d (real: %d) - [%f, %f, %f]\n", idx, indices[0], picked_pt.x, picked_pt.y, picked_pt.z); From f43e1c6bfc48fdde6c5e44d41dfcaa314aef6352 Mon Sep 17 00:00:00 2001 From: Markus Vieth Date: Wed, 11 Mar 2026 17:20:10 +0100 Subject: [PATCH 2/3] Changes per review --- search/include/pcl/search/impl/auto.hpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/search/include/pcl/search/impl/auto.hpp b/search/include/pcl/search/impl/auto.hpp index ace5aafc3e9..5ae92a9293b 100644 --- a/search/include/pcl/search/impl/auto.hpp +++ b/search/include/pcl/search/impl/auto.hpp @@ -26,6 +26,7 @@ pcl::search::Search * pcl::search::autoSelectMethod(const typename pcl:: if(searcher->setInputCloud (cloud, indices)) { // may return false if OrganizedNeighbor cannot work with the cloud, then use another search method instead return searcher; } + delete searcher; } } #if PCL_HAS_NANOFLANN @@ -34,6 +35,7 @@ pcl::search::Search * pcl::search::autoSelectMethod(const typename pcl:: if(searcher->setInputCloud (cloud, indices)) { return searcher; } + delete searcher; #else pcl::utils::ignore(purpose); #endif @@ -42,13 +44,16 @@ pcl::search::Search * pcl::search::autoSelectMethod(const typename pcl:: if(searcher->setInputCloud (cloud, indices)) { return searcher; } + delete searcher; #endif // If nothing else works, and the point type has xyz coordinates, use brute force method if constexpr (pcl::traits::has_xyz_v) { searcher = new pcl::search::BruteForce (sorted_results); searcher->setInputCloud (cloud, indices); + return searcher; } - return searcher; + PCL_ERROR("[pcl::search::autoSelectMethod] No suitable method found. Make sure you have nanoflann and/or FLANN installed.\n"); + return nullptr; } #define PCL_INSTANTIATE_AutoSelectMethod(T) template PCL_EXPORTS pcl::search::Search * pcl::search::autoSelectMethod(const typename pcl::PointCloud::ConstPtr& cloud, const pcl::IndicesConstPtr& indices, bool sorted_results, pcl::search::Purpose purpose); From 6534e3f31a6859519afbdd466b2560096895be33 Mon Sep 17 00:00:00 2001 From: Markus Vieth Date: Wed, 11 Mar 2026 17:53:16 +0100 Subject: [PATCH 3/3] Add octree to autoSelectMethod --- search/include/pcl/search/impl/auto.hpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/search/include/pcl/search/impl/auto.hpp b/search/include/pcl/search/impl/auto.hpp index 5ae92a9293b..f6748f4ebf3 100644 --- a/search/include/pcl/search/impl/auto.hpp +++ b/search/include/pcl/search/impl/auto.hpp @@ -15,6 +15,7 @@ #include #include #include +#include #include template @@ -29,6 +30,7 @@ pcl::search::Search * pcl::search::autoSelectMethod(const typename pcl:: delete searcher; } } + #if PCL_HAS_NANOFLANN // we get the number of search dimensions as a compile-time-constant via NR_DIMS. NR_DIMS may be -1 if it is not possible to determine the dimensions at compile-time (only at run-time), however then searching may be slower. If NR_DIMS is not -1, it must be the same as the return value of getNumberOfDimensions(). searcher = new pcl::search::KdTreeNanoflann::NR_DIMS> (sorted_results, (purpose == pcl::search::Purpose::one_knn_search ? 10 : 20)); @@ -39,6 +41,7 @@ pcl::search::Search * pcl::search::autoSelectMethod(const typename pcl:: #else pcl::utils::ignore(purpose); #endif + #if PCL_HAS_FLANN searcher = new pcl::search::KdTree (sorted_results); if(searcher->setInputCloud (cloud, indices)) { @@ -46,6 +49,16 @@ pcl::search::Search * pcl::search::autoSelectMethod(const typename pcl:: } delete searcher; #endif + + if constexpr (pcl::traits::has_xyz_v) { + searcher = new pcl::search::Octree (0.01); // TODO a better heuristic to choose octree resolution? + searcher->setSortedResults (sorted_results); + if(searcher->setInputCloud (cloud, indices)) { + return searcher; + } + delete searcher; + } + // If nothing else works, and the point type has xyz coordinates, use brute force method if constexpr (pcl::traits::has_xyz_v) { searcher = new pcl::search::BruteForce (sorted_results);