1 #ifndef MEDUSA_BITS_SPATIAL_SEARCH_KDTREE_HPP_
2 #define MEDUSA_BITS_SPATIAL_SEARCH_KDTREE_HPP_
15 template <
class vec_t>
18 assert_msg(point.array().isFinite().prod() == 1,
"Invalid point.");
21 int actual_k = tree.knnSearch(point.data(), k, &ret_index[0], &out_dist_sqr[0]);
22 assert_msg(actual_k == k,
"There were not enough points in the tree, you requested %d "
23 "points, the tree only contains %d points.", k, actual_k);
24 return {ret_index, out_dist_sqr};
27 template <
class vec_t>
29 const vec_t& point,
const scalar_t& radius_squared)
const {
30 assert_msg(point.array().isFinite().prod() == 1,
"Invalid point.");
31 std::vector<std::pair<int, scalar_t>> idx_dist;
32 int k = tree.radiusSearch(point.data(), radius_squared, idx_dist, nanoflann::SearchParams());
34 for (
int i = 0; i < k; ++i) {
35 std::tie(idx[i], dists[i]) = idx_dist[i];
43 return os <<
"KDTree:\n"
44 <<
" dimension: " << tree.
dim <<
'\n'
45 <<
" num of points: " << tree.
size() <<
"\n"
46 <<
" first point: " << tree.
get(0) <<
"\n"
47 <<
" last point: " << tree.
get(tree.
size() - 1) <<
"\n";
52 #endif // MEDUSA_BITS_SPATIAL_SEARCH_KDTREE_HPP_