MRPT  1.9.9
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-2019, 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 = kdTreeClosestPoint2D(
190  static_cast<float>(p0.x), static_cast<float>(p0.y), dmy1, dmy2,
191  outDistSqr);
192  pOut.x = dmy1;
193  pOut.y = dmy2;
194  return res;
195  }
196 
197  /** Like kdTreeClosestPoint2D, but just return the square error from some
198  * point to its closest neighbor.
199  */
200  inline float kdTreeClosestPoint2DsqrError(float x0, float y0) const
201  {
202  float closerx, closery, closer_dist;
203  kdTreeClosestPoint2D(x0, y0, closerx, closery, closer_dist);
204  return closer_dist;
205  }
206 
207  inline float kdTreeClosestPoint2DsqrError(const TPoint2D& p0) const
208  {
210  static_cast<float>(p0.x), static_cast<float>(p0.y));
211  }
212 
213  /** KD Tree-based search for the TWO closest point to some given 2D
214  *coordinates.
215  * This method automatically build the "m_kdtree_data" structure when:
216  * - It is called for the first time
217  * - The map has changed
218  * - The KD-tree was build for 3D.
219  *
220  * \param x0 The X coordinate of the query.
221  * \param y0 The Y coordinate of the query.
222  * \param out_x1 The X coordinate of the first correspondence.
223  * \param out_y1 The Y coordinate of the first correspondence.
224  * \param out_x2 The X coordinate of the second correspondence.
225  * \param out_y2 The Y coordinate of the second correspondence.
226  * \param out_dist_sqr1 The square distance between the query and the first
227  *returned point.
228  * \param out_dist_sqr2 The square distance between the query and the
229  *second returned point.
230  *
231  * \sa kdTreeClosestPoint2D
232  */
234  float x0, float y0, float& out_x1, float& out_y1, float& out_x2,
235  float& out_y2, float& out_dist_sqr1, float& out_dist_sqr2) const
236  {
237  MRPT_START
238  rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
240  THROW_EXCEPTION("There are no points in the KD-tree.");
241 
242  const size_t knn = 2; // Number of points to retrieve
243  size_t ret_indexes[2];
244  float ret_sqdist[2];
245  nanoflann::KNNResultSet<num_t> resultSet(knn);
246  resultSet.init(&ret_indexes[0], &ret_sqdist[0]);
247 
248  const std::array<num_t, 2> query_point{{x0, y0}};
249  m_kdtree2d_data.index->findNeighbors(
250  resultSet, &query_point[0], nanoflann::SearchParams());
251 
252  // Copy output to user vars:
253  out_x1 = derived().kdtree_get_pt(ret_indexes[0], 0);
254  out_y1 = derived().kdtree_get_pt(ret_indexes[0], 1);
255  out_dist_sqr1 = ret_sqdist[0];
256 
257  out_x2 = derived().kdtree_get_pt(ret_indexes[1], 0);
258  out_y2 = derived().kdtree_get_pt(ret_indexes[1], 1);
259  out_dist_sqr2 = ret_sqdist[0];
260 
261  MRPT_END
262  }
263 
265  const TPoint2D& p0, TPoint2D& pOut1, TPoint2D& pOut2,
266  float& outDistSqr1, float& outDistSqr2) const
267  {
268  float dmy1, dmy2, dmy3, dmy4;
270  p0.x, p0.y, dmy1, dmy2, dmy3, dmy4, outDistSqr1, outDistSqr2);
271  pOut1.x = static_cast<double>(dmy1);
272  pOut1.y = static_cast<double>(dmy2);
273  pOut2.x = static_cast<double>(dmy3);
274  pOut2.y = static_cast<double>(dmy4);
275  }
276 
277  /** KD Tree-based search for the N closest point to some given 2D
278  *coordinates.
279  * This method automatically build the "m_kdtree_data" structure when:
280  * - It is called for the first time
281  * - The map has changed
282  * - The KD-tree was build for 3D.
283  *
284  * \param x0 The X coordinate of the query.
285  * \param y0 The Y coordinate of the query.
286  * \param N The number of closest points to search.
287  * \param out_x The vector containing the X coordinates of the
288  *correspondences.
289  * \param out_y The vector containing the Y coordinates of the
290  *correspondences.
291  * \param out_dist_sqr The vector containing the square distance between
292  *the query and the returned points.
293  *
294  * \return The list of indices
295  * \sa kdTreeClosestPoint2D
296  * \sa kdTreeTwoClosestPoint2D
297  */
298  inline std::vector<size_t> kdTreeNClosestPoint2D(
299  float x0, float y0, size_t knn, std::vector<float>& out_x,
300  std::vector<float>& out_y, std::vector<float>& out_dist_sqr) const
301  {
302  MRPT_START
303  rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
305  THROW_EXCEPTION("There are no points in the KD-tree.");
306 
307  std::vector<size_t> ret_indexes(knn);
308  out_x.resize(knn);
309  out_y.resize(knn);
310  out_dist_sqr.resize(knn);
311 
312  nanoflann::KNNResultSet<num_t> resultSet(knn);
313  resultSet.init(&ret_indexes[0], &out_dist_sqr[0]);
314 
315  const std::array<num_t, 2> query_point{{x0, y0}};
316  m_kdtree2d_data.index->findNeighbors(
317  resultSet, &query_point[0], nanoflann::SearchParams());
318 
319  for (size_t i = 0; i < knn; i++)
320  {
321  out_x[i] = derived().kdtree_get_pt(ret_indexes[i], 0);
322  out_y[i] = derived().kdtree_get_pt(ret_indexes[i], 1);
323  }
324  return ret_indexes;
325  MRPT_END
326  }
327 
328  inline std::vector<size_t> kdTreeNClosestPoint2D(
329  const TPoint2D& p0, size_t N, std::vector<TPoint2D>& pOut,
330  std::vector<float>& outDistSqr) const
331  {
332  std::vector<float> dmy1, dmy2;
333  std::vector<size_t> res = kdTreeNClosestPoint2D(
334  static_cast<float>(p0.x), static_cast<float>(p0.y), N, dmy1, dmy2,
335  outDistSqr);
336  pOut.resize(dmy1.size());
337  for (size_t i = 0; i < dmy1.size(); i++)
338  {
339  pOut[i].x = static_cast<double>(dmy1[i]);
340  pOut[i].y = static_cast<double>(dmy2[i]);
341  }
342  return res;
343  }
344 
345  /** KD Tree-based search for the N closest point to some given 2D
346  *coordinates and returns their indexes.
347  * This method automatically build the "m_kdtree_data" structure when:
348  * - It is called for the first time
349  * - The map has changed
350  * - The KD-tree was build for 3D.
351  *
352  * \param x0 The X coordinate of the query.
353  * \param y0 The Y coordinate of the query.
354  * \param N The number of closest points to search.
355  * \param out_idx The indexes of the found closest correspondence.
356  * \param out_dist_sqr The square distance between the query and the
357  *returned point.
358  *
359  * \sa kdTreeClosestPoint2D
360  */
362  float x0, float y0, size_t knn, std::vector<size_t>& out_idx,
363  std::vector<float>& out_dist_sqr) const
364  {
365  MRPT_START
366  rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
368  THROW_EXCEPTION("There are no points in the KD-tree.");
369 
370  out_idx.resize(knn);
371  out_dist_sqr.resize(knn);
372  nanoflann::KNNResultSet<num_t> resultSet(knn);
373  resultSet.init(&out_idx[0], &out_dist_sqr[0]);
374 
375  const std::array<num_t, 2> query_point{{x0, y0}};
376  m_kdtree2d_data.index->findNeighbors(
377  resultSet, &query_point[0], nanoflann::SearchParams());
378  MRPT_END
379  }
380 
382  const TPoint2D& p0, size_t N, std::vector<size_t>& outIdx,
383  std::vector<float>& outDistSqr) const
384  {
386  static_cast<float>(p0.x), static_cast<float>(p0.y), N, outIdx,
387  outDistSqr);
388  }
389 
390  /** KD Tree-based search for the closest point (only ONE) to some given 3D
391  *coordinates.
392  * This method automatically build the "m_kdtree_data" structure when:
393  * - It is called for the first time
394  * - The map has changed
395  * - The KD-tree was build for 2D.
396  *
397  * \param x0 The X coordinate of the query.
398  * \param y0 The Y coordinate of the query.
399  * \param z0 The Z coordinate of the query.
400  * \param out_x The X coordinate of the found closest correspondence.
401  * \param out_y The Y coordinate of the found closest correspondence.
402  * \param out_z The Z coordinate of the found closest correspondence.
403  * \param out_dist_sqr The square distance between the query and the
404  *returned point.
405  *
406  * \return The index of the closest point in the map array.
407  * \sa kdTreeClosestPoint2D
408  */
409  inline size_t kdTreeClosestPoint3D(
410  float x0, float y0, float z0, float& out_x, float& out_y, float& out_z,
411  float& out_dist_sqr) const
412  {
413  MRPT_START
414  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
416  THROW_EXCEPTION("There are no points in the KD-tree.");
417 
418  const size_t knn = 1; // Number of points to retrieve
419  size_t ret_index;
420  nanoflann::KNNResultSet<num_t> resultSet(knn);
421  resultSet.init(&ret_index, &out_dist_sqr);
422 
423  const std::array<num_t, 3> query_point{{x0, y0, z0}};
424  m_kdtree3d_data.index->findNeighbors(
425  resultSet, &query_point[0], nanoflann::SearchParams());
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 
450  const std::array<num_t, 3> query_point{{x0, y0, z0}};
451  m_kdtree3d_data.index->findNeighbors(
452  resultSet, &query_point[0], nanoflann::SearchParams());
453 
454  return ret_index;
455  MRPT_END
456  }
457 
458  /// \overload
459  inline size_t kdTreeClosestPoint3D(
460  const TPoint3D& p0, TPoint3D& pOut, float& outDistSqr) const
461  {
462  float dmy1, dmy2, dmy3;
463  size_t res = kdTreeClosestPoint3D(
464  static_cast<float>(p0.x), static_cast<float>(p0.y),
465  static_cast<float>(p0.z), dmy1, dmy2, dmy3, outDistSqr);
466  pOut.x = static_cast<double>(dmy1);
467  pOut.y = static_cast<double>(dmy2);
468  pOut.z = static_cast<double>(dmy3);
469  return res;
470  }
471 
472  /** KD Tree-based search for the N closest points to some given 3D
473  *coordinates.
474  * This method automatically build the "m_kdtree_data" structure when:
475  * - It is called for the first time
476  * - The map has changed
477  * - The KD-tree was build for 2D.
478  *
479  * \param x0 The X coordinate of the query.
480  * \param y0 The Y coordinate of the query.
481  * \param z0 The Z coordinate of the query.
482  * \param N The number of closest points to search.
483  * \param out_x The vector containing the X coordinates of the
484  *correspondences.
485  * \param out_y The vector containing the Y coordinates of the
486  *correspondences.
487  * \param out_z The vector containing the Z coordinates of the
488  *correspondences.
489  * \param out_dist_sqr The vector containing the square distance between
490  *the query and the returned points.
491  *
492  * \sa kdTreeNClosestPoint2D
493  */
495  float x0, float y0, float z0, size_t knn, std::vector<float>& out_x,
496  std::vector<float>& out_y, std::vector<float>& out_z,
497  std::vector<float>& out_dist_sqr) const
498  {
499  MRPT_START
500  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
502  THROW_EXCEPTION("There are no points in the KD-tree.");
503 
504  std::vector<size_t> ret_indexes(knn);
505  out_x.resize(knn);
506  out_y.resize(knn);
507  out_z.resize(knn);
508  out_dist_sqr.resize(knn);
509 
510  nanoflann::KNNResultSet<num_t> resultSet(knn);
511  resultSet.init(&ret_indexes[0], &out_dist_sqr[0]);
512 
513  const std::array<num_t, 3> query_point{{x0, y0, z0}};
514  m_kdtree3d_data.index->findNeighbors(
515  resultSet, &query_point[0], nanoflann::SearchParams());
516 
517  for (size_t i = 0; i < knn; i++)
518  {
519  out_x[i] = derived().kdtree_get_pt(ret_indexes[i], 0);
520  out_y[i] = derived().kdtree_get_pt(ret_indexes[i], 1);
521  out_z[i] = derived().kdtree_get_pt(ret_indexes[i], 2);
522  }
523  MRPT_END
524  }
525 
526  /** KD Tree-based search for the N closest points to some given 3D
527  *coordinates.
528  * This method automatically build the "m_kdtree_data" structure when:
529  * - It is called for the first time
530  * - The map has changed
531  * - The KD-tree was build for 2D.
532  *
533  * \param x0 The X coordinate of the query.
534  * \param y0 The Y coordinate of the query.
535  * \param z0 The Z coordinate of the query.
536  * \param N The number of closest points to search.
537  * \param out_x The vector containing the X coordinates of the
538  *correspondences.
539  * \param out_y The vector containing the Y coordinates of the
540  *correspondences.
541  * \param out_z The vector containing the Z coordinates of the
542  *correspondences.
543  * \param out_idx The vector containing the indexes of the correspondences.
544  * \param out_dist_sqr The vector containing the square distance between
545  *the query and the returned points.
546  *
547  * \sa kdTreeNClosestPoint2D
548  */
550  float x0, float y0, float z0, size_t knn, std::vector<float>& out_x,
551  std::vector<float>& out_y, std::vector<float>& out_z,
552  std::vector<size_t>& out_idx, std::vector<float>& out_dist_sqr) const
553  {
554  MRPT_START
555  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
557  THROW_EXCEPTION("There are no points in the KD-tree.");
558 
559  out_x.resize(knn);
560  out_y.resize(knn);
561  out_z.resize(knn);
562  out_idx.resize(knn);
563  out_dist_sqr.resize(knn);
564 
565  nanoflann::KNNResultSet<num_t> resultSet(knn);
566  resultSet.init(&out_idx[0], &out_dist_sqr[0]);
567 
568  const std::array<num_t, 3> query_point{{x0, y0, z0}};
569  m_kdtree3d_data.index->findNeighbors(
570  resultSet, &query_point[0], nanoflann::SearchParams());
571 
572  for (size_t i = 0; i < knn; i++)
573  {
574  out_x[i] = derived().kdtree_get_pt(out_idx[i], 0);
575  out_y[i] = derived().kdtree_get_pt(out_idx[i], 1);
576  out_z[i] = derived().kdtree_get_pt(out_idx[i], 2);
577  }
578  MRPT_END
579  }
580 
582  const TPoint3D& p0, size_t N, std::vector<TPoint3D>& pOut,
583  std::vector<float>& outDistSqr) const
584  {
585  std::vector<float> dmy1, dmy2, dmy3;
587  static_cast<float>(p0.x), static_cast<float>(p0.y),
588  static_cast<float>(p0.z), N, dmy1, dmy2, dmy3, outDistSqr);
589  pOut.resize(dmy1.size());
590  for (size_t i = 0; i < dmy1.size(); i++)
591  {
592  pOut[i].x = static_cast<double>(dmy1[i]);
593  pOut[i].y = static_cast<double>(dmy2[i]);
594  pOut[i].z = static_cast<double>(dmy3[i]);
595  }
596  }
597 
598  /** KD Tree-based search for all the points within a given radius of some 3D
599  *point.
600  * This method automatically build the "m_kdtree_data" structure when:
601  * - It is called for the first time
602  * - The map has changed
603  * - The KD-tree was build for 2D.
604  *
605  * \param x0 The X coordinate of the query.
606  * \param y0 The Y coordinate of the query.
607  * \param z0 The Z coordinate of the query.
608  * \param maxRadiusSqr The square of the desired search radius.
609  * \param out_indices_dist The output list, with pairs of indeces/squared
610  *distances for the found correspondences.
611  * \return Number of found points.
612  *
613  * \sa kdTreeRadiusSearch2D, kdTreeNClosestPoint3DIdx
614  */
615  inline size_t kdTreeRadiusSearch3D(
616  const num_t x0, const num_t y0, const num_t z0,
617  const num_t maxRadiusSqr,
618  std::vector<std::pair<size_t, num_t>>& out_indices_dist) const
619  {
620  MRPT_START
621  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
622  out_indices_dist.clear();
623  if (m_kdtree3d_data.m_num_points != 0)
624  {
625  const num_t xyz[3] = {x0, y0, z0};
626  m_kdtree3d_data.index->radiusSearch(
627  &xyz[0], maxRadiusSqr, out_indices_dist,
629  }
630  return out_indices_dist.size();
631  MRPT_END
632  }
633 
634  /** KD Tree-based search for all the points within a given radius of some 2D
635  *point.
636  * This method automatically build the "m_kdtree_data" structure when:
637  * - It is called for the first time
638  * - The map has changed
639  * - The KD-tree was build for 3D.
640  *
641  * \param x0 The X coordinate of the query.
642  * \param y0 The Y coordinate of the query.
643  * \param maxRadiusSqr The square of the desired search radius.
644  * \param out_indices_dist The output list, with pairs of indeces/squared
645  *distances for the found correspondences.
646  * \return Number of found points.
647  *
648  * \sa kdTreeRadiusSearch3D, kdTreeNClosestPoint2DIdx
649  */
650  inline size_t kdTreeRadiusSearch2D(
651  const num_t x0, const num_t y0, const num_t maxRadiusSqr,
652  std::vector<std::pair<size_t, num_t>>& out_indices_dist) const
653  {
654  MRPT_START
655  rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
656  out_indices_dist.clear();
657  if (m_kdtree2d_data.m_num_points != 0)
658  {
659  const num_t xyz[2] = {x0, y0};
660  m_kdtree2d_data.index->radiusSearch(
661  &xyz[0], maxRadiusSqr, out_indices_dist,
663  }
664  return out_indices_dist.size();
665  MRPT_END
666  }
667 
668  /** KD Tree-based search for the N closest point to some given 3D
669  *coordinates and returns their indexes.
670  * This method automatically build the "m_kdtree_data" structure when:
671  * - It is called for the first time
672  * - The map has changed
673  * - The KD-tree was build for 2D.
674  *
675  * \param x0 The X coordinate of the query.
676  * \param y0 The Y coordinate of the query.
677  * \param z0 The Z coordinate of the query.
678  * \param N The number of closest points to search.
679  * \param out_idx The indexes of the found closest correspondence.
680  * \param out_dist_sqr The square distance between the query and the
681  *returned point.
682  *
683  * \sa kdTreeClosestPoint2D, kdTreeRadiusSearch3D
684  */
686  float x0, float y0, float z0, size_t knn, std::vector<size_t>& out_idx,
687  std::vector<float>& out_dist_sqr) const
688  {
689  MRPT_START
690  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
692  THROW_EXCEPTION("There are no points in the KD-tree.");
693 
694  out_idx.resize(knn);
695  out_dist_sqr.resize(knn);
696  nanoflann::KNNResultSet<num_t> resultSet(knn);
697  resultSet.init(&out_idx[0], &out_dist_sqr[0]);
698 
699  const std::array<num_t, 3> query_point{{x0, y0, z0}};
700  m_kdtree3d_data.index->findNeighbors(
701  resultSet, &query_point[0], nanoflann::SearchParams());
702  MRPT_END
703  }
704 
706  const TPoint3D& p0, size_t N, std::vector<size_t>& outIdx,
707  std::vector<float>& outDistSqr) const
708  {
710  static_cast<float>(p0.x), static_cast<float>(p0.y),
711  static_cast<float>(p0.z), N, outIdx, outDistSqr);
712  }
713 
716 
717  /* @} */
718 
719  protected:
720  /** To be called by child classes when KD tree data changes. */
721  inline void kdtree_mark_as_outdated() const
722  {
723  std::lock_guard<std::mutex> lck(m_kdtree_mtx);
724  m_kdtree_is_uptodate = false;
725  }
726 
727  private:
728  /** Internal structure with the KD-tree representation (mainly used to avoid
729  * copying pointers with the = operator) */
730  template <int _DIM = -1>
732  {
733  inline TKDTreeDataHolder() = default;
734  /** Copy constructor: It actually does NOT copy the kd-tree, a new
735  * object will be created if required! */
737  {
738  }
739  /** Copy operator: It actually does NOT copy the kd-tree, a new object
740  * will be created if required! */
741  inline TKDTreeDataHolder& operator=(const TKDTreeDataHolder& o) noexcept
742  {
743  if (&o != this) clear();
744  return *this;
745  }
746 
747  /** Free memory (if allocated) */
748  inline void clear() noexcept { index.reset(); }
749  using kdtree_index_t =
751 
752  /** nullptr or the up-to-date index */
753  std::unique_ptr<kdtree_index_t> index;
754 
755  /** Dimensionality. typ: 2,3 */
756  size_t m_dim = _DIM;
757  size_t m_num_points = 0;
758  };
759 
760  mutable std::mutex m_kdtree_mtx;
761  mutable TKDTreeDataHolder<2> m_kdtree2d_data;
762  mutable TKDTreeDataHolder<3> m_kdtree3d_data;
763  /** whether the KD tree needs to be rebuilt or not. */
764  mutable std::atomic_bool m_kdtree_is_uptodate{false};
765 
766  /// Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ...
767  /// asking the child class for the data points.
768  void rebuild_kdTree_2D() const
769  {
770  if (m_kdtree_is_uptodate) return;
771 
772  std::lock_guard<std::mutex> lck(m_kdtree_mtx);
773  using tree2d_t = typename TKDTreeDataHolder<2>::kdtree_index_t;
774 
776  {
779  }
780 
781  if (!m_kdtree2d_data.index)
782  {
783  // Erase previous tree:
785  // And build new index:
786  const size_t N = derived().kdtree_get_point_count();
789  if (N)
790  {
791  m_kdtree2d_data.index.reset(new tree2d_t(
792  2, derived(),
795  m_kdtree2d_data.index->buildIndex();
796  }
797  m_kdtree_is_uptodate = true;
798  }
799  }
800 
801  /// Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ...
802  /// asking the child class for the data points.
803  void rebuild_kdTree_3D() const
804  {
805  if (m_kdtree_is_uptodate) return;
806  std::lock_guard<std::mutex> lck(m_kdtree_mtx);
807  using tree3d_t = typename TKDTreeDataHolder<3>::kdtree_index_t;
808 
810  {
813  }
814 
815  if (!m_kdtree3d_data.index)
816  {
817  // Erase previous tree:
819  // And build new index:
820  const size_t N = derived().kdtree_get_point_count();
823  if (N)
824  {
825  m_kdtree3d_data.index.reset(new tree3d_t(
826  3, derived(),
829  m_kdtree3d_data.index->buildIndex();
830  }
831  m_kdtree_is_uptodate = true;
832  }
833  }
834 
835 }; // end of KDTreeCapable
836 
837 /** @} */ // end of grouping
838 
839 } // 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...
double x
X,Y coordinates.
Definition: TPoint2D.h:23
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
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
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
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
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:4068
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.
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.
GLuint res
Definition: glext.h:7385
TKDTreeSearchParams kdtree_search_params
Parameters to tune the ANN searches.
Lightweight 3D point.
Definition: TPoint3D.h:90
Lightweight 2D point.
Definition: TPoint2D.h:31
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: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019