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


Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at mié abr 15 19:30:12 CEST 2020