From 08a7a66991eae35c8a530df640a0d8fde967d7b3 Mon Sep 17 00:00:00 2001 From: Sven Oesau Date: Wed, 28 Feb 2024 10:28:19 +0100 Subject: [PATCH] bugfix nearest neighbor search --- .../Orthtree/octree_find_nearest_neighbor.cpp | 43 ++++++++----------- Orthtree/include/CGAL/Orthtree.h | 19 ++++---- 2 files changed, 28 insertions(+), 34 deletions(-) diff --git a/Orthtree/examples/Orthtree/octree_find_nearest_neighbor.cpp b/Orthtree/examples/Orthtree/octree_find_nearest_neighbor.cpp index 9c9986fe2a1..3ce7056424d 100644 --- a/Orthtree/examples/Orthtree/octree_find_nearest_neighbor.cpp +++ b/Orthtree/examples/Orthtree/octree_find_nearest_neighbor.cpp @@ -44,7 +44,6 @@ int main(int argc, char** argv) { {-0.46026, -0.25353, 0.32051}, {-0.460261, -0.253533, 0.320513} }; - typename Octree::Sphere s(Point(0, -0.1, 0), 5.0); for (const Point& p : points_to_find) octree.nearest_neighbors @@ -56,31 +55,25 @@ int main(int argc, char** argv) { ") is (" << points.point(nearest) << ")" << std::endl; })); + typename Octree::Sphere s(points_to_find[0], 0.0375); + std::cout << std::endl << "Closest points within the sphere around " << s.center() << " with squared radius of " << s.squared_radius() << ":" << std::endl; + octree.nearest_neighbors + (s, + boost::make_function_output_iterator + ([&](const Point_set::Index& nearest) + { + std::cout << points.point(nearest) << " dist: " << (Point(0, 0, 0) - points.point(nearest)).squared_length() << std::endl; + })); - std::cout << std::endl << "Closest points within sphere" << std::endl; -/* - for (const Point& p : points_to_find) - octree.nearest_neighbors - (s, - boost::make_function_output_iterator - ([&](const Point_set::Index& nearest) - { - std::cout << points.point(nearest) << std::endl; - }));*/ - - std::cout << std::endl << "Closest points within sphere" << std::endl; - for (const Point& p : points_to_find) { - std::cout << "The up to three closest points to (" << p << - ") within a squared radius of " << s.squared_radius() << " are:" << std::endl; - octree.nearest_k_neighbors_in_radius - (s, 3, - boost::make_function_output_iterator - ([&](const Point_set::Index& nearest) - { - std::cout << "(" << points.point(nearest) << ")" << std::endl; - })); - std::cout << std::endl; - } + std::size_t k = 3; + std::cout << std::endl << "The up to " << k << " closest points to(" << s.center() << ") within a squared radius of " << s.squared_radius() << " are:" << std::endl; + octree.nearest_k_neighbors_in_radius + (s, k, + boost::make_function_output_iterator + ([&](const Point_set::Index& nearest) + { + std::cout << "(" << points.point(nearest) << " dist: " << (Point(0, 0, 0) - points.point(nearest)).squared_length() << std::endl; + })); return EXIT_SUCCESS; } diff --git a/Orthtree/include/CGAL/Orthtree.h b/Orthtree/include/CGAL/Orthtree.h index ae662c94c36..552026d0b3f 100644 --- a/Orthtree/include/CGAL/Orthtree.h +++ b/Orthtree/include/CGAL/Orthtree.h @@ -678,6 +678,7 @@ public: std::size_t k, OutputIterator output) const -> std::enable_if_t { Sphere query_sphere(query, (std::numeric_limits::max)()); + CGAL_precondition(k > 0); return nearest_k_neighbors_in_radius(query_sphere, k, output); } @@ -697,9 +698,7 @@ public: */ template auto nearest_neighbors(const Sphere& query, OutputIterator output) const -> std::enable_if_t { - Sphere query_sphere = query; - - return nearest_k_neighbors_in_radius(query_sphere, (std::numeric_limits::max)(), output); + return nearest_k_neighbors_in_radius(query, (std::numeric_limits::max)(), output); } /*! @@ -721,10 +720,12 @@ public: */ template auto nearest_k_neighbors_in_radius( - Sphere& query, + const Sphere& query, std::size_t k, OutputIterator output ) const -> std::enable_if_t { + CGAL_precondition(k > 0); + Sphere query_sphere = query; // todo: this type is over-constrained, this must be made more generic struct Node_element_with_distance { @@ -738,7 +739,7 @@ public: element_list.reserve(k); // Invoking the recursive function adds those elements to the vector (passed by reference) - nearest_k_neighbors_recursive(query, root(), element_list); + nearest_k_neighbors_recursive(query_sphere, root(), element_list, k); // Add all the points found to the output for (auto& item : element_list) @@ -1294,6 +1295,7 @@ private: // functions : Sphere& search_bounds, Node_index node, std::vector& results, + std::size_t k, FT epsilon = 0) const -> std::enable_if_t { // Check whether the node has children @@ -1313,8 +1315,7 @@ private: // functions : if (current_element_with_distance.distance < m_traits.compute_squared_radius_d_object()(search_bounds)) { // Check if the results list is full - if (results.size() == results.capacity()) { - + if (results.size() == k) { // Delete a element if we need to make room results.pop_back(); } @@ -1328,7 +1329,7 @@ private: // functions : }); // Check if the results list is full - if (results.size() == results.capacity()) { + if (results.size() == k) { // Set the search radius search_bounds = m_traits.construct_sphere_d_object()(m_traits.construct_center_d_object()(search_bounds), results.back().distance + epsilon); @@ -1381,7 +1382,7 @@ private: // functions : if (CGAL::do_intersect(bbox(child_with_distance.index), search_bounds)) { // Recursively invoke this function - nearest_k_neighbors_recursive(search_bounds, child_with_distance.index, results); + nearest_k_neighbors_recursive(search_bounds, child_with_distance.index, results, k); } } }