Main MRPT website > C++ reference for MRPT 1.4.0
KDTreeCapable.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9#ifndef KDTreeCapable_H
10#define KDTreeCapable_H
11
13
14// nanoflann library:
17
18namespace mrpt
19{
20 namespace math
21 {
22 /** \addtogroup kdtree_grp KD-Trees
23 * \ingroup mrpt_base_grp
24 * @{ */
25
26
27 /** A generic adaptor class for providing Nearest Neighbor (NN) lookup via the `nanoflann` library.
28 * This makes use of the CRTP design pattern.
29 *
30 * Derived classes must be aware of the need to call "kdtree_mark_as_outdated()" when the data points
31 * change to mark the cached KD-tree (an "index") as invalid, and also implement the following interface
32 * (note that these are not virtual functions due to the usage of CRTP):
33 *
34 * \code
35 * // Must return the number of data points
36 * inline size_t kdtree_get_point_count() const { ... }
37 *
38 * // Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
39 * inline float kdtree_distance(const float *p1, const size_t idx_p2,size_t size) const { ... }
40 *
41 * // Returns the dim'th component of the idx'th point in the class:
42 * inline num_t kdtree_get_pt(const size_t idx, int dim) const { ... }
43 *
44 * // Optional bounding-box computation: return false to default to a standard bbox computation loop.
45 * // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
46 * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
47 * template <class BBOX>
48 * bool kdtree_get_bbox(BBOX &bb) const
49 * {
50 * bb[0].low = ...; bb[0].high = ...; // "x" limits
51 * return true;
52 * }
53 *
54 * \endcode
55 *
56 * The KD-tree index will be built on demand only upon call of any of the query methods provided by this class.
57 *
58 * Notice that there is only ONE internal cached KD-tree, so if a method to query a 2D point is called,
59 * then another method for 3D points, then again the 2D method, three KD-trees will be built. So, try
60 * to group all the calls for a given dimensionality together or build different class instances for
61 * queries of each dimensionality, etc.
62 *
63 * \sa See some of the derived classes for example implementations. See also the documentation of nanoflann
64 * \ingroup mrpt_base_grp
65 */
66 template <class Derived, typename num_t = float, typename metric_t = nanoflann::L2_Simple_Adaptor<num_t,Derived> >
68 {
69 public:
70 // Types ---------------
72 // ---------------------
73
74 /// Constructor
75 inline KDTreeCapable() : m_kdtree_is_uptodate(false) { }
76
77 /// Destructor (nothing needed to do here)
78 inline ~KDTreeCapable() { }
79
80 /// CRTP helper method
81 inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
82 /// CRTP helper method
83 inline Derived& derived() { return *static_cast<Derived*>(this); }
84
86 {
89 {
90 }
91 size_t leaf_max_size; //!< Max points per leaf
92 };
93
94 TKDTreeSearchParams kdtree_search_params; //!< Parameters to tune the ANN searches
95
96 /** @name Public utility methods to query the KD-tree
97 @{ */
98
99 /** KD Tree-based search for the closest point (only ONE) to some given 2D coordinates.
100 * This method automatically build the "m_kdtree_data" structure when:
101 * - It is called for the first time
102 * - The map has changed
103 * - The KD-tree was build for 3D.
104 *
105 * \param x0 The X coordinate of the query.
106 * \param y0 The Y coordinate of the query.
107 * \param out_x The X coordinate of the found closest correspondence.
108 * \param out_y The Y coordinate of the found closest correspondence.
109 * \param out_dist_sqr The square distance between the query and the returned point.
110 *
111 * \return The index of the closest point in the map array.
112 * \sa kdTreeClosestPoint3D, kdTreeTwoClosestPoint2D
113 */
114 inline size_t kdTreeClosestPoint2D(
115 float x0,
116 float y0,
117 float &out_x,
118 float &out_y,
119 float &out_dist_sqr
120 ) const
121 {
123 rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
124 if ( !m_kdtree2d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
125
126 const size_t knn = 1; // Number of points to retrieve
127 size_t ret_index;
128 nanoflann::KNNResultSet<num_t> resultSet(knn);
129 resultSet.init(&ret_index, &out_dist_sqr );
130
134
135 // Copy output to user vars:
136 out_x = derived().kdtree_get_pt(ret_index,0);
137 out_y = derived().kdtree_get_pt(ret_index,1);
138
139 return ret_index;
141 }
142
143 /// \overload
144 inline size_t kdTreeClosestPoint2D(
145 float x0,
146 float y0,
147 float &out_dist_sqr ) const
148 {
150 rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
151 if ( !m_kdtree2d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
152
153 const size_t knn = 1; // Number of points to retrieve
154 size_t ret_index;
155 nanoflann::KNNResultSet<num_t> resultSet(knn);
156 resultSet.init(&ret_index, &out_dist_sqr );
157
161
162 return ret_index;
164 }
165
166 /// \overload
167 inline size_t kdTreeClosestPoint2D(const TPoint2D &p0,TPoint2D &pOut,float &outDistSqr) const {
168 float dmy1,dmy2;
169 size_t res=kdTreeClosestPoint2D(static_cast<float>(p0.x),static_cast<float>(p0.y),dmy1,dmy2,outDistSqr);
170 pOut.x=dmy1;
171 pOut.y=dmy2;
172 return res;
173 }
174
175 /** Like kdTreeClosestPoint2D, but just return the square error from some point to its closest neighbor.
176 */
178 float x0,
179 float y0 ) const
180 {
181 float closerx,closery,closer_dist;
182 kdTreeClosestPoint2D(x0,y0, closerx,closery,closer_dist);
183 return closer_dist;
184 }
185
186 inline float kdTreeClosestPoint2DsqrError(const TPoint2D &p0) const {
187 return kdTreeClosestPoint2DsqrError(static_cast<float>(p0.x),static_cast<float>(p0.y));
188 }
189
190 /** KD Tree-based search for the TWO closest point to some given 2D coordinates.
191 * This method automatically build the "m_kdtree_data" structure when:
192 * - It is called for the first time
193 * - The map has changed
194 * - The KD-tree was build for 3D.
195 *
196 * \param x0 The X coordinate of the query.
197 * \param y0 The Y coordinate of the query.
198 * \param out_x1 The X coordinate of the first correspondence.
199 * \param out_y1 The Y coordinate of the first correspondence.
200 * \param out_x2 The X coordinate of the second correspondence.
201 * \param out_y2 The Y coordinate of the second correspondence.
202 * \param out_dist_sqr1 The square distance between the query and the first returned point.
203 * \param out_dist_sqr2 The square distance between the query and the second returned point.
204 *
205 * \sa kdTreeClosestPoint2D
206 */
208 float x0,
209 float y0,
210 float &out_x1,
211 float &out_y1,
212 float &out_x2,
213 float &out_y2,
214 float &out_dist_sqr1,
215 float &out_dist_sqr2 ) const
216 {
218 rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
219 if ( !m_kdtree2d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
220
221 const size_t knn = 2; // Number of points to retrieve
222 size_t ret_indexes[2];
223 float ret_sqdist[2];
224 nanoflann::KNNResultSet<num_t> resultSet(knn);
225 resultSet.init(&ret_indexes[0], &ret_sqdist[0] );
226
230
231 // Copy output to user vars:
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];
235
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];
239
241 }
242
243
244 inline void kdTreeTwoClosestPoint2D(const TPoint2D &p0,TPoint2D &pOut1,TPoint2D &pOut2,float &outDistSqr1,float &outDistSqr2) const {
245 float dmy1,dmy2,dmy3,dmy4;
246 kdTreeTwoClosestPoint2D(p0.x,p0.y,dmy1,dmy2,dmy3,dmy4,outDistSqr1,outDistSqr2);
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);
251 }
252
253 /** KD Tree-based search for the N closest point to some given 2D coordinates.
254 * This method automatically build the "m_kdtree_data" structure when:
255 * - It is called for the first time
256 * - The map has changed
257 * - The KD-tree was build for 3D.
258 *
259 * \param x0 The X coordinate of the query.
260 * \param y0 The Y coordinate of the query.
261 * \param N The number of closest points to search.
262 * \param out_x The vector containing the X coordinates of the correspondences.
263 * \param out_y The vector containing the Y coordinates of the correspondences.
264 * \param out_dist_sqr The vector containing the square distance between the query and the returned points.
265 *
266 * \return The list of indices
267 * \sa kdTreeClosestPoint2D
268 * \sa kdTreeTwoClosestPoint2D
269 */
270 inline
271 std::vector<size_t> kdTreeNClosestPoint2D(
272 float x0,
273 float y0,
274 size_t knn,
275 std::vector<float> &out_x,
276 std::vector<float> &out_y,
277 std::vector<float> &out_dist_sqr ) const
278 {
280 rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
281 if ( !m_kdtree2d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
282
283 std::vector<size_t> ret_indexes(knn);
284 out_x.resize(knn);
285 out_y.resize(knn);
286 out_dist_sqr.resize(knn);
287
288 nanoflann::KNNResultSet<num_t> resultSet(knn);
289 resultSet.init(&ret_indexes[0], &out_dist_sqr[0] );
290
294
295 for (size_t i=0;i<knn;i++)
296 {
297 out_x[i] = derived().kdtree_get_pt(ret_indexes[i],0);
298 out_y[i] = derived().kdtree_get_pt(ret_indexes[i],1);
299 }
300 return ret_indexes;
302 }
303
304 inline std::vector<size_t> kdTreeNClosestPoint2D(const TPoint2D &p0,size_t N,std::vector<TPoint2D> &pOut,std::vector<float> &outDistSqr) const {
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]);
311 }
312 return res;
313 }
314
315 /** KD Tree-based search for the N closest point to some given 2D coordinates and returns their indexes.
316 * This method automatically build the "m_kdtree_data" structure when:
317 * - It is called for the first time
318 * - The map has changed
319 * - The KD-tree was build for 3D.
320 *
321 * \param x0 The X coordinate of the query.
322 * \param y0 The Y coordinate of the query.
323 * \param N The number of closest points to search.
324 * \param out_idx The indexes of the found closest correspondence.
325 * \param out_dist_sqr The square distance between the query and the returned point.
326 *
327 * \sa kdTreeClosestPoint2D
328 */
330 float x0,
331 float y0,
332 size_t knn,
333 std::vector<size_t> &out_idx,
334 std::vector<float> &out_dist_sqr ) const
335 {
337 rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
338 if ( !m_kdtree2d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
339
340 out_idx.resize(knn);
341 out_dist_sqr.resize(knn);
342 nanoflann::KNNResultSet<num_t> resultSet(knn);
343 resultSet.init(&out_idx[0], &out_dist_sqr[0] );
344
349 }
350
351 inline void kdTreeNClosestPoint2DIdx(const TPoint2D &p0,size_t N,std::vector<size_t> &outIdx,std::vector<float> &outDistSqr) const {
352 return kdTreeNClosestPoint2DIdx(static_cast<float>(p0.x),static_cast<float>(p0.y),N,outIdx,outDistSqr);
353 }
354
355 /** KD Tree-based search for the closest point (only ONE) to some given 3D coordinates.
356 * This method automatically build the "m_kdtree_data" structure when:
357 * - It is called for the first time
358 * - The map has changed
359 * - The KD-tree was build for 2D.
360 *
361 * \param x0 The X coordinate of the query.
362 * \param y0 The Y coordinate of the query.
363 * \param z0 The Z coordinate of the query.
364 * \param out_x The X coordinate of the found closest correspondence.
365 * \param out_y The Y coordinate of the found closest correspondence.
366 * \param out_z The Z coordinate of the found closest correspondence.
367 * \param out_dist_sqr The square distance between the query and the returned point.
368 *
369 * \return The index of the closest point in the map array.
370 * \sa kdTreeClosestPoint2D
371 */
372 inline size_t kdTreeClosestPoint3D(
373 float x0,
374 float y0,
375 float z0,
376 float &out_x,
377 float &out_y,
378 float &out_z,
379 float &out_dist_sqr
380 ) const
381 {
383 rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
384 if ( !m_kdtree3d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
385
386 const size_t knn = 1; // Number of points to retrieve
387 size_t ret_index;
388 nanoflann::KNNResultSet<num_t> resultSet(knn);
389 resultSet.init(&ret_index, &out_dist_sqr );
390
395
396 // Copy output to user vars:
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);
400
401 return ret_index;
403 }
404
405 /// \overload
406 inline size_t kdTreeClosestPoint3D(
407 float x0,
408 float y0,
409 float z0,
410 float &out_dist_sqr
411 ) const
412 {
414 rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
415 if ( !m_kdtree3d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
416
417 const size_t knn = 1; // Number of points to retrieve
418 size_t ret_index;
419 nanoflann::KNNResultSet<num_t> resultSet(knn);
420 resultSet.init(&ret_index, &out_dist_sqr );
421
426
427 return ret_index;
429 }
430
431 /// \overload
432 inline size_t kdTreeClosestPoint3D(const TPoint3D &p0,TPoint3D &pOut,float &outDistSqr) const {
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);
438 return res;
439 }
440
441 /** KD Tree-based search for the N closest points to some given 3D coordinates.
442 * This method automatically build the "m_kdtree_data" structure when:
443 * - It is called for the first time
444 * - The map has changed
445 * - The KD-tree was build for 2D.
446 *
447 * \param x0 The X coordinate of the query.
448 * \param y0 The Y coordinate of the query.
449 * \param z0 The Z coordinate of the query.
450 * \param N The number of closest points to search.
451 * \param out_x The vector containing the X coordinates of the correspondences.
452 * \param out_y The vector containing the Y coordinates of the correspondences.
453 * \param out_z The vector containing the Z coordinates of the correspondences.
454 * \param out_dist_sqr The vector containing the square distance between the query and the returned points.
455 *
456 * \sa kdTreeNClosestPoint2D
457 */
459 float x0,
460 float y0,
461 float z0,
462 size_t knn,
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
467 {
469 rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
470 if ( !m_kdtree3d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
471
472 std::vector<size_t> ret_indexes(knn);
473 out_x.resize(knn);
474 out_y.resize(knn);
475 out_z.resize(knn);
476 out_dist_sqr.resize(knn);
477
478 nanoflann::KNNResultSet<num_t> resultSet(knn);
479 resultSet.init(&ret_indexes[0], &out_dist_sqr[0] );
480
485
486 for (size_t i=0;i<knn;i++)
487 {
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);
491 }
493 }
494
495 /** KD Tree-based search for the N closest points to some given 3D coordinates.
496 * This method automatically build the "m_kdtree_data" structure when:
497 * - It is called for the first time
498 * - The map has changed
499 * - The KD-tree was build for 2D.
500 *
501 * \param x0 The X coordinate of the query.
502 * \param y0 The Y coordinate of the query.
503 * \param z0 The Z coordinate of the query.
504 * \param N The number of closest points to search.
505 * \param out_x The vector containing the X coordinates of the correspondences.
506 * \param out_y The vector containing the Y coordinates of the correspondences.
507 * \param out_z The vector containing the Z coordinates of the correspondences.
508 * \param out_idx The vector containing the indexes of the correspondences.
509 * \param out_dist_sqr The vector containing the square distance between the query and the returned points.
510 *
511 * \sa kdTreeNClosestPoint2D
512 */
514 float x0,
515 float y0,
516 float z0,
517 size_t knn,
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
523 {
525 rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
526 if ( !m_kdtree3d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
527
528 out_x.resize(knn);
529 out_y.resize(knn);
530 out_z.resize(knn);
531 out_idx.resize(knn);
532 out_dist_sqr.resize(knn);
533
534 nanoflann::KNNResultSet<num_t> resultSet(knn);
535 resultSet.init(&out_idx[0], &out_dist_sqr[0] );
536
541
542 for (size_t i=0;i<knn;i++)
543 {
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);
547 }
549 }
550
551 inline void kdTreeNClosestPoint3D(const TPoint3D &p0,size_t N,std::vector<TPoint3D> &pOut,std::vector<float> &outDistSqr) const {
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]);
559 }
560 }
561
562 /** KD Tree-based search for all the points within a given radius of some 3D point.
563 * This method automatically build the "m_kdtree_data" structure when:
564 * - It is called for the first time
565 * - The map has changed
566 * - The KD-tree was build for 2D.
567 *
568 * \param x0 The X coordinate of the query.
569 * \param y0 The Y coordinate of the query.
570 * \param z0 The Z coordinate of the query.
571 * \param maxRadiusSqr The square of the desired search radius.
572 * \param out_indices_dist The output list, with pairs of indeces/squared distances for the found correspondences.
573 * \return Number of found points.
574 *
575 * \sa kdTreeRadiusSearch2D, kdTreeNClosestPoint3DIdx
576 */
577 inline size_t kdTreeRadiusSearch3D(
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
581 {
583 rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
584 out_indices_dist.clear();
586 {
587 const num_t xyz[3] = {x0,y0,z0};
588 m_kdtree3d_data.index->radiusSearch(&xyz[0], maxRadiusSqr, out_indices_dist, nanoflann::SearchParams() );
589 }
590 return out_indices_dist.size();
592 }
593
594 /** KD Tree-based search for all the points within a given radius of some 2D point.
595 * This method automatically build the "m_kdtree_data" structure when:
596 * - It is called for the first time
597 * - The map has changed
598 * - The KD-tree was build for 3D.
599 *
600 * \param x0 The X coordinate of the query.
601 * \param y0 The Y coordinate of the query.
602 * \param maxRadiusSqr The square of the desired search radius.
603 * \param out_indices_dist The output list, with pairs of indeces/squared distances for the found correspondences.
604 * \return Number of found points.
605 *
606 * \sa kdTreeRadiusSearch3D, kdTreeNClosestPoint2DIdx
607 */
608 inline size_t kdTreeRadiusSearch2D(
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
612 {
614 rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
615 out_indices_dist.clear();
617 {
618 const num_t xyz[2] = {x0,y0};
619 m_kdtree2d_data.index->radiusSearch(&xyz[0], maxRadiusSqr, out_indices_dist, nanoflann::SearchParams() );
620 }
621 return out_indices_dist.size();
623 }
624
625 /** KD Tree-based search for the N closest point to some given 3D coordinates and returns their indexes.
626 * This method automatically build the "m_kdtree_data" structure when:
627 * - It is called for the first time
628 * - The map has changed
629 * - The KD-tree was build for 2D.
630 *
631 * \param x0 The X coordinate of the query.
632 * \param y0 The Y coordinate of the query.
633 * \param z0 The Z coordinate of the query.
634 * \param N The number of closest points to search.
635 * \param out_idx The indexes of the found closest correspondence.
636 * \param out_dist_sqr The square distance between the query and the returned point.
637 *
638 * \sa kdTreeClosestPoint2D, kdTreeRadiusSearch3D
639 */
641 float x0,
642 float y0,
643 float z0,
644 size_t knn,
645 std::vector<size_t> &out_idx,
646 std::vector<float> &out_dist_sqr ) const
647 {
649 rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
650 if ( !m_kdtree3d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
651
652 out_idx.resize(knn);
653 out_dist_sqr.resize(knn);
654 nanoflann::KNNResultSet<num_t> resultSet(knn);
655 resultSet.init(&out_idx[0], &out_dist_sqr[0] );
656
662 }
663
664 inline void kdTreeNClosestPoint3DIdx(const TPoint3D &p0,size_t N,std::vector<size_t> &outIdx,std::vector<float> &outDistSqr) const {
665 kdTreeNClosestPoint3DIdx(static_cast<float>(p0.x),static_cast<float>(p0.y),static_cast<float>(p0.z),N,outIdx,outDistSqr);
666 }
667
668 /* @} */
669
670 protected:
671 /** To be called by child classes when KD tree data changes. */
672 inline void kdtree_mark_as_outdated() const { m_kdtree_is_uptodate = false; }
673
674 private:
675 /** Internal structure with the KD-tree representation (mainly used to avoid copying pointers with the = operator) */
676 template <int _DIM = -1>
678 {
679 /** Init the pointer to NULL. */
680 inline TKDTreeDataHolder() : index(NULL),m_dim(_DIM), m_num_points(0) { }
681
682 /** Copy constructor: It actually does NOT copy the kd-tree, a new object will be created if required! */
683 inline TKDTreeDataHolder(const TKDTreeDataHolder &) : index(NULL),m_dim(_DIM), m_num_points(0) { }
684
685 /** Copy operator: It actually does NOT copy the kd-tree, a new object will be created if required! */
687 if (&o!=this) clear();
688 return *this;
689 }
690
691 /** Free memory (if allocated) */
692 inline ~TKDTreeDataHolder() { clear(); }
693
694 /** Free memory (if allocated) */
696
698
699 kdtree_index_t *index; //!< NULL or the up-to-date index
700
701 std::vector<num_t> query_point;
702 size_t m_dim; //!< Dimensionality. typ: 2,3
704 };
705
709 mutable bool m_kdtree_is_uptodate; //!< whether the KD tree needs to be rebuilt or not.
710
711 /// Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the data points.
712 void rebuild_kdTree_2D() const
713 {
714 typedef typename TKDTreeDataHolder<2>::kdtree_index_t tree2d_t;
715
717
719 {
720 // Erase previous tree:
722 // And build new index:
723 const size_t N = derived().kdtree_get_point_count();
727 if (N)
728 {
731 }
733 }
734 }
735
736 /// Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the data points.
737 void rebuild_kdTree_3D() const
738 {
739 typedef typename TKDTreeDataHolder<3>::kdtree_index_t tree3d_t;
740
742
744 {
745 // Erase previous tree:
747 // And build new index:
748 const size_t N = derived().kdtree_get_point_count();
752 if (N)
753 {
756 }
758 }
759 }
760
761 }; // end of KDTreeCapable
762
763 /** @} */ // end of grouping
764
765 } // End of namespace
766} // End of namespace
767#endif
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.
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_)
Definition nanoflann.hpp:76
#define MRPT_START
#define MRPT_END
#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.
Definition bits.h:165
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.
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!
Lightweight 2D point.
double y
X,Y coordinates.
Lightweight 3D point.
double z
X,Y,Z coordinates.
Parameters (see http://code.google.com/p/nanoflann/ for help choosing the parameters)
Search options for KDTreeSingleIndexAdaptor::findNeighbors()



Page generated by Doxygen 1.9.7 for MRPT 1.4.0 SVN: at Tue Jun 13 14:10:35 UTC 2023