From a0a6b5c13fb903cb767983187275873aaacf25fd Mon Sep 17 00:00:00 2001 From: jeevan6996 Date: Mon, 3 Aug 2026 23:16:48 +0100 Subject: [PATCH 1/2] Extend BruteForce search to non-XYZ point types --- search/include/pcl/search/brute_force.h | 52 ++++++ search/include/pcl/search/impl/auto.hpp | 11 +- .../include/pcl/search/impl/brute_force.hpp | 52 +++++- search/src/brute_force.cpp | 2 +- test/search/CMakeLists.txt | 4 + test/search/test_brute_force.cpp | 158 ++++++++++++++++++ 6 files changed, 261 insertions(+), 18 deletions(-) create mode 100644 test/search/test_brute_force.cpp diff --git a/search/include/pcl/search/brute_force.h b/search/include/pcl/search/brute_force.h index 0a7a610f7cc..dbb53abbf7d 100644 --- a/search/include/pcl/search/brute_force.h +++ b/search/include/pcl/search/brute_force.h @@ -37,12 +37,39 @@ #pragma once +#include #include +#include + namespace pcl { namespace search { + namespace detail + { + template + struct has_descriptor_size : std::false_type + { + }; + + template + struct has_descriptor_size::value)>> + : std::true_type + { + }; + + template + typename PointRepresentation::ConstPtr + makeDefaultPointRepresentation () + { + if constexpr (!pcl::traits::has_xyz_v && has_descriptor_size::value) + return (pcl::make_shared> ()); + else + return (pcl::make_shared> ()); + } + } // namespace detail + /** \brief Implementation of a simple brute force search algorithm. * \author Suat Gedikli * \ingroup search @@ -52,6 +79,7 @@ namespace pcl { using PointCloud = typename Search::PointCloud; using PointCloudConstPtr = typename Search::PointCloudConstPtr; + using PointRepresentationConstPtr = typename PointRepresentation::ConstPtr; using IndicesPtr = pcl::IndicesPtr; using IndicesConstPtr = pcl::IndicesConstPtr; @@ -83,9 +111,14 @@ namespace pcl // replace by some metric functor float getDistSqr (const PointT& point1, const PointT& point2) const; + + bool + isValidPoint (const PointT& point) const; + public: BruteForce (bool sorted_results = false) : Search ("BruteForce", sorted_results) + , point_representation_ (detail::makeDefaultPointRepresentation ()) { } @@ -93,6 +126,23 @@ namespace pcl ~BruteForce () override = default; + /** \brief Provide a pointer to the point representation used for converting + * points into k-D vectors. + * \param[in] point_representation the const shared pointer to a PointRepresentation + */ + inline void + setPointRepresentation (const PointRepresentationConstPtr &point_representation) + { + point_representation_ = point_representation; + } + + /** \brief Get the point representation used for converting points into k-D vectors. */ + inline PointRepresentationConstPtr + getPointRepresentation () const + { + return (point_representation_); + } + /** \brief Search for the k-nearest neighbors for the given query point. * \param[in] point the given query point * \param[in] k the number of neighbors to search for @@ -135,6 +185,8 @@ namespace pcl sparseRadiusSearch (const PointT& point, double radius, Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const; + + PointRepresentationConstPtr point_representation_; }; } } diff --git a/search/include/pcl/search/impl/auto.hpp b/search/include/pcl/search/impl/auto.hpp index f6748f4ebf3..3316f49ca1b 100644 --- a/search/include/pcl/search/impl/auto.hpp +++ b/search/include/pcl/search/impl/auto.hpp @@ -59,14 +59,9 @@ pcl::search::Search * pcl::search::autoSelectMethod(const typename pcl:: 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); - searcher->setInputCloud (cloud, indices); - return searcher; - } - PCL_ERROR("[pcl::search::autoSelectMethod] No suitable method found. Make sure you have nanoflann and/or FLANN installed.\n"); - return nullptr; + searcher = new pcl::search::BruteForce (sorted_results); + searcher->setInputCloud (cloud, indices); + return searcher; } #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); diff --git a/search/include/pcl/search/impl/brute_force.hpp b/search/include/pcl/search/impl/brute_force.hpp index dcd2e8dcc72..151cdb098b2 100644 --- a/search/include/pcl/search/impl/brute_force.hpp +++ b/search/include/pcl/search/impl/brute_force.hpp @@ -40,13 +40,43 @@ #include // for pcl::isFinite #include #include +#include ////////////////////////////////////////////////////////////////////////////////////////////// template float pcl::search::BruteForce::getDistSqr ( const PointT& point1, const PointT& point2) const { - return (point1.getVector3fMap () - point2.getVector3fMap ()).squaredNorm (); + if constexpr (pcl::traits::has_xyz_v) + { + if (point_representation_->isTrivial () && + point_representation_->getNumberOfDimensions () == 3) + return (point1.getVector3fMap () - point2.getVector3fMap ()).squaredNorm (); + } + + const int nr_dimensions = point_representation_->getNumberOfDimensions (); + std::vector point1_representation (nr_dimensions); + std::vector point2_representation (nr_dimensions); + point_representation_->vectorize (point1, point1_representation); + point_representation_->vectorize (point2, point2_representation); + + float dist = 0.0f; + for (int i = 0; i < nr_dimensions; ++i) + { + const float diff = point1_representation[i] - point2_representation[i]; + dist += diff * diff; + } + return (dist); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::search::BruteForce::isValidPoint (const PointT& point) const +{ + if constexpr (pcl::traits::has_xyz_v) + return (pcl::isFinite (point)); + + return (point_representation_->isValid (point)); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -54,7 +84,7 @@ template int pcl::search::BruteForce::nearestKSearch ( const PointT& point, int k, Indices& k_indices, std::vector& k_distances) const { - assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + assert (isValidPoint (point) && "Invalid (NaN, Inf) point given to nearestKSearch!"); k_indices.clear (); k_distances.clear (); @@ -149,18 +179,20 @@ pcl::search::BruteForce::sparseKSearch ( auto iIt =indices_->cbegin (); for (; iIt != indices_->cend () && result.size () < static_cast (k); ++iIt) { - if (std::isfinite ((*input_)[*iIt].x)) + if (isValidPoint ((*input_)[*iIt])) result.push_back (Entry (*iIt, getDistSqr ((*input_)[*iIt], point))); } queue = std::priority_queue (result.begin (), result.end ()); + if (queue.empty ()) + return 0; // either we have k elements, or there are none left to iterate >in either case we're fine // add the rest Entry entry; for (; iIt != indices_->cend (); ++iIt) { - if (!std::isfinite ((*input_)[*iIt].x)) + if (!isValidPoint ((*input_)[*iIt])) continue; entry.distance = getDistSqr ((*input_)[*iIt], point); @@ -177,18 +209,20 @@ pcl::search::BruteForce::sparseKSearch ( Entry entry; for (entry.index = 0; (entry.index < static_cast(input_->size ())) && (result.size () < static_cast (k)); ++entry.index) { - if (std::isfinite ((*input_)[entry.index].x)) + if (isValidPoint ((*input_)[entry.index])) { entry.distance = getDistSqr ((*input_)[entry.index], point); result.push_back (entry); } } queue = std::priority_queue (result.begin (), result.end ()); + if (queue.empty ()) + return 0; // add the rest for (; entry.index < static_cast(input_->size ()); ++entry.index) { - if (!std::isfinite ((*input_)[entry.index].x)) + if (!isValidPoint ((*input_)[entry.index])) continue; entry.distance = getDistSqr ((*input_)[entry.index], point); @@ -293,7 +327,7 @@ pcl::search::BruteForce::sparseRadiusSearch ( { for (const auto& idx : *indices_) { - if (!std::isfinite ((*input_)[idx].x)) + if (!isValidPoint ((*input_)[idx])) continue; distance = getDistSqr ((*input_)[idx], point); @@ -310,7 +344,7 @@ pcl::search::BruteForce::sparseRadiusSearch ( { for (std::size_t index = 0; index < input_->size (); ++index) { - if (!std::isfinite ((*input_)[index].x)) + if (!isValidPoint ((*input_)[index])) continue; distance = getDistSqr ((*input_)[index], point); if (distance <= radius) @@ -335,7 +369,7 @@ pcl::search::BruteForce::radiusSearch ( const PointT& point, double radius, Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn) const { - assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + assert (isValidPoint (point) && "Invalid (NaN, Inf) point given to radiusSearch!"); k_indices.clear (); k_sqr_distances.clear (); diff --git a/search/src/brute_force.cpp b/search/src/brute_force.cpp index 76d48cf71fe..3cd29fc4740 100644 --- a/search/src/brute_force.cpp +++ b/search/src/brute_force.cpp @@ -39,4 +39,4 @@ #include // Instantiations of specific point types -PCL_INSTANTIATE (BruteForce, PCL_XYZ_POINT_TYPES) +PCL_INSTANTIATE (BruteForce, PCL_POINT_TYPES) diff --git a/test/search/CMakeLists.txt b/test/search/CMakeLists.txt index 3729072b075..51a14bf567e 100644 --- a/test/search/CMakeLists.txt +++ b/test/search/CMakeLists.txt @@ -14,6 +14,10 @@ PCL_ADD_TEST(kdtree_search test_kdtree_search FILES test_kdtree.cpp LINK_WITH pcl_gtest pcl_search pcl_kdtree) +PCL_ADD_TEST(brute_force_search test_brute_force_search + FILES test_brute_force.cpp + LINK_WITH pcl_gtest pcl_search pcl_common) + PCL_ADD_TEST(flann_search test_flann_search FILES test_flann_search.cpp LINK_WITH pcl_gtest pcl_search pcl_kdtree) diff --git a/test/search/test_brute_force.cpp b/test/search/test_brute_force.cpp new file mode 100644 index 00000000000..22beb94bb36 --- /dev/null +++ b/test/search/test_brute_force.cpp @@ -0,0 +1,158 @@ +/* + * SPDX-License-Identifier: BSD-3-Clause + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2026-, Open Perception Inc. + * + * All rights reserved + */ + +#define PCL_NO_PRECOMPILE 1 +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace +{ +class PointXYRepresentation : public pcl::PointRepresentation +{ +public: + PointXYRepresentation () + { + this->nr_dimensions_ = 2; + } + + void + copyToFloatArray (const pcl::PointXYZ& point, float* out) const override + { + out[0] = point.x; + out[1] = point.y; + } +}; + +float +descriptorDistance (const pcl::FPFHSignature33& lhs, const pcl::FPFHSignature33& rhs) +{ + float distance = 0.0f; + for (std::size_t index = 0; index < 33; ++index) + { + const float diff = lhs.histogram[index] - rhs.histogram[index]; + distance += diff * diff; + } + return (distance); +} +} // namespace + +TEST (PCL, BruteForce_setPointRepresentation) +{ + auto cloud = pcl::make_shared> (); + cloud->emplace_back (0.0f, 100.0f, 0.0f); + cloud->emplace_back (1.0f, 0.0f, 100.0f); + cloud->emplace_back (2.0f, 0.0f, 0.0f); + + pcl::search::BruteForce search (true); + search.setInputCloud (cloud); + search.setPointRepresentation ( + pcl::make_shared ()); + + pcl::Indices indices; + std::vector distances; + EXPECT_EQ (3, search.nearestKSearch (pcl::PointXYZ (0.0f, 0.0f, 0.0f), + 3, + indices, + distances)); + + ASSERT_EQ (3, indices.size ()); + ASSERT_EQ (3, distances.size ()); + EXPECT_EQ (1, indices[0]); + EXPECT_EQ (2, indices[1]); + EXPECT_EQ (0, indices[2]); + EXPECT_FLOAT_EQ (1.0f, distances[0]); + EXPECT_FLOAT_EQ (4.0f, distances[1]); + EXPECT_FLOAT_EQ (10000.0f, distances[2]); +} + +TEST (PCL, BruteForce_featurePointRepresentation) +{ + using FeatureT = pcl::FPFHSignature33; + + auto cloud = pcl::make_shared> (); + cloud->resize (4); + for (std::size_t point_index = 0; point_index < cloud->size (); ++point_index) + { + for (std::size_t dimension = 0; dimension < 33; ++dimension) + (*cloud)[point_index].histogram[dimension] = + static_cast (point_index * 10 + dimension); + } + (*cloud)[3].histogram[32] = 500.0f; + + FeatureT query{}; + for (std::size_t dimension = 0; dimension < 33; ++dimension) + query.histogram[dimension] = static_cast (10 + dimension); + + pcl::search::BruteForce search (true); + EXPECT_EQ (33, search.getPointRepresentation ()->getNumberOfDimensions ()); + search.setPointRepresentation ( + pcl::make_shared> ()); + search.setInputCloud (cloud); + + pcl::Indices indices; + std::vector distances; + EXPECT_EQ (3, search.nearestKSearch (query, 3, indices, distances)); + + std::vector> expected; + for (std::size_t index = 0; index < cloud->size (); ++index) + expected.emplace_back (descriptorDistance ((*cloud)[index], query), + static_cast (index)); + std::sort (expected.begin (), expected.end ()); + + ASSERT_EQ (3, indices.size ()); + ASSERT_EQ (3, distances.size ()); + for (std::size_t i = 0; i < indices.size (); ++i) + { + EXPECT_EQ (expected[i].second, indices[i]); + EXPECT_FLOAT_EQ (expected[i].first, distances[i]); + } +} + +TEST (PCL, BruteForce_sparseFeatureCloudSkipsInvalidDescriptors) +{ + using FeatureT = pcl::FPFHSignature33; + + auto cloud = pcl::make_shared> (); + cloud->is_dense = false; + cloud->resize (3); + for (std::size_t point_index = 0; point_index < cloud->size (); ++point_index) + { + for (std::size_t dimension = 0; dimension < 33; ++dimension) + (*cloud)[point_index].histogram[dimension] = + static_cast (point_index + dimension); + } + (*cloud)[1].histogram[0] = std::numeric_limits::quiet_NaN (); + + pcl::search::BruteForce search (true); + search.setPointRepresentation ( + pcl::make_shared> ()); + search.setInputCloud (cloud); + + pcl::Indices indices; + std::vector distances; + EXPECT_EQ (2, search.nearestKSearch ((*cloud)[0], 3, indices, distances)); + + ASSERT_EQ (2, indices.size ()); + EXPECT_NE (1, indices[0]); + EXPECT_NE (1, indices[1]); +} + +int +main (int argc, char** argv) +{ + testing::InitGoogleTest (&argc, argv); + return (RUN_ALL_TESTS ()); +} From 4d6aee2d2d731be72d803c05dbf5b99f02d5404b Mon Sep 17 00:00:00 2001 From: jeevan6996 Date: Tue, 4 Aug 2026 23:57:46 +0100 Subject: [PATCH 2/2] Limit BruteForce precompiled point instantiations --- search/src/brute_force.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/search/src/brute_force.cpp b/search/src/brute_force.cpp index 3cd29fc4740..e14716f545b 100644 --- a/search/src/brute_force.cpp +++ b/search/src/brute_force.cpp @@ -39,4 +39,5 @@ #include // Instantiations of specific point types -PCL_INSTANTIATE (BruteForce, PCL_POINT_TYPES) +PCL_INSTANTIATE (BruteForce, PCL_XYZ_POINT_TYPES) +PCL_INSTANTIATE (BruteForce, PCL_DESCRIPTOR_FEATURE_POINT_TYPES)