81 class Derived,
typename num_t = float,
102 return *
static_cast<const Derived*
>(
this);
105 inline Derived&
derived() {
return *
static_cast<Derived*
>(
this); }
137 float x0,
float y0,
float& out_x,
float& out_y,
138 float& out_dist_sqr)
const 145 const size_t knn = 1;
148 resultSet.
init(&ret_index, &out_dist_sqr);
150 const std::array<num_t, 2> query_point{{x0, y0}};
155 out_x =
derived().kdtree_get_pt(ret_index, 0);
156 out_y =
derived().kdtree_get_pt(ret_index, 1);
164 float x0,
float y0,
float& out_dist_sqr)
const 171 const size_t knn = 1;
174 resultSet.
init(&ret_index, &out_dist_sqr);
176 const std::array<num_t, 2> query_point{{x0, y0}};
190 static_cast<float>(p0.
x), static_cast<float>(p0.
y), dmy1, dmy2,
202 float closerx, closery, closer_dist;
210 static_cast<float>(p0.
x), static_cast<float>(p0.
y));
234 float x0,
float y0,
float& out_x1,
float& out_y1,
float& out_x2,
235 float& out_y2,
float& out_dist_sqr1,
float& out_dist_sqr2)
const 242 const size_t knn = 2;
243 size_t ret_indexes[2];
246 resultSet.
init(&ret_indexes[0], &ret_sqdist[0]);
248 const std::array<num_t, 2> query_point{{x0, y0}};
253 out_x1 =
derived().kdtree_get_pt(ret_indexes[0], 0);
254 out_y1 =
derived().kdtree_get_pt(ret_indexes[0], 1);
255 out_dist_sqr1 = ret_sqdist[0];
257 out_x2 =
derived().kdtree_get_pt(ret_indexes[1], 0);
258 out_y2 =
derived().kdtree_get_pt(ret_indexes[1], 1);
259 out_dist_sqr2 = ret_sqdist[0];
266 float& outDistSqr1,
float& outDistSqr2)
const 268 float dmy1, dmy2, dmy3, dmy4;
270 p0.
x, p0.
y, dmy1, dmy2, dmy3, dmy4, outDistSqr1, outDistSqr2);
271 pOut1.
x =
static_cast<double>(dmy1);
272 pOut1.
y =
static_cast<double>(dmy2);
273 pOut2.
x =
static_cast<double>(dmy3);
274 pOut2.
y =
static_cast<double>(dmy4);
299 float x0,
float y0,
size_t knn, std::vector<float>& out_x,
300 std::vector<float>& out_y, std::vector<float>& out_dist_sqr)
const 307 std::vector<size_t> ret_indexes(knn);
310 out_dist_sqr.resize(knn);
313 resultSet.
init(&ret_indexes[0], &out_dist_sqr[0]);
315 const std::array<num_t, 2> query_point{{x0, y0}};
319 for (
size_t i = 0; i < knn; i++)
321 out_x[i] =
derived().kdtree_get_pt(ret_indexes[i], 0);
322 out_y[i] =
derived().kdtree_get_pt(ret_indexes[i], 1);
329 const TPoint2D& p0,
size_t N, std::vector<TPoint2D>& pOut,
330 std::vector<float>& outDistSqr)
const 332 std::vector<float> dmy1, dmy2;
334 static_cast<float>(p0.
x), static_cast<float>(p0.
y), N, dmy1, dmy2,
336 pOut.resize(dmy1.size());
337 for (
size_t i = 0; i < dmy1.size(); i++)
339 pOut[i].x =
static_cast<double>(dmy1[i]);
340 pOut[i].y =
static_cast<double>(dmy2[i]);
362 float x0,
float y0,
size_t knn, std::vector<size_t>& out_idx,
363 std::vector<float>& out_dist_sqr)
const 371 out_dist_sqr.resize(knn);
373 resultSet.
init(&out_idx[0], &out_dist_sqr[0]);
375 const std::array<num_t, 2> query_point{{x0, y0}};
382 const TPoint2D& p0,
size_t N, std::vector<size_t>& outIdx,
383 std::vector<float>& outDistSqr)
const 386 static_cast<float>(p0.
x), static_cast<float>(p0.
y), N, outIdx,
410 float x0,
float y0,
float z0,
float& out_x,
float& out_y,
float& out_z,
411 float& out_dist_sqr)
const 418 const size_t knn = 1;
421 resultSet.
init(&ret_index, &out_dist_sqr);
423 const std::array<num_t, 3> query_point{{x0, y0, z0}};
428 out_x =
derived().kdtree_get_pt(ret_index, 0);
429 out_y =
derived().kdtree_get_pt(ret_index, 1);
430 out_z =
derived().kdtree_get_pt(ret_index, 2);
438 float x0,
float y0,
float z0,
float& out_dist_sqr)
const 445 const size_t knn = 1;
448 resultSet.
init(&ret_index, &out_dist_sqr);
450 const std::array<num_t, 3> query_point{{x0, y0, z0}};
462 float dmy1, dmy2, dmy3;
464 static_cast<float>(p0.
x), static_cast<float>(p0.
y),
465 static_cast<float>(p0.
z), dmy1, dmy2, dmy3, outDistSqr);
466 pOut.
x =
static_cast<double>(dmy1);
467 pOut.
y =
static_cast<double>(dmy2);
468 pOut.
z =
static_cast<double>(dmy3);
495 float x0,
float y0,
float z0,
size_t knn, std::vector<float>& out_x,
496 std::vector<float>& out_y, std::vector<float>& out_z,
497 std::vector<float>& out_dist_sqr)
const 504 std::vector<size_t> ret_indexes(knn);
508 out_dist_sqr.resize(knn);
511 resultSet.
init(&ret_indexes[0], &out_dist_sqr[0]);
513 const std::array<num_t, 3> query_point{{x0, y0, z0}};
517 for (
size_t i = 0; i < knn; i++)
519 out_x[i] =
derived().kdtree_get_pt(ret_indexes[i], 0);
520 out_y[i] =
derived().kdtree_get_pt(ret_indexes[i], 1);
521 out_z[i] =
derived().kdtree_get_pt(ret_indexes[i], 2);
550 float x0,
float y0,
float z0,
size_t knn, std::vector<float>& out_x,
551 std::vector<float>& out_y, std::vector<float>& out_z,
552 std::vector<size_t>& out_idx, std::vector<float>& out_dist_sqr)
const 563 out_dist_sqr.resize(knn);
566 resultSet.
init(&out_idx[0], &out_dist_sqr[0]);
568 const std::array<num_t, 3> query_point{{x0, y0, z0}};
572 for (
size_t i = 0; i < knn; i++)
574 out_x[i] =
derived().kdtree_get_pt(out_idx[i], 0);
575 out_y[i] =
derived().kdtree_get_pt(out_idx[i], 1);
576 out_z[i] =
derived().kdtree_get_pt(out_idx[i], 2);
582 const TPoint3D& p0,
size_t N, std::vector<TPoint3D>& pOut,
583 std::vector<float>& outDistSqr)
const 585 std::vector<float> dmy1, dmy2, dmy3;
587 static_cast<float>(p0.
x), static_cast<float>(p0.
y),
588 static_cast<float>(p0.
z), N, dmy1, dmy2, dmy3, outDistSqr);
589 pOut.resize(dmy1.size());
590 for (
size_t i = 0; i < dmy1.size(); i++)
592 pOut[i].x =
static_cast<double>(dmy1[i]);
593 pOut[i].y =
static_cast<double>(dmy2[i]);
594 pOut[i].z =
static_cast<double>(dmy3[i]);
616 const num_t x0,
const num_t y0,
const num_t z0,
617 const num_t maxRadiusSqr,
618 std::vector<std::pair<size_t, num_t>>& out_indices_dist)
const 622 out_indices_dist.clear();
625 const num_t xyz[3] = {x0, y0, z0};
627 &xyz[0], maxRadiusSqr, out_indices_dist,
630 return out_indices_dist.size();
651 const num_t x0,
const num_t y0,
const num_t maxRadiusSqr,
652 std::vector<std::pair<size_t, num_t>>& out_indices_dist)
const 656 out_indices_dist.clear();
659 const num_t xyz[2] = {x0, y0};
661 &xyz[0], maxRadiusSqr, out_indices_dist,
664 return out_indices_dist.size();
686 float x0,
float y0,
float z0,
size_t knn, std::vector<size_t>& out_idx,
687 std::vector<float>& out_dist_sqr)
const 695 out_dist_sqr.resize(knn);
697 resultSet.
init(&out_idx[0], &out_dist_sqr[0]);
699 const std::array<num_t, 3> query_point{{x0, y0, z0}};
706 const TPoint3D& p0,
size_t N, std::vector<size_t>& outIdx,
707 std::vector<float>& outDistSqr)
const 710 static_cast<float>(p0.
x), static_cast<float>(p0.
y),
711 static_cast<float>(p0.
z), N, outIdx, outDistSqr);
730 template <
int _DIM = -1>
743 if (&o !=
this)
clear();
753 std::unique_ptr<kdtree_index_t>
index;
786 const size_t N =
derived().kdtree_get_point_count();
820 const size_t N =
derived().kdtree_get_point_count();
void kdTreeNClosestPoint3DIdx(float x0, float y0, float z0, size_t knn, std::vector< size_t > &out_idx, std::vector< float > &out_dist_sqr) const
KD Tree-based search for the N closest point to some given 3D coordinates and returns their indexes...
void kdTreeNClosestPoint3DWithIdx(float x0, float y0, float z0, size_t knn, std::vector< float > &out_x, std::vector< float > &out_y, std::vector< float > &out_z, std::vector< size_t > &out_idx, std::vector< float > &out_dist_sqr) const
KD Tree-based search for the N closest points to some given 3D coordinates.
TKDTreeDataHolder()=default
void kdTreeNClosestPoint3DIdx(const TPoint3D &p0, size_t N, std::vector< size_t > &outIdx, std::vector< float > &outDistSqr) const
double x
X,Y,Z coordinates.
KDTreeCapable & operator=(const KDTreeCapable &)
#define THROW_EXCEPTION(msg)
Squared Euclidean (L2) distance functor (suitable for low-dimensionality datasets, like 2D or 3D point clouds) Corresponding distance traits: nanoflann::metric_L2_Simple.
void kdTreeNClosestPoint2DIdx(const TPoint2D &p0, size_t N, std::vector< size_t > &outIdx, std::vector< float > &outDistSqr) const
nanoflann::KDTreeSingleIndexAdaptor< metric_t, Derived, _DIM > kdtree_index_t
TKDTreeDataHolder & operator=(const TKDTreeDataHolder &o) noexcept
Copy operator: It actually does NOT copy the kd-tree, a new object will be created if required! ...
void kdTreeTwoClosestPoint2D(const TPoint2D &p0, TPoint2D &pOut1, TPoint2D &pOut2, float &outDistSqr1, float &outDistSqr2) const
size_t kdTreeClosestPoint3D(const TPoint3D &p0, TPoint3D &pOut, float &outDistSqr) const
void init(IndexType *indices_, DistanceType *dists_)
TKDTreeDataHolder< 2 > m_kdtree2d_data
size_t m_dim
Dimensionality.
const Derived & derived() const
CRTP helper method.
void kdTreeNClosestPoint2DIdx(float x0, float y0, size_t knn, std::vector< size_t > &out_idx, std::vector< float > &out_dist_sqr) const
KD Tree-based search for the N closest point to some given 2D coordinates and returns their indexes...
void kdTreeEnsureIndexBuilt3D()
void rebuild_kdTree_2D() const
Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ...
Derived & derived()
CRTP helper method.
Internal structure with the KD-tree representation (mainly used to avoid copying pointers with the = ...
TKDTreeSearchParams()=default
A generic adaptor class for providing Nearest Neighbor (NN) lookup via the nanoflann library...
TKDTreeDataHolder< 3 > m_kdtree3d_data
This base provides a set of functions for maths stuff.
size_t kdTreeRadiusSearch2D(const num_t x0, const num_t y0, const num_t maxRadiusSqr, std::vector< std::pair< size_t, num_t >> &out_indices_dist) const
KD Tree-based search for all the points within a given radius of some 2D point.
void rebuild_kdTree_3D() const
Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ...
float kdTreeClosestPoint2DsqrError(const TPoint2D &p0) const
void kdTreeNClosestPoint3D(float x0, float y0, float z0, size_t knn, std::vector< float > &out_x, std::vector< float > &out_y, std::vector< float > &out_z, std::vector< float > &out_dist_sqr) const
KD Tree-based search for the N closest points to some given 3D coordinates.
std::vector< size_t > kdTreeNClosestPoint2D(const TPoint2D &p0, size_t N, std::vector< TPoint2D > &pOut, std::vector< float > &outDistSqr) const
size_t kdTreeClosestPoint2D(float x0, float y0, float &out_dist_sqr) const
size_t kdTreeClosestPoint2D(const TPoint2D &p0, TPoint2D &pOut, float &outDistSqr) const
size_t kdTreeClosestPoint3D(float x0, float y0, float z0, float &out_dist_sqr) const
KDTreeCapable(const KDTreeCapable &)
KDTreeCapable()=default
Constructor.
size_t kdTreeRadiusSearch3D(const num_t x0, const num_t y0, const num_t z0, const num_t maxRadiusSqr, std::vector< std::pair< size_t, num_t >> &out_indices_dist) const
KD Tree-based search for all the points within a given radius of some 3D point.
size_t leaf_max_size
Max points per leaf.
Parameters (see README.md)
void kdTreeNClosestPoint3D(const TPoint3D &p0, size_t N, std::vector< TPoint3D > &pOut, std::vector< float > &outDistSqr) const
void kdtree_mark_as_outdated() const
To be called by child classes when KD tree data changes.
std::vector< size_t > kdTreeNClosestPoint2D(float x0, float y0, size_t knn, std::vector< float > &out_x, std::vector< float > &out_y, std::vector< float > &out_dist_sqr) const
KD Tree-based search for the N closest point to some given 2D coordinates.
Search options for KDTreeSingleIndexAdaptor::findNeighbors()
std::atomic_bool m_kdtree_is_uptodate
whether the KD tree needs to be rebuilt or not.
TKDTreeDataHolder(const TKDTreeDataHolder &)
Copy constructor: It actually does NOT copy the kd-tree, a new object will be created if required! ...
size_t kdTreeClosestPoint2D(float x0, float y0, float &out_x, float &out_y, float &out_dist_sqr) const
KD Tree-based search for the closest point (only ONE) to some given 2D coordinates.
TKDTreeSearchParams kdtree_search_params
Parameters to tune the ANN searches.
void clear() noexcept
Free memory (if allocated)
float kdTreeClosestPoint2DsqrError(float x0, float y0) const
Like kdTreeClosestPoint2D, but just return the square error from some point to its closest neighbor...
void kdTreeEnsureIndexBuilt2D()
void kdTreeTwoClosestPoint2D(float x0, float y0, float &out_x1, float &out_y1, float &out_x2, float &out_y2, float &out_dist_sqr1, float &out_dist_sqr2) const
KD Tree-based search for the TWO closest point to some given 2D coordinates.
std::unique_ptr< kdtree_index_t > index
nullptr or the up-to-date index
size_t kdTreeClosestPoint3D(float x0, float y0, float z0, float &out_x, float &out_y, float &out_z, float &out_dist_sqr) const
KD Tree-based search for the closest point (only ONE) to some given 3D coordinates.