BUGFIX: sorted was ignored in some resultsets

jlblancoc · jlblancoc · commit a74fc3b5b359 · 2024-07-11T11:29:13.000+02:00
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -1,3 +1,8 @@
+# nanoflann 1.6.0: Released Jul 11, 2024
+ - BUG FIX: nanoflann::SearchParameters::sorted was ignored for RadiusResultSet.
+ - ResultSet classes now must implement a sort() method.
+ - Added type IndexType to nanoflann:KDTreeBaseClass
+
 # nanoflann 1.5.5: Released Mar 12, 2024
  - Potentially more efficient scheduling of multi-thread index building ([PR #236](https://github.com/jlblancoc/nanoflann/pull/236))
  - Bump minimum required cmake version to 3.5 ([PR #230](https://github.com/jlblancoc/nanoflann/pull/230/))
diff --git a/examples/pointcloud_custom_resultset.cpp b/examples/pointcloud_custom_resultset.cpp
@@ -82,6 +82,13 @@ class MyCustomResultSet
     }
 
     DistanceType worstDist() const { return radius; }
+
+    void sort()
+    {
+        std::sort(
+            m_indices_dists.begin(), m_indices_dists.end(),
+            nanoflann::IndexDist_Sorter());
+    }
 };
 
 void kdtree_demo(const size_t N)
diff --git a/include/nanoflann.hpp b/include/nanoflann.hpp
@@ -157,6 +157,39 @@ inline typename std::enable_if<!has_assign<Container>::value, void>::type
     for (size_t i = 0; i < nElements; i++) c[i] = value;
 }
 
+
+/** operator "<" for std::sort() */
+struct IndexDist_Sorter
+{
+    /** PairType will be typically: ResultItem<IndexType,DistanceType> */
+    template <typename PairType>
+    bool operator()(const PairType& p1, const PairType& p2) const
+    {
+        return p1.second < p2.second;
+    }
+};
+
+/**
+ * Each result element in RadiusResultSet. Note that distances and indices
+ * are named `first` and `second` to keep backward-compatibility with the
+ * `std::pair<>` type used in the past. In contrast, this structure is ensured
+ * to be `std::is_standard_layout` so it can be used in wrappers to other
+ * languages.
+ * See: https://github.com/jlblancoc/nanoflann/issues/166
+ */
+template <typename IndexType = size_t, typename DistanceType = double>
+struct ResultItem
+{
+    ResultItem() = default;
+    ResultItem(const IndexType index, const DistanceType distance)
+        : first(index), second(distance)
+    {
+    }
+
+    IndexType    first;  //!< Index of the sample in the dataset
+    DistanceType second;  //!< Distance from sample to query point
+};
+
 /** @addtogroup result_sets_grp Result set classes
  *  @{ */
 
@@ -237,6 +270,11 @@ class KNNResultSet
     }
 
     DistanceType worstDist() const { return dists[capacity - 1]; }
+
+    void sort()
+    {
+        // already sorted
+    }
 };
 
 /** Result set for RKNN searches (N-closest neighbors with a maximum radius) */
@@ -321,38 +359,11 @@ class RKNNResultSet
     }
 
     DistanceType worstDist() const { return dists[capacity - 1]; }
-};
 
-/** operator "<" for std::sort() */
-struct IndexDist_Sorter
-{
-    /** PairType will be typically: ResultItem<IndexType,DistanceType> */
-    template <typename PairType>
-    bool operator()(const PairType& p1, const PairType& p2) const
-    {
-        return p1.second < p2.second;
-    }
-};
-
-/**
- * Each result element in RadiusResultSet. Note that distances and indices
- * are named `first` and `second` to keep backward-compatibility with the
- * `std::pair<>` type used in the past. In contrast, this structure is ensured
- * to be `std::is_standard_layout` so it can be used in wrappers to other
- * languages.
- * See: https://github.com/jlblancoc/nanoflann/issues/166
- */
-template <typename IndexType = size_t, typename DistanceType = double>
-struct ResultItem
-{
-    ResultItem() = default;
-    ResultItem(const IndexType index, const DistanceType distance)
-        : first(index), second(distance)
+    void sort()
     {
+        // already sorted
     }
-
-    IndexType    first;  //!< Index of the sample in the dataset
-    DistanceType second;  //!< Distance from sample to query point
 };
 
 /**
@@ -413,6 +424,12 @@ class RadiusResultSet
             m_indices_dists.begin(), m_indices_dists.end(), IndexDist_Sorter());
         return *it;
     }
+
+    void sort()
+    {
+        std::sort(
+            m_indices_dists.begin(), m_indices_dists.end(), IndexDist_Sorter());
+    }
 };
 
 /** @} */
