75 class Derived,
typename num_t = float,
89 return *
static_cast<const Derived*
>(
this);
92 inline Derived&
derived() {
return *
static_cast<Derived*
>(
this); }
124 float x0,
float y0,
float& out_x,
float& out_y,
125 float& out_dist_sqr)
const 132 const size_t knn = 1;
135 resultSet.
init(&ret_index, &out_dist_sqr);
144 out_x =
derived().kdtree_get_pt(ret_index, 0);
145 out_y =
derived().kdtree_get_pt(ret_index, 1);
153 float x0,
float y0,
float& out_dist_sqr)
const 160 const size_t knn = 1;
163 resultSet.
init(&ret_index, &out_dist_sqr);
181 static_cast<float>(p0.
x), static_cast<float>(p0.
y), dmy1, dmy2,
193 float closerx, closery, closer_dist;
201 static_cast<float>(p0.
x), static_cast<float>(p0.
y));
225 float x0,
float y0,
float& out_x1,
float& out_y1,
float& out_x2,
226 float& out_y2,
float& out_dist_sqr1,
float& out_dist_sqr2)
const 233 const size_t knn = 2;
234 size_t ret_indexes[2];
237 resultSet.
init(&ret_indexes[0], &ret_sqdist[0]);
246 out_x1 =
derived().kdtree_get_pt(ret_indexes[0], 0);
247 out_y1 =
derived().kdtree_get_pt(ret_indexes[0], 1);
248 out_dist_sqr1 = ret_sqdist[0];
250 out_x2 =
derived().kdtree_get_pt(ret_indexes[1], 0);
251 out_y2 =
derived().kdtree_get_pt(ret_indexes[1], 1);
252 out_dist_sqr2 = ret_sqdist[0];
259 float& outDistSqr1,
float& outDistSqr2)
const 261 float dmy1, dmy2, dmy3, dmy4;
263 p0.
x, p0.
y, dmy1, dmy2, dmy3, dmy4, outDistSqr1, outDistSqr2);
264 pOut1.
x =
static_cast<double>(dmy1);
265 pOut1.
y =
static_cast<double>(dmy2);
266 pOut2.
x =
static_cast<double>(dmy3);
267 pOut2.
y =
static_cast<double>(dmy4);
292 float x0,
float y0,
size_t knn, std::vector<float>& out_x,
293 std::vector<float>& out_y, std::vector<float>& out_dist_sqr)
const 300 std::vector<size_t> ret_indexes(knn);
303 out_dist_sqr.resize(knn);
306 resultSet.
init(&ret_indexes[0], &out_dist_sqr[0]);
314 for (
size_t i = 0; i < knn; i++)
316 out_x[i] =
derived().kdtree_get_pt(ret_indexes[i], 0);
317 out_y[i] =
derived().kdtree_get_pt(ret_indexes[i], 1);
324 const TPoint2D& p0,
size_t N, std::vector<TPoint2D>& pOut,
325 std::vector<float>& outDistSqr)
const 327 std::vector<float> dmy1, dmy2;
329 static_cast<float>(p0.
x), static_cast<float>(p0.
y), N, dmy1, dmy2,
331 pOut.resize(dmy1.size());
332 for (
size_t i = 0; i < dmy1.size(); i++)
334 pOut[i].x =
static_cast<double>(dmy1[i]);
335 pOut[i].y =
static_cast<double>(dmy2[i]);
357 float x0,
float y0,
size_t knn, std::vector<size_t>& out_idx,
358 std::vector<float>& out_dist_sqr)
const 366 out_dist_sqr.resize(knn);
368 resultSet.
init(&out_idx[0], &out_dist_sqr[0]);
379 const TPoint2D& p0,
size_t N, std::vector<size_t>& outIdx,
380 std::vector<float>& outDistSqr)
const 383 static_cast<float>(p0.
x), static_cast<float>(p0.
y), N, outIdx,
407 float x0,
float y0,
float z0,
float& out_x,
float& out_y,
float& out_z,
408 float& out_dist_sqr)
const 415 const size_t knn = 1;
418 resultSet.
init(&ret_index, &out_dist_sqr);
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);
465 float dmy1, dmy2, dmy3;
467 static_cast<float>(p0.
x), static_cast<float>(p0.
y),
468 static_cast<float>(p0.
z), dmy1, dmy2, dmy3, outDistSqr);
469 pOut.
x =
static_cast<double>(dmy1);
470 pOut.
y =
static_cast<double>(dmy2);
471 pOut.
z =
static_cast<double>(dmy3);
498 float x0,
float y0,
float z0,
size_t knn, std::vector<float>& out_x,
499 std::vector<float>& out_y, std::vector<float>& out_z,
500 std::vector<float>& out_dist_sqr)
const 507 std::vector<size_t> ret_indexes(knn);
511 out_dist_sqr.resize(knn);
514 resultSet.
init(&ret_indexes[0], &out_dist_sqr[0]);
523 for (
size_t i = 0; i < knn; i++)
525 out_x[i] =
derived().kdtree_get_pt(ret_indexes[i], 0);
526 out_y[i] =
derived().kdtree_get_pt(ret_indexes[i], 1);
527 out_z[i] =
derived().kdtree_get_pt(ret_indexes[i], 2);
556 float x0,
float y0,
float z0,
size_t knn, std::vector<float>& out_x,
557 std::vector<float>& out_y, std::vector<float>& out_z,
558 std::vector<size_t>& out_idx, std::vector<float>& out_dist_sqr)
const 569 out_dist_sqr.resize(knn);
572 resultSet.
init(&out_idx[0], &out_dist_sqr[0]);
581 for (
size_t i = 0; i < knn; i++)
583 out_x[i] =
derived().kdtree_get_pt(out_idx[i], 0);
584 out_y[i] =
derived().kdtree_get_pt(out_idx[i], 1);
585 out_z[i] =
derived().kdtree_get_pt(out_idx[i], 2);
591 const TPoint3D& p0,
size_t N, std::vector<TPoint3D>& pOut,
592 std::vector<float>& outDistSqr)
const 594 std::vector<float> dmy1, dmy2, dmy3;
596 static_cast<float>(p0.
x), static_cast<float>(p0.
y),
597 static_cast<float>(p0.
z), N, dmy1, dmy2, dmy3, outDistSqr);
598 pOut.resize(dmy1.size());
599 for (
size_t i = 0; i < dmy1.size(); i++)
601 pOut[i].x =
static_cast<double>(dmy1[i]);
602 pOut[i].y =
static_cast<double>(dmy2[i]);
603 pOut[i].z =
static_cast<double>(dmy3[i]);
625 const num_t x0,
const num_t y0,
const num_t z0,
626 const num_t maxRadiusSqr,
627 std::vector<std::pair<size_t, num_t>>& out_indices_dist)
const 631 out_indices_dist.clear();
634 const num_t xyz[3] = {x0, y0, z0};
636 &xyz[0], maxRadiusSqr, out_indices_dist,
639 return out_indices_dist.size();
660 const num_t x0,
const num_t y0,
const num_t maxRadiusSqr,
661 std::vector<std::pair<size_t, num_t>>& out_indices_dist)
const 665 out_indices_dist.clear();
668 const num_t xyz[2] = {x0, y0};
670 &xyz[0], maxRadiusSqr, out_indices_dist,
673 return out_indices_dist.size();
695 float x0,
float y0,
float z0,
size_t knn, std::vector<size_t>& out_idx,
696 std::vector<float>& out_dist_sqr)
const 704 out_dist_sqr.resize(knn);
706 resultSet.
init(&out_idx[0], &out_dist_sqr[0]);
718 const TPoint3D& p0,
size_t N, std::vector<size_t>& outIdx,
719 std::vector<float>& outDistSqr)
const 722 static_cast<float>(p0.
x), static_cast<float>(p0.
y),
723 static_cast<float>(p0.
z), N, outIdx, outDistSqr);
738 template <
int _DIM = -1>
751 if (&o !=
this)
clear();
761 std::unique_ptr<kdtree_index_t>
index;
793 const size_t N =
derived().kdtree_get_point_count();
827 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.
void kdTreeNClosestPoint3DIdx(const TPoint3D &p0, size_t N, std::vector< size_t > &outIdx, std::vector< float > &outDistSqr) const
KDTreeCapable()
Constructor.
#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 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 = ...
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
double x
X,Y,Z coordinates.
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.
bool m_kdtree_is_uptodate
whether the KD tree needs to be rebuilt or not.
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::vector< num_t > query_point
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.
TKDTreeDataHolder m_kdtreeNd_data
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 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.