9 #ifndef KDTreeCapable_H 10 #define KDTreeCapable_H 66 template <
class Derived,
typename num_t =
float,
typename metric_t = nanoflann::L2_Simple_Adaptor<num_t,Derived> >
81 inline const Derived&
derived()
const {
return *
static_cast<const Derived*
>(
this); }
83 inline Derived&
derived() {
return *
static_cast<Derived*
>(
this); }
126 const size_t knn = 1;
129 resultSet.
init(&ret_index, &out_dist_sqr );
136 out_x =
derived().kdtree_get_pt(ret_index,0);
137 out_y =
derived().kdtree_get_pt(ret_index,1);
147 float &out_dist_sqr )
const 153 const size_t knn = 1;
156 resultSet.
init(&ret_index, &out_dist_sqr );
181 float closerx,closery,closer_dist;
214 float &out_dist_sqr1,
215 float &out_dist_sqr2 )
const 221 const size_t knn = 2;
222 size_t ret_indexes[2];
225 resultSet.
init(&ret_indexes[0], &ret_sqdist[0] );
232 out_x1 =
derived().kdtree_get_pt(ret_indexes[0],0);
233 out_y1 =
derived().kdtree_get_pt(ret_indexes[0],1);
234 out_dist_sqr1 = ret_sqdist[0];
236 out_x2 =
derived().kdtree_get_pt(ret_indexes[1],0);
237 out_y2 =
derived().kdtree_get_pt(ret_indexes[1],1);
238 out_dist_sqr2 = ret_sqdist[0];
245 float dmy1,dmy2,dmy3,dmy4;
247 pOut1.
x=
static_cast<double>(dmy1);
248 pOut1.
y=
static_cast<double>(dmy2);
249 pOut2.
x=
static_cast<double>(dmy3);
250 pOut2.
y=
static_cast<double>(dmy4);
275 std::vector<float> &out_x,
276 std::vector<float> &out_y,
277 std::vector<float> &out_dist_sqr )
const 283 std::vector<size_t> ret_indexes(knn);
286 out_dist_sqr.resize(knn);
289 resultSet.
init(&ret_indexes[0], &out_dist_sqr[0] );
295 for (
size_t i=0;i<knn;i++)
297 out_x[i] =
derived().kdtree_get_pt(ret_indexes[i],0);
298 out_y[i] =
derived().kdtree_get_pt(ret_indexes[i],1);
305 std::vector<float> dmy1,dmy2;
306 std::vector<size_t> res=
kdTreeNClosestPoint2D(static_cast<float>(p0.
x),static_cast<float>(p0.
y),N,dmy1,dmy2,outDistSqr);
307 pOut.resize(dmy1.size());
308 for (
size_t i=0;i<dmy1.size();i++) {
309 pOut[i].x=
static_cast<double>(dmy1[i]);
310 pOut[i].y=
static_cast<double>(dmy2[i]);
333 std::vector<size_t> &out_idx,
334 std::vector<float> &out_dist_sqr )
const 341 out_dist_sqr.resize(knn);
343 resultSet.
init(&out_idx[0], &out_dist_sqr[0] );
386 const size_t knn = 1;
389 resultSet.
init(&ret_index, &out_dist_sqr );
397 out_x =
derived().kdtree_get_pt(ret_index,0);
398 out_y =
derived().kdtree_get_pt(ret_index,1);
399 out_z =
derived().kdtree_get_pt(ret_index,2);
417 const size_t knn = 1;
420 resultSet.
init(&ret_index, &out_dist_sqr );
433 float dmy1,dmy2,dmy3;
434 size_t res=
kdTreeClosestPoint3D(static_cast<float>(p0.
x),static_cast<float>(p0.
y),static_cast<float>(p0.
z),dmy1,dmy2,dmy3,outDistSqr);
435 pOut.
x=
static_cast<double>(dmy1);
436 pOut.
y=
static_cast<double>(dmy2);
437 pOut.
z=
static_cast<double>(dmy3);
463 std::vector<float> &out_x,
464 std::vector<float> &out_y,
465 std::vector<float> &out_z,
466 std::vector<float> &out_dist_sqr )
const 472 std::vector<size_t> ret_indexes(knn);
476 out_dist_sqr.resize(knn);
479 resultSet.
init(&ret_indexes[0], &out_dist_sqr[0] );
486 for (
size_t i=0;i<knn;i++)
488 out_x[i] =
derived().kdtree_get_pt(ret_indexes[i],0);
489 out_y[i] =
derived().kdtree_get_pt(ret_indexes[i],1);
490 out_z[i] =
derived().kdtree_get_pt(ret_indexes[i],2);
518 std::vector<float> &out_x,
519 std::vector<float> &out_y,
520 std::vector<float> &out_z,
521 std::vector<size_t> &out_idx,
522 std::vector<float> &out_dist_sqr )
const 532 out_dist_sqr.resize(knn);
535 resultSet.
init(&out_idx[0], &out_dist_sqr[0] );
542 for (
size_t i=0;i<knn;i++)
544 out_x[i] =
derived().kdtree_get_pt(out_idx[i],0);
545 out_y[i] =
derived().kdtree_get_pt(out_idx[i],1);
546 out_z[i] =
derived().kdtree_get_pt(out_idx[i],2);
552 std::vector<float> dmy1,dmy2,dmy3;
553 kdTreeNClosestPoint3D(static_cast<float>(p0.
x),static_cast<float>(p0.
y),static_cast<float>(p0.
z),N,dmy1,dmy2,dmy3,outDistSqr);
554 pOut.resize(dmy1.size());
555 for (
size_t i=0;i<dmy1.size();i++) {
556 pOut[i].x=
static_cast<double>(dmy1[i]);
557 pOut[i].y=
static_cast<double>(dmy2[i]);
558 pOut[i].z=
static_cast<double>(dmy3[i]);
578 const num_t x0,
const num_t y0,
const num_t z0,
579 const num_t maxRadiusSqr,
580 std::vector<std::pair<size_t,num_t> >& out_indices_dist )
const 584 out_indices_dist.clear();
587 const num_t xyz[3] = {x0,y0,z0};
590 return out_indices_dist.size();
609 const num_t x0,
const num_t y0,
610 const num_t maxRadiusSqr,
611 std::vector<std::pair<size_t,num_t> >& out_indices_dist )
const 615 out_indices_dist.clear();
618 const num_t xyz[2] = {x0,y0};
621 return out_indices_dist.size();
645 std::vector<size_t> &out_idx,
646 std::vector<float> &out_dist_sqr )
const 653 out_dist_sqr.resize(knn);
655 resultSet.
init(&out_idx[0], &out_dist_sqr[0] );
676 template <
int _DIM = -1>
687 if (&o!=
this)
clear();
716 if (!m_kdtree_is_uptodate) { m_kdtree2d_data.clear(); m_kdtree3d_data.clear(); m_kdtreeNd_data.clear(); }
718 if (!m_kdtree2d_data.index)
721 m_kdtree2d_data.clear();
723 const size_t N =
derived().kdtree_get_point_count();
724 m_kdtree2d_data.m_num_points = N;
725 m_kdtree2d_data.m_dim = 2;
726 m_kdtree2d_data.query_point.resize(2);
730 m_kdtree2d_data.index->buildIndex();
732 m_kdtree_is_uptodate =
true;
741 if (!m_kdtree_is_uptodate) { m_kdtree2d_data.clear(); m_kdtree3d_data.clear(); m_kdtreeNd_data.clear(); }
743 if (!m_kdtree3d_data.index)
746 m_kdtree3d_data.clear();
748 const size_t N =
derived().kdtree_get_point_count();
749 m_kdtree3d_data.m_num_points = N;
750 m_kdtree3d_data.m_dim = 3;
751 m_kdtree3d_data.query_point.resize(3);
755 m_kdtree3d_data.index->buildIndex();
757 m_kdtree_is_uptodate =
true;
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...
~KDTreeCapable()
Destructor (nothing needed to do here)
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.
void kdTreeNClosestPoint2DIdx(const TPoint2D &p0, size_t N, std::vector< size_t > &outIdx, std::vector< float > &outDistSqr) const
void kdTreeTwoClosestPoint2D(const TPoint2D &p0, TPoint2D &pOut1, TPoint2D &pOut2, float &outDistSqr1, float &outDistSqr2) const
#define THROW_EXCEPTION(msg)
size_t kdTreeClosestPoint3D(const TPoint3D &p0, TPoint3D &pOut, float &outDistSqr) const
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 init(IndexType *indices_, DistanceType *dists_)
TKDTreeDataHolder< 2 > m_kdtree2d_data
size_t m_dim
Dimensionality. typ: 2,3.
const Derived & derived() const
CRTP helper method.
double z
X,Y,Z coordinates.
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), ... asking the child class for the dat...
Derived & derived()
CRTP helper method.
void clear()
Clear the contents of this container.
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
bool findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const
Find set of nearest neighbors to vec[0:dim-1].
void rebuild_kdTree_3D() const
Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the dat...
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.
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.
TKDTreeDataHolder()
Init the pointer to NULL.
std::vector< size_t > kdTreeNClosestPoint2D(const TPoint2D &p0, size_t N, std::vector< TPoint2D > &pOut, std::vector< float > &outDistSqr) const
~TKDTreeDataHolder()
Free memory (if allocated)
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
size_t radiusSearch(const ElementType *query_point, const DistanceType &radius, std::vector< std::pair< IndexType, DistanceType > > &IndicesDists, const SearchParams &searchParams) const
Find all the neighbors to query_point[0:dim-1] within a maximum radius.
nanoflann::KDTreeSingleIndexAdaptor< metric_t, Derived, _DIM > kdtree_index_t
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void delete_safe(T *&ptr)
Calls "delete" to free an object only if the pointer is not NULL, then set the pointer to NULL...
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.
void clear()
Free memory (if allocated)
kdtree_index_t * index
NULL or the up-to-date index.
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
KDTreeCapable< Derived, num_t, metric_t > self_t
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.
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.
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.