@@ -980,7 +997,7 @@ struct array_or_vector<-1, T>
  */
 template <
     class Derived, typename Distance, class DatasetAdaptor, int32_t DIM = -1,
-    typename IndexType = uint32_t>
+    typename index_t = uint32_t>
 class KDTreeBaseClass
 {
    public:
@@ -995,6 +1012,7 @@ class KDTreeBaseClass
 
     using ElementType  = typename Distance::ElementType;
     using DistanceType = typename Distance::DistanceType;
+    using IndexType    = index_t;
 
     /**
      *  Array of indices to vectors in the dataset_.
@@ -1239,10 +1257,7 @@ class KDTreeBaseClass
                     std::ref(right_bbox), std::ref(thread_count),
                     std::ref(mutex));
             }
-            else
-            {
-                --thread_count;
-            }
+            else { --thread_count; }
 
             BoundingBox left_bbox(bbox);
             left_bbox[cutfeat].high = cutval;
@@ -1495,17 +1510,17 @@ class KDTreeBaseClass
  */
 template <
     typename Distance, class DatasetAdaptor, int32_t DIM = -1,
-    typename IndexType = uint32_t>
+    typename index_t = uint32_t>
 class KDTreeSingleIndexAdaptor
     : public KDTreeBaseClass<
-          KDTreeSingleIndexAdaptor<Distance, DatasetAdaptor, DIM, IndexType>,
-          Distance, DatasetAdaptor, DIM, IndexType>
+          KDTreeSingleIndexAdaptor<Distance, DatasetAdaptor, DIM, index_t>,
+          Distance, DatasetAdaptor, DIM, index_t>
 {
    public:
     /** Deleted copy constructor*/
     explicit KDTreeSingleIndexAdaptor(
         const KDTreeSingleIndexAdaptor<
-            Distance, DatasetAdaptor, DIM, IndexType>&) = delete;
+            Distance, DatasetAdaptor, DIM, index_t>&) = delete;
 
     /** The data source used by this index */
     const DatasetAdaptor& dataset_;
@@ -1516,15 +1531,16 @@ class KDTreeSingleIndexAdaptor
 
     using Base = typename nanoflann::KDTreeBaseClass<
         nanoflann::KDTreeSingleIndexAdaptor<
-            Distance, DatasetAdaptor, DIM, IndexType>,
-        Distance, DatasetAdaptor, DIM, IndexType>;
+            Distance, DatasetAdaptor, DIM, index_t>,
+        Distance, DatasetAdaptor, DIM, index_t>;
 
     using Offset    = typename Base::Offset;
     using Size      = typename Base::Size;
     using Dimension = typename Base::Dimension;
 
     using ElementType  = typename Base::ElementType;
     using DistanceType = typename Base::DistanceType;
+    using IndexType    = typename Base::IndexType;
 
     using Node    = typename Base::Node;
     using NodePtr = Node*;
@@ -1677,6 +1693,9 @@ class KDTreeSingleIndexAdaptor
         assign(dists, (DIM > 0 ? DIM : Base::dim_), zero);
         DistanceType dist = this->computeInitialDistances(*this, vec, dists);
         searchLevel(result, vec, Base::root_node_, dist, dists, epsError);
+
+        if (searchParams.sorted) result.sort();
+
         return result.full();
     }
 
@@ -1733,9 +1752,6 @@ class KDTreeSingleIndexAdaptor
             radius, IndicesDists);
         const Size nFound =
             radiusSearchCustomCallback(query_point, resultSet, searchParams);
