Extend BruteForce search to non-XYZ point types

common/include/pcl/impl/point_types.hpp

@@ -166,7 +166,7 @@ namespace pcl
       template<> struct descriptorSize<FPFHSignature33> { static constexpr const int value = 33; };
       template<> struct descriptorSize<VFHSignature308> { static constexpr const int value = 308; };
       template<> struct descriptorSize<GRSDSignature21> { static constexpr const int value = 21; };
-      template<> struct descriptorSize<BRISKSignature512> { static constexpr const int value = 512; };
+      template<> struct descriptorSize<BRISKSignature512> { static constexpr const int value = 64; };
       template<> struct descriptorSize<ESFSignature640> { static constexpr const int value = 640; };
       template<> struct descriptorSize<GASDSignature512> { static constexpr const int value = 512; };
       template<> struct descriptorSize<GASDSignature984> { static constexpr const int value = 984; };

common/include/pcl/point_representation.h

@@ -485,6 +485,42 @@ namespace pcl
   class DefaultPointRepresentation<NormalBasedSignature12> : public DefaultFeatureRepresentation <NormalBasedSignature12>
   {};
 
+  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+  template <>
+  class DefaultPointRepresentation<GRSDSignature21> : public DefaultFeatureRepresentation <GRSDSignature21>
+  {};
+
+  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+  template <>
+  class DefaultPointRepresentation <BRISKSignature512> : public PointRepresentation <BRISKSignature512>
+  {
+    public:
+      static constexpr const std::int32_t NR_DIMS = 64;
+
+      DefaultPointRepresentation ()
+      {
+        nr_dimensions_ = NR_DIMS;
+        trivial_ = false;
+      }
+
+      void
+      copyToFloatArray (const BRISKSignature512 &p, float * out) const override
+      {
+        for (int i = 0; i < nr_dimensions_; ++i)
+          out[i] = p.descriptor[i];
+      }
+  };
+
+  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+  template <>
+  class DefaultPointRepresentation<ESFSignature640> : public DefaultFeatureRepresentation <ESFSignature640>
+  {};
+
+  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+  template <>
+  class DefaultPointRepresentation<GFPFHSignature16> : public DefaultFeatureRepresentation <GFPFHSignature16>
+  {};
+
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   template <>
   class DefaultPointRepresentation<ShapeContext1980> : public PointRepresentation<ShapeContext1980>

search/include/pcl/search/brute_force.h

@@ -37,6 +37,7 @@
 
 #pragma once
 
+#include <pcl/point_representation.h>
 #include <pcl/search/search.h>
 
 namespace pcl
@@ -52,6 +53,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 +85,35 @@ namespace pcl
 
       // replace by some metric functor
       float getDistSqr (const PointT& point1, const PointT& point2) const;
+
       public:
         BruteForce (bool sorted_results = false)
         : Search<PointT> ("BruteForce", sorted_results)
+        , point_representation_ (new DefaultPointRepresentation<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 +156,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

@@ -37,24 +37,43 @@
 
 #pragma once
 
-#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> 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 (point_representation_->isValid (point) && "Invalid (NaN, Inf) point given to nearestKSearch!");
 
   k_indices.clear ();
   k_distances.clear ();
@@ -149,18 +168,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 (point_representation_->isValid ((*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 (!point_representation_->isValid ((*input_)[*iIt]))
         continue;
 
       entry.distance = getDistSqr ((*input_)[*iIt], point);
@@ -177,18 +198,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 (point_representation_->isValid ((*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 (!point_representation_->isValid ((*input_)[entry.index]))
         continue;
 
       entry.distance = getDistSqr ((*input_)[entry.index], point);
@@ -293,7 +316,7 @@ pcl::search::BruteForce<PointT>::sparseRadiusSearch (
   {
     for (const auto& idx : *indices_)
     {
-      if (!std::isfinite ((*input_)[idx].x))
+      if (!point_representation_->isValid ((*input_)[idx]))
         continue;
 
       distance = getDistSqr ((*input_)[idx], point);
@@ -310,7 +333,7 @@ pcl::search::BruteForce<PointT>::sparseRadiusSearch (
   {
     for (std::size_t index = 0; index < input_->size (); ++index)
     {
-      if (!std::isfinite ((*input_)[index].x))
+      if (!point_representation_->isValid ((*input_)[index]))
         continue;
       distance = getDistSqr ((*input_)[index], point);
       if (distance <= radius)
@@ -335,7 +358,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 (point_representation_->isValid (point) && "Invalid (NaN, Inf) point given to radiusSearch!");
 
   k_indices.clear ();
   k_sqr_distances.clear ();

search/src/brute_force.cpp

@@ -38,5 +38,5 @@
 #include <pcl/search/brute_force.h>
 #include <pcl/search/impl/brute_force.hpp>
 
-// Instantiations of specific point types
-PCL_INSTANTIATE (BruteForce, PCL_XYZ_POINT_TYPES)
+// Keep the precompiled BruteForce types in sync with AutoSelectMethod.
+PCL_INSTANTIATE (BruteForce, PCL_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)