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



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020