-        if (searchParams.sorted)
-            std::sort(
-                IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter());
         return nFound;
     }
 
@@ -1848,7 +1864,7 @@ class KDTreeSingleIndexAdaptor
             {
                 const IndexType accessor = Base::vAcc_[i];  // reorder... : i;
                 DistanceType    dist     = distance_.evalMetric(
-                    vec, accessor, (DIM > 0 ? DIM : Base::dim_));
+                           vec, accessor, (DIM > 0 ? DIM : Base::dim_));
                 if (dist < worst_dist)
                 {
                     if (!result_set.addPoint(dist, Base::vAcc_[i]))
@@ -2202,9 +2218,6 @@ class KDTreeSingleIndexDynamicAdaptor_
             radius, IndicesDists);
         const size_t nFound =
             radiusSearchCustomCallback(query_point, resultSet, searchParams);
-        if (searchParams.sorted)
-            std::sort(
-                IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter());
         return nFound;
     }
 
diff --git a/tests/test_main.cpp b/tests/test_main.cpp
@@ -59,16 +59,14 @@ void L2_vs_L2_simple_test(const size_t N, const size_t num_results)
     num_t query_pt[3] = {0.5, 0.5, 0.5};
 
     // construct a kd-tree index:
-    typedef KDTreeSingleIndexAdaptor<
+    using my_kd_tree_simple_t = KDTreeSingleIndexAdaptor<
         L2_Simple_Adaptor<num_t, PointCloud<num_t>>, PointCloud<num_t>,
         3 /* dim */
-        >
-        my_kd_tree_simple_t;
+        >;
 
-    typedef KDTreeSingleIndexAdaptor<
+    using my_kd_tree_t = KDTreeSingleIndexAdaptor<
         L2_Adaptor<num_t, PointCloud<num_t>>, PointCloud<num_t>, 3 /* dim */
-        >
-        my_kd_tree_t;
+        >;
 
     my_kd_tree_simple_t index1(
         3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */));
@@ -95,6 +93,38 @@ void L2_vs_L2_simple_test(const size_t N, const size_t num_results)
         EXPECT_EQ(ret_index1[i], ret_index[i]);
         EXPECT_DOUBLE_EQ(out_dist_sqr1[i], out_dist_sqr[i]);
     }
+    // Ensure results are sorted:
+    num_t lastDist = -1;
+    for (size_t i = 0; i < out_dist_sqr.size(); i++)
+    {
+        const num_t newDist = out_dist_sqr[i];
+        EXPECT_GE(newDist, lastDist);
+        lastDist = newDist;
+    }
+
+    // Test "RadiusResultSet" too:
+    const num_t maxRadiusSqrSearch = 10.0 * 10.0;
+
+    std::vector<nanoflann::ResultItem<
+        typename my_kd_tree_simple_t::IndexType,
+        typename my_kd_tree_simple_t::DistanceType>>
+        radiusIdxs;
+
+    nanoflann::RadiusResultSet<num_t, typename my_kd_tree_simple_t::IndexType>
+        radiusResults(maxRadiusSqrSearch, radiusIdxs);
+    radiusResults.init();
+    nanoflann::SearchParameters searchParams;
+    searchParams.sorted = true;
+    index1.findNeighbors(radiusResults, &query_pt[0], searchParams);
+
+    // Ensure results are sorted:
+    lastDist = -1;
+    for (const auto& r : radiusIdxs)
+    {
+        const num_t newDist = r.second;
+        EXPECT_GE(newDist, lastDist);
+        lastDist = newDist;
+    }
 }
 
 using namespace nanoflann;
@@ -725,7 +755,8 @@ TEST(kdtree, add_and_remove_points)
     my_kd_tree_simple_t index(
         3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */));
 
-    const auto query = [&index]() -> size_t {
+    const auto query = [&index]() -> size_t
+    {
         const double                    query_pt[3] = {0.5, 0.5, 0.5};
         const size_t                    num_results = 1;
         std::vector<size_t>             ret_index(num_results);
