MRPT  2.0.4
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
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:241
void kdTreeNClosestPoint3DIdx(const TPoint3D &p0, size_t N, std::vector< size_t > &outIdx, std::vector< float > &outDistSqr) const
KDTreeCapable & operator=(const KDTreeCapable &)
Definition: KDTreeCapable.h:93
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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
T x
X,Y coordinates.
Definition: TPoint2D.h:25
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.
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.
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:83
TKDTreeDataHolder< 3 > m_kdtree3d_data
float d2f(const double d)
shortcut for static_cast<float>(double)
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.
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
KDTreeCapable(const KDTreeCapable &)
Definition: KDTreeCapable.h:92
KDTreeCapable()=default
Constructor.
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
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.
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:245
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
std::atomic_bool m_kdtree_is_uptodate
whether the KD tree needs to be rebuilt or not.
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.
TKDTreeSearchParams kdtree_search_params
Parameters to tune the ANN searches.
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 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020