Main MRPT website > C++ reference for MRPT 1.5.7
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>
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> >
68  {
69  public:
70  // Types ---------------
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 
86  {
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 
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 
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  */
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  */
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 
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 
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  */
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 
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 
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 
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  */
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 
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  */
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 
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  */
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 
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>
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! */
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 
717 
718  if (!m_kdtree2d_data.index)
719  {
720  // Erase previous tree:
722  // And build new index:
723  const size_t N = derived().kdtree_get_point_count();
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 
742 
743  if (!m_kdtree3d_data.index)
744  {
745  // Erase previous tree:
747  // And build new index:
748  const size_t N = derived().kdtree_get_point_count();
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
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...
~KDTreeCapable()
Destructor (nothing needed to do here)
Definition: KDTreeCapable.h:78
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.
double y
X,Y coordinates.
void kdTreeNClosestPoint3DIdx(const TPoint3D &p0, size_t N, std::vector< size_t > &outIdx, std::vector< float > &outDistSqr) const
KDTreeCapable()
Constructor.
Definition: KDTreeCapable.h:75
void kdTreeNClosestPoint2DIdx(const TPoint2D &p0, size_t N, std::vector< size_t > &outIdx, std::vector< float > &outDistSqr) const
void kdTreeTwoClosestPoint2D(const TPoint2D &p0, TPoint2D &pOut1, TPoint2D &pOut2, float &outDistSqr1, float &outDistSqr2) const
#define THROW_EXCEPTION(msg)
size_t kdTreeClosestPoint3D(const TPoint3D &p0, TPoint3D &pOut, float &outDistSqr) const
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.
TKDTreeDataHolder< 2 > m_kdtree2d_data
size_t m_dim
Dimensionality. typ: 2,3.
const Derived & derived() const
CRTP helper method.
Definition: KDTreeCapable.h:81
double z
X,Y,Z coordinates.
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), ... asking the child class for the dat...
Derived & derived()
CRTP helper method.
Definition: KDTreeCapable.h:83
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:67
TKDTreeDataHolder< 3 > m_kdtree3d_data
TKDTreeDataHolder & operator=(const TKDTreeDataHolder &o)
Copy operator: It actually does NOT copy the kd-tree, a new object will be created if required! ...
#define MRPT_END
GLuint index
Definition: glext.h:3891
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...
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.
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.
TKDTreeDataHolder()
Init the pointer to NULL.
std::vector< size_t > kdTreeNClosestPoint2D(const TPoint2D &p0, size_t N, std::vector< TPoint2D > &pOut, std::vector< float > &outDistSqr) const
~TKDTreeDataHolder()
Free memory (if allocated)
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
nanoflann::KDTreeSingleIndexAdaptor< metric_t, Derived, _DIM > kdtree_index_t
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void delete_safe(T *&ptr)
Calls "delete" to free an object only if the pointer is not NULL, then set the pointer to NULL...
bool m_kdtree_is_uptodate
whether the KD tree needs to be rebuilt or not.
size_t leaf_max_size
Max points per leaf.
Definition: KDTreeCapable.h:91
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.
void clear()
Free memory (if allocated)
kdtree_index_t * index
NULL or the up-to-date index.
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.
KDTreeCapable< Derived, num_t, metric_t > self_t
Definition: KDTreeCapable.h:71
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:6298
TKDTreeDataHolder m_kdtreeNd_data
TKDTreeSearchParams kdtree_search_params
Parameters to tune the ANN searches.
Definition: KDTreeCapable.h:94
Lightweight 3D point.
Lightweight 2D point.
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.
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.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019