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