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