Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 #pragma once
10 
11 // nanoflann library:
12 #include <nanoflann.hpp>
14 #include <memory> // unique_ptr
15 
16 namespace mrpt
17 {
18 namespace math
19 {
20 /** \addtogroup kdtree_grp KD-Trees
21  * \ingroup mrpt_math_grp
22  * @{ */
23 
24 /** A generic adaptor class for providing Nearest Neighbor (NN) lookup via the
25  * `nanoflann` library.
26  * This makes use of the CRTP design pattern.
27  *
28  * Derived classes must be aware of the need to call
29  * "kdtree_mark_as_outdated()" when the data points
30  * change to mark the cached KD-tree (an "index") as invalid, and also
31  * 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
39  * point with index "idx_p2" stored in the class:
40  * inline float kdtree_distance(const float *p1, const size_t idx_p2,size_t
41  * size) const { ... }
42  *
43  * // Returns the dim'th component of the idx'th point in the class:
44  * inline num_t kdtree_get_pt(const size_t idx, int dim) const { ... }
45  *
46  * // Optional bounding-box computation: return false to default to a standard
47  * bbox computation loop.
48  * // Return true if the BBOX was already computed by the class and returned
49  * in "bb" so it can be avoided to redo it again.
50  * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3
51  * for point clouds)
52  * template <class BBOX>
53  * bool kdtree_get_bbox(BBOX &bb) const
54  * {
55  * bb[0].low = ...; bb[0].high = ...; // "x" limits
56  * return true;
57  * }
58  *
59  * \endcode
60  *
61  * The KD-tree index will be built on demand only upon call of any of the query
62  * methods provided by this class.
63  *
64  * Notice that there is only ONE internal cached KD-tree, so if a method to
65  * query a 2D point is called,
66  * then another method for 3D points, then again the 2D method, three KD-trees
67  * will be built. So, try
68  * to group all the calls for a given dimensionality together or build
69  * different class instances for
70  * queries of each dimensionality, etc.
71  *
72  * \sa See some of the derived classes for example implementations. See also
73  * the documentation of nanoflann
74  * \ingroup mrpt_math_grp
75  */
76 template <
77  class Derived, typename num_t = float,
80 {
81  public:
82  // Types ---------------
84  // ---------------------
85 
86  /// Constructor
87  inline KDTreeCapable() : m_kdtree_is_uptodate(false) {}
88  /// CRTP helper method
89  inline const Derived& derived() const
90  {
91  return *static_cast<const Derived*>(this);
92  }
93  /// CRTP helper method
94  inline Derived& derived() { return *static_cast<Derived*>(this); }
96  {
98  /** Max points per leaf */
99  size_t leaf_max_size;
100  };
101 
102  /** Parameters to tune the ANN searches */
103  TKDTreeSearchParams kdtree_search_params;
104 
105  /** @name Public utility methods to query the KD-tree
106  @{ */
107 
108  /** KD Tree-based search for the closest point (only ONE) to some given 2D
109  *coordinates.
110  * This method automatically build the "m_kdtree_data" structure when:
111  * - It is called for the first time
112  * - The map has changed
113  * - The KD-tree was build for 3D.
114  *
115  * \param x0 The X coordinate of the query.
116  * \param y0 The Y coordinate of the query.
117  * \param out_x The X coordinate of the found closest correspondence.
118  * \param out_y The Y coordinate of the found closest correspondence.
119  * \param out_dist_sqr The square distance between the query and the
120  *returned point.
121  *
122  * \return The index of the closest point in the map array.
123  * \sa kdTreeClosestPoint3D, kdTreeTwoClosestPoint2D
124  */
125  inline size_t kdTreeClosestPoint2D(
126  float x0, float y0, float& out_x, float& out_y,
127  float& out_dist_sqr) const
128  {
129  MRPT_START
130  rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
132  THROW_EXCEPTION("There are no points in the KD-tree.");
133 
134  const size_t knn = 1; // Number of points to retrieve
135  size_t ret_index;
136  nanoflann::KNNResultSet<num_t> resultSet(knn);
137  resultSet.init(&ret_index, &out_dist_sqr);
138 
141  m_kdtree2d_data.index->findNeighbors(
142  resultSet, &m_kdtree2d_data.query_point[0],
144 
145  // Copy output to user vars:
146  out_x = derived().kdtree_get_pt(ret_index, 0);
147  out_y = derived().kdtree_get_pt(ret_index, 1);
148 
149  return ret_index;
150  MRPT_END
151  }
152 
153  /// \overload
154  inline size_t kdTreeClosestPoint2D(
155  float x0, float y0, float& out_dist_sqr) const
156  {
157  MRPT_START
158  rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
160  THROW_EXCEPTION("There are no points in the KD-tree.");
161 
162  const size_t knn = 1; // Number of points to retrieve
163  size_t ret_index;
164  nanoflann::KNNResultSet<num_t> resultSet(knn);
165  resultSet.init(&ret_index, &out_dist_sqr);
166 
169  m_kdtree2d_data.index->findNeighbors(
170  resultSet, &m_kdtree2d_data.query_point[0],
172 
173  return ret_index;
174  MRPT_END
175  }
176 
177  /// \overload
178  inline size_t kdTreeClosestPoint2D(
179  const TPoint2D& p0, TPoint2D& pOut, float& outDistSqr) const
180  {
181  float dmy1, dmy2;
182  size_t res = kdTreeClosestPoint2D(
183  static_cast<float>(p0.x), static_cast<float>(p0.y), dmy1, dmy2,
184  outDistSqr);
185  pOut.x = dmy1;
186  pOut.y = dmy2;
187  return res;
188  }
189 
190  /** Like kdTreeClosestPoint2D, but just return the square error from some
191  * point to its closest neighbor.
192  */
193  inline float kdTreeClosestPoint2DsqrError(float x0, float y0) const
194  {
195  float closerx, closery, closer_dist;
196  kdTreeClosestPoint2D(x0, y0, closerx, closery, closer_dist);
197  return closer_dist;
198  }
199 
200  inline float kdTreeClosestPoint2DsqrError(const TPoint2D& p0) const
201  {
203  static_cast<float>(p0.x), static_cast<float>(p0.y));
204  }
205 
206  /** KD Tree-based search for the TWO closest point to some given 2D
207  *coordinates.
208  * This method automatically build the "m_kdtree_data" structure when:
209  * - It is called for the first time
210  * - The map has changed
211  * - The KD-tree was build for 3D.
212  *
213  * \param x0 The X coordinate of the query.
214  * \param y0 The Y coordinate of the query.
215  * \param out_x1 The X coordinate of the first correspondence.
216  * \param out_y1 The Y coordinate of the first correspondence.
217  * \param out_x2 The X coordinate of the second correspondence.
218  * \param out_y2 The Y coordinate of the second correspondence.
219  * \param out_dist_sqr1 The square distance between the query and the first
220  *returned point.
221  * \param out_dist_sqr2 The square distance between the query and the
222  *second returned point.
223  *
224  * \sa kdTreeClosestPoint2D
225  */
227  float x0, float y0, float& out_x1, float& out_y1, float& out_x2,
228  float& out_y2, float& out_dist_sqr1, float& out_dist_sqr2) const
229  {
230  MRPT_START
231  rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
233  THROW_EXCEPTION("There are no points in the KD-tree.");
234 
235  const size_t knn = 2; // Number of points to retrieve
236  size_t ret_indexes[2];
237  float ret_sqdist[2];
238  nanoflann::KNNResultSet<num_t> resultSet(knn);
239  resultSet.init(&ret_indexes[0], &ret_sqdist[0]);
240 
243  m_kdtree2d_data.index->findNeighbors(
244  resultSet, &m_kdtree2d_data.query_point[0],
246 
247  // Copy output to user vars:
248  out_x1 = derived().kdtree_get_pt(ret_indexes[0], 0);
249  out_y1 = derived().kdtree_get_pt(ret_indexes[0], 1);
250  out_dist_sqr1 = ret_sqdist[0];
251 
252  out_x2 = derived().kdtree_get_pt(ret_indexes[1], 0);
253  out_y2 = derived().kdtree_get_pt(ret_indexes[1], 1);
254  out_dist_sqr2 = ret_sqdist[0];
255 
256  MRPT_END
257  }
258 
260  const TPoint2D& p0, TPoint2D& pOut1, TPoint2D& pOut2,
261  float& outDistSqr1, float& outDistSqr2) const
262  {
263  float dmy1, dmy2, dmy3, dmy4;
265  p0.x, p0.y, dmy1, dmy2, dmy3, dmy4, outDistSqr1, outDistSqr2);
266  pOut1.x = static_cast<double>(dmy1);
267  pOut1.y = static_cast<double>(dmy2);
268  pOut2.x = static_cast<double>(dmy3);
269  pOut2.y = static_cast<double>(dmy4);
270  }
271 
272  /** KD Tree-based search for the N closest point to some given 2D
273  *coordinates.
274  * This method automatically build the "m_kdtree_data" structure when:
275  * - It is called for the first time
276  * - The map has changed
277  * - The KD-tree was build for 3D.
278  *
279  * \param x0 The X coordinate of the query.
280  * \param y0 The Y coordinate of the query.
281  * \param N The number of closest points to search.
282  * \param out_x The vector containing the X coordinates of the
283  *correspondences.
284  * \param out_y The vector containing the Y coordinates of the
285  *correspondences.
286  * \param out_dist_sqr The vector containing the square distance between
287  *the query and the returned points.
288  *
289  * \return The list of indices
290  * \sa kdTreeClosestPoint2D
291  * \sa kdTreeTwoClosestPoint2D
292  */
293  inline std::vector<size_t> kdTreeNClosestPoint2D(
294  float x0, float y0, size_t knn, std::vector<float>& out_x,
295  std::vector<float>& out_y, std::vector<float>& out_dist_sqr) const
296  {
297  MRPT_START
298  rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
300  THROW_EXCEPTION("There are no points in the KD-tree.");
301 
302  std::vector<size_t> ret_indexes(knn);
303  out_x.resize(knn);
304  out_y.resize(knn);
305  out_dist_sqr.resize(knn);
306 
307  nanoflann::KNNResultSet<num_t> resultSet(knn);
308  resultSet.init(&ret_indexes[0], &out_dist_sqr[0]);
309 
312  m_kdtree2d_data.index->findNeighbors(
313  resultSet, &m_kdtree2d_data.query_point[0],
315 
316  for (size_t i = 0; i < knn; i++)
317  {
318  out_x[i] = derived().kdtree_get_pt(ret_indexes[i], 0);
319  out_y[i] = derived().kdtree_get_pt(ret_indexes[i], 1);
320  }
321  return ret_indexes;
322  MRPT_END
323  }
324 
325  inline std::vector<size_t> kdTreeNClosestPoint2D(
326  const TPoint2D& p0, size_t N, std::vector<TPoint2D>& pOut,
327  std::vector<float>& outDistSqr) const
328  {
329  std::vector<float> dmy1, dmy2;
330  std::vector<size_t> res = kdTreeNClosestPoint2D(
331  static_cast<float>(p0.x), static_cast<float>(p0.y), N, dmy1, dmy2,
332  outDistSqr);
333  pOut.resize(dmy1.size());
334  for (size_t i = 0; i < dmy1.size(); i++)
335  {
336  pOut[i].x = static_cast<double>(dmy1[i]);
337  pOut[i].y = static_cast<double>(dmy2[i]);
338  }
339  return res;
340  }
341 
342  /** KD Tree-based search for the N closest point to some given 2D
343  *coordinates and returns their indexes.
344  * This method automatically build the "m_kdtree_data" structure when:
345  * - It is called for the first time
346  * - The map has changed
347  * - The KD-tree was build for 3D.
348  *
349  * \param x0 The X coordinate of the query.
350  * \param y0 The Y coordinate of the query.
351  * \param N The number of closest points to search.
352  * \param out_idx The indexes of the found closest correspondence.
353  * \param out_dist_sqr The square distance between the query and the
354  *returned point.
355  *
356  * \sa kdTreeClosestPoint2D
357  */
359  float x0, float y0, size_t knn, std::vector<size_t>& out_idx,
360  std::vector<float>& out_dist_sqr) const
361  {
362  MRPT_START
363  rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
365  THROW_EXCEPTION("There are no points in the KD-tree.");
366 
367  out_idx.resize(knn);
368  out_dist_sqr.resize(knn);
369  nanoflann::KNNResultSet<num_t> resultSet(knn);
370  resultSet.init(&out_idx[0], &out_dist_sqr[0]);
371 
374  m_kdtree2d_data.index->findNeighbors(
375  resultSet, &m_kdtree2d_data.query_point[0],
377  MRPT_END
378  }
379 
381  const TPoint2D& p0, size_t N, std::vector<size_t>& outIdx,
382  std::vector<float>& outDistSqr) const
383  {
385  static_cast<float>(p0.x), static_cast<float>(p0.y), N, outIdx,
386  outDistSqr);
387  }
388 
389  /** KD Tree-based search for the closest point (only ONE) to some given 3D
390  *coordinates.
391  * This method automatically build the "m_kdtree_data" structure when:
392  * - It is called for the first time
393  * - The map has changed
394  * - The KD-tree was build for 2D.
395  *
396  * \param x0 The X coordinate of the query.
397  * \param y0 The Y coordinate of the query.
398  * \param z0 The Z coordinate of the query.
399  * \param out_x The X coordinate of the found closest correspondence.
400  * \param out_y The Y coordinate of the found closest correspondence.
401  * \param out_z The Z coordinate of the found closest correspondence.
402  * \param out_dist_sqr The square distance between the query and the
403  *returned point.
404  *
405  * \return The index of the closest point in the map array.
406  * \sa kdTreeClosestPoint2D
407  */
408  inline size_t kdTreeClosestPoint3D(
409  float x0, float y0, float z0, float& out_x, float& out_y, float& out_z,
410  float& out_dist_sqr) const
411  {
412  MRPT_START
413  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
415  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 
425  m_kdtree3d_data.index->findNeighbors(
426  resultSet, &m_kdtree3d_data.query_point[0],
428 
429  // Copy output to user vars:
430  out_x = derived().kdtree_get_pt(ret_index, 0);
431  out_y = derived().kdtree_get_pt(ret_index, 1);
432  out_z = derived().kdtree_get_pt(ret_index, 2);
433 
434  return ret_index;
435  MRPT_END
436  }
437 
438  /// \overload
439  inline size_t kdTreeClosestPoint3D(
440  float x0, float y0, float z0, float& out_dist_sqr) const
441  {
442  MRPT_START
443  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
445  THROW_EXCEPTION("There are no points in the KD-tree.");
446 
447  const size_t knn = 1; // Number of points to retrieve
448  size_t ret_index;
449  nanoflann::KNNResultSet<num_t> resultSet(knn);
450  resultSet.init(&ret_index, &out_dist_sqr);
451 
455  m_kdtree3d_data.index->findNeighbors(
456  resultSet, &m_kdtree3d_data.query_point[0],
458 
459  return ret_index;
460  MRPT_END
461  }
462 
463  /// \overload
464  inline size_t kdTreeClosestPoint3D(
465  const TPoint3D& p0, TPoint3D& pOut, float& outDistSqr) const
466  {
467  float dmy1, dmy2, dmy3;
468  size_t res = kdTreeClosestPoint3D(
469  static_cast<float>(p0.x), static_cast<float>(p0.y),
470  static_cast<float>(p0.z), dmy1, dmy2, dmy3, outDistSqr);
471  pOut.x = static_cast<double>(dmy1);
472  pOut.y = static_cast<double>(dmy2);
473  pOut.z = static_cast<double>(dmy3);
474  return res;
475  }
476 
477  /** KD Tree-based search for the N closest points to some given 3D
478  *coordinates.
479  * This method automatically build the "m_kdtree_data" structure when:
480  * - It is called for the first time
481  * - The map has changed
482  * - The KD-tree was build for 2D.
483  *
484  * \param x0 The X coordinate of the query.
485  * \param y0 The Y coordinate of the query.
486  * \param z0 The Z coordinate of the query.
487  * \param N The number of closest points to search.
488  * \param out_x The vector containing the X coordinates of the
489  *correspondences.
490  * \param out_y The vector containing the Y coordinates of the
491  *correspondences.
492  * \param out_z The vector containing the Z coordinates of the
493  *correspondences.
494  * \param out_dist_sqr The vector containing the square distance between
495  *the query and the returned points.
496  *
497  * \sa kdTreeNClosestPoint2D
498  */
500  float x0, float y0, float z0, size_t knn, std::vector<float>& out_x,
501  std::vector<float>& out_y, std::vector<float>& out_z,
502  std::vector<float>& out_dist_sqr) const
503  {
504  MRPT_START
505  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
507  THROW_EXCEPTION("There are no points in the KD-tree.");
508 
509  std::vector<size_t> ret_indexes(knn);
510  out_x.resize(knn);
511  out_y.resize(knn);
512  out_z.resize(knn);
513  out_dist_sqr.resize(knn);
514 
515  nanoflann::KNNResultSet<num_t> resultSet(knn);
516  resultSet.init(&ret_indexes[0], &out_dist_sqr[0]);
517 
521  m_kdtree3d_data.index->findNeighbors(
522  resultSet, &m_kdtree3d_data.query_point[0],
524 
525  for (size_t i = 0; i < knn; i++)
526  {
527  out_x[i] = derived().kdtree_get_pt(ret_indexes[i], 0);
528  out_y[i] = derived().kdtree_get_pt(ret_indexes[i], 1);
529  out_z[i] = derived().kdtree_get_pt(ret_indexes[i], 2);
530  }
531  MRPT_END
532  }
533 
534  /** KD Tree-based search for the N closest points to some given 3D
535  *coordinates.
536  * This method automatically build the "m_kdtree_data" structure when:
537  * - It is called for the first time
538  * - The map has changed
539  * - The KD-tree was build for 2D.
540  *
541  * \param x0 The X coordinate of the query.
542  * \param y0 The Y coordinate of the query.
543  * \param z0 The Z coordinate of the query.
544  * \param N The number of closest points to search.
545  * \param out_x The vector containing the X coordinates of the
546  *correspondences.
547  * \param out_y The vector containing the Y coordinates of the
548  *correspondences.
549  * \param out_z The vector containing the Z coordinates of the
550  *correspondences.
551  * \param out_idx The vector containing the indexes of the correspondences.
552  * \param out_dist_sqr The vector containing the square distance between
553  *the query and the returned points.
554  *
555  * \sa kdTreeNClosestPoint2D
556  */
558  float x0, float y0, float z0, size_t knn, std::vector<float>& out_x,
559  std::vector<float>& out_y, std::vector<float>& out_z,
560  std::vector<size_t>& out_idx, std::vector<float>& out_dist_sqr) const
561  {
562  MRPT_START
563  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
565  THROW_EXCEPTION("There are no points in the KD-tree.");
566 
567  out_x.resize(knn);
568  out_y.resize(knn);
569  out_z.resize(knn);
570  out_idx.resize(knn);
571  out_dist_sqr.resize(knn);
572 
573  nanoflann::KNNResultSet<num_t> resultSet(knn);
574  resultSet.init(&out_idx[0], &out_dist_sqr[0]);
575 
579  m_kdtree3d_data.index->findNeighbors(
580  resultSet, &m_kdtree3d_data.query_point[0],
582 
583  for (size_t i = 0; i < knn; i++)
584  {
585  out_x[i] = derived().kdtree_get_pt(out_idx[i], 0);
586  out_y[i] = derived().kdtree_get_pt(out_idx[i], 1);
587  out_z[i] = derived().kdtree_get_pt(out_idx[i], 2);
588  }
589  MRPT_END
590  }
591 
593  const TPoint3D& p0, size_t N, std::vector<TPoint3D>& pOut,
594  std::vector<float>& outDistSqr) const
595  {
596  std::vector<float> dmy1, dmy2, dmy3;
598  static_cast<float>(p0.x), static_cast<float>(p0.y),
599  static_cast<float>(p0.z), N, dmy1, dmy2, dmy3, outDistSqr);
600  pOut.resize(dmy1.size());
601  for (size_t i = 0; i < dmy1.size(); i++)
602  {
603  pOut[i].x = static_cast<double>(dmy1[i]);
604  pOut[i].y = static_cast<double>(dmy2[i]);
605  pOut[i].z = static_cast<double>(dmy3[i]);
606  }
607  }
608 
609  /** KD Tree-based search for all the points within a given radius of some 3D
610  *point.
611  * This method automatically build the "m_kdtree_data" structure when:
612  * - It is called for the first time
613  * - The map has changed
614  * - The KD-tree was build for 2D.
615  *
616  * \param x0 The X coordinate of the query.
617  * \param y0 The Y coordinate of the query.
618  * \param z0 The Z coordinate of the query.
619  * \param maxRadiusSqr The square of the desired search radius.
620  * \param out_indices_dist The output list, with pairs of indeces/squared
621  *distances for the found correspondences.
622  * \return Number of found points.
623  *
624  * \sa kdTreeRadiusSearch2D, kdTreeNClosestPoint3DIdx
625  */
626  inline size_t kdTreeRadiusSearch3D(
627  const num_t x0, const num_t y0, const num_t z0,
628  const num_t maxRadiusSqr,
629  std::vector<std::pair<size_t, num_t>>& out_indices_dist) const
630  {
631  MRPT_START
632  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
633  out_indices_dist.clear();
634  if (m_kdtree3d_data.m_num_points != 0)
635  {
636  const num_t xyz[3] = {x0, y0, z0};
637  m_kdtree3d_data.index->radiusSearch(
638  &xyz[0], maxRadiusSqr, out_indices_dist,
640  }
641  return out_indices_dist.size();
642  MRPT_END
643  }
644 
645  /** KD Tree-based search for all the points within a given radius of some 2D
646  *point.
647  * This method automatically build the "m_kdtree_data" structure when:
648  * - It is called for the first time
649  * - The map has changed
650  * - The KD-tree was build for 3D.
651  *
652  * \param x0 The X coordinate of the query.
653  * \param y0 The Y coordinate of the query.
654  * \param maxRadiusSqr The square of the desired search radius.
655  * \param out_indices_dist The output list, with pairs of indeces/squared
656  *distances for the found correspondences.
657  * \return Number of found points.
658  *
659  * \sa kdTreeRadiusSearch3D, kdTreeNClosestPoint2DIdx
660  */
661  inline size_t kdTreeRadiusSearch2D(
662  const num_t x0, const num_t y0, const num_t maxRadiusSqr,
663  std::vector<std::pair<size_t, num_t>>& out_indices_dist) const
664  {
665  MRPT_START
666  rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
667  out_indices_dist.clear();
668  if (m_kdtree2d_data.m_num_points != 0)
669  {
670  const num_t xyz[2] = {x0, y0};
671  m_kdtree2d_data.index->radiusSearch(
672  &xyz[0], maxRadiusSqr, out_indices_dist,
674  }
675  return out_indices_dist.size();
676  MRPT_END
677  }
678 
679  /** KD Tree-based search for the N closest point to some given 3D
680  *coordinates and returns their indexes.
681  * This method automatically build the "m_kdtree_data" structure when:
682  * - It is called for the first time
683  * - The map has changed
684  * - The KD-tree was build for 2D.
685  *
686  * \param x0 The X coordinate of the query.
687  * \param y0 The Y coordinate of the query.
688  * \param z0 The Z coordinate of the query.
689  * \param N The number of closest points to search.
690  * \param out_idx The indexes of the found closest correspondence.
691  * \param out_dist_sqr The square distance between the query and the
692  *returned point.
693  *
694  * \sa kdTreeClosestPoint2D, kdTreeRadiusSearch3D
695  */
697  float x0, float y0, float z0, size_t knn, std::vector<size_t>& out_idx,
698  std::vector<float>& out_dist_sqr) const
699  {
700  MRPT_START
701  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
703  THROW_EXCEPTION("There are no points in the KD-tree.");
704 
705  out_idx.resize(knn);
706  out_dist_sqr.resize(knn);
707  nanoflann::KNNResultSet<num_t> resultSet(knn);
708  resultSet.init(&out_idx[0], &out_dist_sqr[0]);
709 
713  m_kdtree3d_data.index->findNeighbors(
714  resultSet, &m_kdtree3d_data.query_point[0],
716  MRPT_END
717  }
718 
720  const TPoint3D& p0, size_t N, std::vector<size_t>& outIdx,
721  std::vector<float>& outDistSqr) const
722  {
724  static_cast<float>(p0.x), static_cast<float>(p0.y),
725  static_cast<float>(p0.z), N, outIdx, outDistSqr);
726  }
727 
728  /* @} */
729 
730  protected:
731  /** To be called by child classes when KD tree data changes. */
732  inline void kdtree_mark_as_outdated() const
733  {
734  m_kdtree_is_uptodate = false;
735  }
736 
737  private:
738  /** Internal structure with the KD-tree representation (mainly used to avoid
739  * copying pointers with the = operator) */
740  template <int _DIM = -1>
742  {
743  inline TKDTreeDataHolder() {}
744  /** Copy constructor: It actually does NOT copy the kd-tree, a new
745  * object will be created if required! */
747  {
748  }
749  /** Copy operator: It actually does NOT copy the kd-tree, a new object
750  * will be created if required! */
751  inline TKDTreeDataHolder& operator=(const TKDTreeDataHolder& o) noexcept
752  {
753  if (&o != this) clear();
754  return *this;
755  }
756 
757  /** Free memory (if allocated) */
758  inline void clear() noexcept { index.reset(); }
759  using kdtree_index_t =
761 
762  /** nullptr or the up-to-date index */
763  std::unique_ptr<kdtree_index_t> index;
764 
765  std::vector<num_t> query_point;
766  /** Dimensionality. typ: 2,3 */
767  size_t m_dim = _DIM;
768  size_t m_num_points = 0;
769  };
770 
771  mutable TKDTreeDataHolder<2> m_kdtree2d_data;
772  mutable TKDTreeDataHolder<3> m_kdtree3d_data;
773  mutable TKDTreeDataHolder<> m_kdtreeNd_data;
774  /** whether the KD tree needs to be rebuilt or not. */
775  mutable bool m_kdtree_is_uptodate;
776 
777  /// Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ...
778  /// asking the child class for the data points.
779  void rebuild_kdTree_2D() const
780  {
781  using tree2d_t = typename TKDTreeDataHolder<2>::kdtree_index_t;
782 
784  {
788  }
789 
790  if (!m_kdtree2d_data.index)
791  {
792  // Erase previous tree:
794  // And build new index:
795  const size_t N = derived().kdtree_get_point_count();
798  m_kdtree2d_data.query_point.resize(2);
799  if (N)
800  {
801  m_kdtree2d_data.index.reset(new tree2d_t(
802  2, derived(),
805  m_kdtree2d_data.index->buildIndex();
806  }
807  m_kdtree_is_uptodate = true;
808  }
809  }
810 
811  /// Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ...
812  /// asking the child class for the data points.
813  void rebuild_kdTree_3D() const
814  {
815  using tree3d_t = typename TKDTreeDataHolder<3>::kdtree_index_t;
816 
818  {
822  }
823 
824  if (!m_kdtree3d_data.index)
825  {
826  // Erase previous tree:
828  // And build new index:
829  const size_t N = derived().kdtree_get_point_count();
832  m_kdtree3d_data.query_point.resize(3);
833  if (N)
834  {
835  m_kdtree3d_data.index.reset(new tree3d_t(
836  3, derived(),
839  m_kdtree3d_data.index->buildIndex();
840  }
841  m_kdtree_is_uptodate = true;
842  }
843  }
844 
845 }; // end of KDTreeCapable
846 
847 /** @} */ // end of grouping
848 
849 } // namespace math
850 } // namespace mrpt
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.
#define MRPT_START
Definition: exceptions.h:262
void kdTreeNClosestPoint3DIdx(const TPoint3D &p0, size_t N, std::vector< size_t > &outIdx, std::vector< float > &outDistSqr) const
KDTreeCapable()
Constructor.
Definition: KDTreeCapable.h:87
double x
X,Y coordinates.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
Squared Euclidean (L2) distance functor (suitable for low-dimensionality datasets, like 2D or 3D point clouds) Corresponding distance traits: nanoflann::metric_L2_Simple.
Definition: nanoflann.hpp:355
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_)
Definition: nanoflann.hpp:90
TKDTreeDataHolder< 2 > m_kdtree2d_data
const Derived & derived() const
CRTP helper method.
Definition: KDTreeCapable.h:89
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.
Definition: KDTreeCapable.h:94
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...
Definition: KDTreeCapable.h:79
TKDTreeDataHolder< 3 > m_kdtree3d_data
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.
GLuint index
Definition: glext.h:4054
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.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool m_kdtree_is_uptodate
whether the KD tree needs to be rebuilt or not.
size_t leaf_max_size
Max points per leaf.
Definition: KDTreeCapable.h:99
Parameters (see README.md)
Definition: nanoflann.hpp:403
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.
#define MRPT_END
Definition: exceptions.h:266
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()
Definition: nanoflann.hpp:413
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.
GLuint res
Definition: glext.h:7268
TKDTreeDataHolder m_kdtreeNd_data
TKDTreeSearchParams kdtree_search_params
Parameters to tune the ANN searches.
Lightweight 3D point.
Lightweight 2D point.
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.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019