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();
723 const size_t N =
derived().kdtree_get_point_count();
748 const size_t N =
derived().kdtree_get_point_count();
A generic adaptor class for providing Nearest Neighbor (NN) lookup via the nanoflann library.
void kdTreeNClosestPoint3DIdx(const TPoint3D &p0, size_t N, std::vector< size_t > &outIdx, std::vector< float > &outDistSqr) const
size_t kdTreeClosestPoint3D(float x0, float y0, float z0, float &out_dist_sqr) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
TKDTreeSearchParams kdtree_search_params
Parameters to tune the ANN searches.
float kdTreeClosestPoint2DsqrError(const TPoint2D &p0) const
~KDTreeCapable()
Destructor (nothing needed to do here)
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.
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.
TKDTreeDataHolder< 3 > m_kdtree3d_data
TKDTreeDataHolder m_kdtreeNd_data
size_t kdTreeClosestPoint2D(const TPoint2D &p0, TPoint2D &pOut, float &outDistSqr) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
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 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.
Derived & derived()
CRTP helper method.
TKDTreeDataHolder< 2 > m_kdtree2d_data
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.
float kdTreeClosestPoint2DsqrError(float x0, float y0) const
Like kdTreeClosestPoint2D, but just return the square error from some point to its closest neighbor.
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...
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...
bool m_kdtree_is_uptodate
whether the KD tree needs to be rebuilt or not.
void kdTreeNClosestPoint2DIdx(const TPoint2D &p0, size_t N, std::vector< size_t > &outIdx, std::vector< float > &outDistSqr) const
const Derived & derived() const
CRTP helper method.
std::vector< size_t > kdTreeNClosestPoint2D(const TPoint2D &p0, size_t N, std::vector< TPoint2D > &pOut, std::vector< float > &outDistSqr) const
KDTreeCapable< Derived, num_t, metric_t > self_t
size_t kdTreeClosestPoint2D(float x0, float y0, float &out_dist_sqr) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
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.
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
This is an overloaded member function, provided for convenience. It differs from the above function o...
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.
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 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 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 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.
KDTreeCapable()
Constructor.
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.
void buildIndex()
Builds the index.
void findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const
Find set of nearest neighbors to vec[0:dim-1].
void init(IndexType *indices_, DistanceType *dists_)
#define THROW_EXCEPTION(msg)
void delete_safe(T *&ptr)
Calls "delete" to free an object only if the pointer is not NULL, then set the pointer to NULL.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Internal structure with the KD-tree representation (mainly used to avoid copying pointers with the = ...
void clear()
Free memory (if allocated)
TKDTreeDataHolder()
Init the pointer to NULL.
nanoflann::KDTreeSingleIndexAdaptor< metric_t, Derived, _DIM > kdtree_index_t
~TKDTreeDataHolder()
Free memory (if allocated)
size_t m_dim
Dimensionality. typ: 2,3.
kdtree_index_t * index
NULL or the up-to-date index.
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!
TKDTreeDataHolder & operator=(const TKDTreeDataHolder &o)
Copy operator: It actually does NOT copy the kd-tree, a new object will be created if required!
size_t leaf_max_size
Max points per leaf.
double z
X,Y,Z coordinates.
Parameters (see http://code.google.com/p/nanoflann/ for help choosing the parameters)
Search options for KDTreeSingleIndexAdaptor::findNeighbors()