Extend BruteForce search to non-XYZ point types

search/include/pcl/search/brute_force.h

@@ -37,12 +37,39 @@
 
 #pragma once
 
+#include <pcl/point_representation.h>
 #include <pcl/search/search.h>
 
+#include <type_traits>
+
 namespace pcl
 {
   namespace search
   {
+    namespace detail
+    {
+      template <typename PointT, typename = void>
+      struct has_descriptor_size : std::false_type
+      {
+      };
+
+      template <typename PointT>
+      struct has_descriptor_size<PointT, std::void_t<decltype (pcl::detail::traits::descriptorSize<PointT>::value)>>
+      : std::true_type
+      {
+      };
+
+      template <typename PointT>
+      typename PointRepresentation<PointT>::ConstPtr
+      makeDefaultPointRepresentation ()
+      {
+        if constexpr (!pcl::traits::has_xyz_v<PointT> && has_descriptor_size<PointT>::value)
+          return (pcl::make_shared<DefaultFeatureRepresentation<PointT>> ());
+        else
+          return (pcl::make_shared<DefaultPointRepresentation<PointT>> ());
+      }
+    } // 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<PointT>::PointCloud;
       using PointCloudConstPtr = typename Search<PointT>::PointCloudConstPtr;
+      using PointRepresentationConstPtr = typename PointRepresentation<PointT>::ConstPtr;
 
       using IndicesPtr = pcl::IndicesPtr;
       using IndicesConstPtr = pcl::IndicesConstPtr;
@@ -83,16 +111,38 @@ 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<PointT> ("BruteForce", sorted_results)
+        , point_representation_ (detail::makeDefaultPointRepresentation<PointT> ())
         {
         }
 
         /** \brief Destructor for KdTree. */
 
         ~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<float> &k_sqr_distances,
                             unsigned int max_nn = 0) const;
+
+        PointRepresentationConstPtr point_representation_;
     };
   }
 }

search/include/pcl/search/impl/auto.hpp

@@ -59,14 +59,9 @@ pcl::search::Search<PointT> * 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<PointT>) {
-    searcher = new pcl::search::BruteForce<PointT> (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<PointT> (sorted_results);
+  searcher->setInputCloud (cloud, indices);
+  return searcher;
 }
 
 #define PCL_INSTANTIATE_AutoSelectMethod(T) template PCL_EXPORTS pcl::search::Search<T> * pcl::search::autoSelectMethod<T>(const typename pcl::PointCloud<T>::ConstPtr& cloud, const pcl::IndicesConstPtr& indices, bool sorted_results, pcl::search::Purpose purpose);

search/include/pcl/search/impl/brute_force.hpp

@@ -40,21 +40,51 @@
 #include <pcl/common/point_tests.h> // for pcl::isFinite
 #include <pcl/search/brute_force.h>
 #include <queue>
+#include <vector>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> float
 pcl::search::BruteForce<PointT>::getDistSqr (
     const PointT& point1, const PointT& point2) const
 {
-  return (point1.getVector3fMap () - point2.getVector3fMap ()).squaredNorm ();
+  if constexpr (pcl::traits::has_xyz_v<PointT>)
+  {
+    if (point_representation_->isTrivial () &&
+        point_representation_->getNumberOfDimensions () == 3)
+      return (point1.getVector3fMap () - point2.getVector3fMap ()).squaredNorm ();
+  }
+
+  const int nr_dimensions = point_representation_->getNumberOfDimensions ();
+  std::vector<float> point1_representation (nr_dimensions);
+  std::vector<float> 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 <typename PointT> bool
+pcl::search::BruteForce<PointT>::isValidPoint (const PointT& point) const
+{
+  if constexpr (pcl::traits::has_xyz_v<PointT>)
+    return (pcl::isFinite (point));
+
+  return (point_representation_->isValid (point));
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> int
 pcl::search::BruteForce<PointT>::nearestKSearch (
     const PointT& point, int k, Indices& k_indices, std::vector<float>& 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<PointT>::sparseKSearch (
     auto iIt =indices_->cbegin ();
     for (; iIt != indices_->cend () && result.size () < static_cast<unsigned> (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<Entry> (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<PointT>::sparseKSearch (
     Entry entry;
     for (entry.index = 0; (entry.index < static_cast<pcl::index_t>(input_->size ())) && (result.size () < static_cast<std::size_t> (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<Entry> (result.begin (), result.end ());
+    if (queue.empty ())
+      return 0;
 
     // add the rest
     for (; entry.index < static_cast<pcl::index_t>(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<PointT>::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<PointT>::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<PointT>::radiusSearch (
     const PointT& point, double radius, Indices &k_indices,
     std::vector<float> &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 ();

search/src/brute_force.cpp

@@ -40,3 +40,4 @@
 
 // Instantiations of specific point types
 PCL_INSTANTIATE (BruteForce, PCL_XYZ_POINT_TYPES)
+PCL_INSTANTIATE (BruteForce, PCL_DESCRIPTOR_FEATURE_POINT_TYPES)

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)