Main MRPT website > C++ reference
MRPT logo
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-2015, 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 Approximate Nearest Neighbors (ANN) (via the nanoflann library) to MRPT classses.
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  nChecks(32),
89  leaf_max_size(10)
90  {
91  }
92 
93  int nChecks; //!< The number of checks for ANN (default: 32) - corresponds to FLANN's SearchParams::check
94  size_t leaf_max_size; //!< Max points per leaf
95  };
96 
97  TKDTreeSearchParams kdtree_search_params; //!< Parameters to tune the ANN searches
98 
99  /** @name Public utility methods to query the KD-tree
100  @{ */
101 
102  /** KD Tree-based search for the closest point (only ONE) to some given 2D coordinates.
103  * This method automatically build the "m_kdtree_data" structure when:
104  * - It is called for the first time
105  * - The map has changed
106  * - The KD-tree was build for 3D.
107  *
108  * \param x0 The X coordinate of the query.
109  * \param y0 The Y coordinate of the query.
110  * \param out_x The X coordinate of the found closest correspondence.
111  * \param out_y The Y coordinate of the found closest correspondence.
112  * \param out_dist_sqr The square distance between the query and the returned point.
113  *
114  * \return The index of the closest point in the map array.
115  * \sa kdTreeClosestPoint3D, kdTreeTwoClosestPoint2D
116  */
117  inline size_t kdTreeClosestPoint2D(
118  float x0,
119  float y0,
120  float &out_x,
121  float &out_y,
122  float &out_dist_sqr
123  ) const
124  {
125  MRPT_START
126  rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
127  if ( !m_kdtree2d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
128 
129  const size_t knn = 1; // Number of points to retrieve
130  size_t ret_index;
131  nanoflann::KNNResultSet<num_t> resultSet(knn);
132  resultSet.init(&ret_index, &out_dist_sqr );
133 
134  m_kdtree2d_data.query_point[0] = x0;
135  m_kdtree2d_data.query_point[1] = y0;
136  m_kdtree2d_data.index->findNeighbors(resultSet, &m_kdtree2d_data.query_point[0], nanoflann::SearchParams(kdtree_search_params.nChecks));
137 
138  // Copy output to user vars:
139  out_x = derived().kdtree_get_pt(ret_index,0);
140  out_y = derived().kdtree_get_pt(ret_index,1);
141 
142  return ret_index;
143  MRPT_END
144  }
145 
146  /// \overload
147  inline size_t kdTreeClosestPoint2D(
148  float x0,
149  float y0,
150  float &out_dist_sqr ) const
151  {
152  MRPT_START
153  rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
154  if ( !m_kdtree2d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
155 
156  const size_t knn = 1; // Number of points to retrieve
157  size_t ret_index;
158  nanoflann::KNNResultSet<num_t> resultSet(knn);
159  resultSet.init(&ret_index, &out_dist_sqr );
160 
161  m_kdtree2d_data.query_point[0] = x0;
162  m_kdtree2d_data.query_point[1] = y0;
163  m_kdtree2d_data.index->findNeighbors(resultSet, &m_kdtree2d_data.query_point[0], nanoflann::SearchParams(kdtree_search_params.nChecks));
164 
165  return ret_index;
166  MRPT_END
167  }
168 
169  /// \overload
170  inline size_t kdTreeClosestPoint2D(const TPoint2D &p0,TPoint2D &pOut,float &outDistSqr) const {
171  float dmy1,dmy2;
172  size_t res=kdTreeClosestPoint2D(static_cast<float>(p0.x),static_cast<float>(p0.y),dmy1,dmy2,outDistSqr);
173  pOut.x=dmy1;
174  pOut.y=dmy2;
175  return res;
176  }
177 
178  /** Like kdTreeClosestPoint2D, but just return the square error from some point to its closest neighbor.
179  */
180  inline float kdTreeClosestPoint2DsqrError(
181  float x0,
182  float y0 ) const
183  {
184  float closerx,closery,closer_dist;
185  kdTreeClosestPoint2D(x0,y0, closerx,closery,closer_dist);
186  return closer_dist;
187  }
188 
189  inline float kdTreeClosestPoint2DsqrError(const TPoint2D &p0) const {
190  return kdTreeClosestPoint2DsqrError(static_cast<float>(p0.x),static_cast<float>(p0.y));
191  }
192 
193  /** KD Tree-based search for the TWO closest point to some given 2D coordinates.
194  * This method automatically build the "m_kdtree_data" structure when:
195  * - It is called for the first time
196  * - The map has changed
197  * - The KD-tree was build for 3D.
198  *
199  * \param x0 The X coordinate of the query.
200  * \param y0 The Y coordinate of the query.
201  * \param out_x1 The X coordinate of the first correspondence.
202  * \param out_y1 The Y coordinate of the first correspondence.
203  * \param out_x2 The X coordinate of the second correspondence.
204  * \param out_y2 The Y coordinate of the second correspondence.
205  * \param out_dist_sqr1 The square distance between the query and the first returned point.
206  * \param out_dist_sqr2 The square distance between the query and the second returned point.
207  *
208  * \sa kdTreeClosestPoint2D
209  */
210  inline void kdTreeTwoClosestPoint2D(
211  float x0,
212  float y0,
213  float &out_x1,
214  float &out_y1,
215  float &out_x2,
216  float &out_y2,
217  float &out_dist_sqr1,
218  float &out_dist_sqr2 ) const
219  {
220  MRPT_START
221  rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
222  if ( !m_kdtree2d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
223 
224  const size_t knn = 2; // Number of points to retrieve
225  size_t ret_indexes[2];
226  float ret_sqdist[2];
227  nanoflann::KNNResultSet<num_t> resultSet(knn);
228  resultSet.init(&ret_indexes[0], &ret_sqdist[0] );
229 
230  m_kdtree2d_data.query_point[0] = x0;
231  m_kdtree2d_data.query_point[1] = y0;
232  m_kdtree2d_data.index->findNeighbors(resultSet, &m_kdtree2d_data.query_point[0], nanoflann::SearchParams(kdtree_search_params.nChecks));
233 
234  // Copy output to user vars:
235  out_x1 = derived().kdtree_get_pt(ret_indexes[0],0);
236  out_y1 = derived().kdtree_get_pt(ret_indexes[0],1);
237  out_dist_sqr1 = ret_sqdist[0];
238 
239  out_x2 = derived().kdtree_get_pt(ret_indexes[1],0);
240  out_y2 = derived().kdtree_get_pt(ret_indexes[1],1);
241  out_dist_sqr2 = ret_sqdist[0];
242 
243  MRPT_END
244  }
245 
246 
247  inline void kdTreeTwoClosestPoint2D(const TPoint2D &p0,TPoint2D &pOut1,TPoint2D &pOut2,float &outDistSqr1,float &outDistSqr2) const {
248  float dmy1,dmy2,dmy3,dmy4;
249  kdTreeTwoClosestPoint2D(p0.x,p0.y,dmy1,dmy2,dmy3,dmy4,outDistSqr1,outDistSqr2);
250  pOut1.x=static_cast<double>(dmy1);
251  pOut1.y=static_cast<double>(dmy2);
252  pOut2.x=static_cast<double>(dmy3);
253  pOut2.y=static_cast<double>(dmy4);
254  }
255 
256  /** KD Tree-based search for the N closest point to some given 2D coordinates.
257  * This method automatically build the "m_kdtree_data" structure when:
258  * - It is called for the first time
259  * - The map has changed
260  * - The KD-tree was build for 3D.
261  *
262  * \param x0 The X coordinate of the query.
263  * \param y0 The Y coordinate of the query.
264  * \param N The number of closest points to search.
265  * \param out_x The vector containing the X coordinates of the correspondences.
266  * \param out_y The vector containing the Y coordinates of the correspondences.
267  * \param out_dist_sqr The vector containing the square distance between the query and the returned points.
268  *
269  * \return The list of indices
270  * \sa kdTreeClosestPoint2D
271  * \sa kdTreeTwoClosestPoint2D
272  */
273  inline
274  std::vector<size_t> kdTreeNClosestPoint2D(
275  float x0,
276  float y0,
277  size_t knn,
278  std::vector<float> &out_x,
279  std::vector<float> &out_y,
280  std::vector<float> &out_dist_sqr ) const
281  {
282  MRPT_START
283  rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
284  if ( !m_kdtree2d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
285 
286  std::vector<size_t> ret_indexes(knn);
287  out_x.resize(knn);
288  out_y.resize(knn);
289  out_dist_sqr.resize(knn);
290 
291  nanoflann::KNNResultSet<num_t> resultSet(knn);
292  resultSet.init(&ret_indexes[0], &out_dist_sqr[0] );
293 
294  m_kdtree2d_data.query_point[0] = x0;
295  m_kdtree2d_data.query_point[1] = y0;
296  m_kdtree2d_data.index->findNeighbors(resultSet, &m_kdtree2d_data.query_point[0], nanoflann::SearchParams(kdtree_search_params.nChecks));
297 
298  for (size_t i=0;i<knn;i++)
299  {
300  out_x[i] = derived().kdtree_get_pt(ret_indexes[i],0);
301  out_y[i] = derived().kdtree_get_pt(ret_indexes[i],1);
302  }
303  return ret_indexes;
304  MRPT_END
305  }
306 
307  inline std::vector<size_t> kdTreeNClosestPoint2D(const TPoint2D &p0,size_t N,std::vector<TPoint2D> &pOut,std::vector<float> &outDistSqr) const {
308  std::vector<float> dmy1,dmy2;
309  std::vector<size_t> res=kdTreeNClosestPoint2D(static_cast<float>(p0.x),static_cast<float>(p0.y),N,dmy1,dmy2,outDistSqr);
310  pOut.resize(dmy1.size());
311  for (size_t i=0;i<dmy1.size();i++) {
312  pOut[i].x=static_cast<double>(dmy1[i]);
313  pOut[i].y=static_cast<double>(dmy2[i]);
314  }
315  return res;
316  }
317 
318  /** KD Tree-based search for the N closest point to some given 2D coordinates and returns their indexes.
319  * This method automatically build the "m_kdtree_data" structure when:
320  * - It is called for the first time
321  * - The map has changed
322  * - The KD-tree was build for 3D.
323  *
324  * \param x0 The X coordinate of the query.
325  * \param y0 The Y coordinate of the query.
326  * \param N The number of closest points to search.
327  * \param out_idx The indexes of the found closest correspondence.
328  * \param out_dist_sqr The square distance between the query and the returned point.
329  *
330  * \sa kdTreeClosestPoint2D
331  */
332  inline void kdTreeNClosestPoint2DIdx(
333  float x0,
334  float y0,
335  size_t knn,
336  std::vector<size_t> &out_idx,
337  std::vector<float> &out_dist_sqr ) const
338  {
339  MRPT_START
340  rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
341  if ( !m_kdtree2d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
342 
343  out_idx.resize(knn);
344  out_dist_sqr.resize(knn);
345  nanoflann::KNNResultSet<num_t> resultSet(knn);
346  resultSet.init(&out_idx[0], &out_dist_sqr[0] );
347 
348  m_kdtree2d_data.query_point[0] = x0;
349  m_kdtree2d_data.query_point[1] = y0;
350  m_kdtree2d_data.index->findNeighbors(resultSet, &m_kdtree2d_data.query_point[0], nanoflann::SearchParams(kdtree_search_params.nChecks));
351  MRPT_END
352  }
353 
354  inline void kdTreeNClosestPoint2DIdx(const TPoint2D &p0,size_t N,std::vector<size_t> &outIdx,std::vector<float> &outDistSqr) const {
355  return kdTreeNClosestPoint2DIdx(static_cast<float>(p0.x),static_cast<float>(p0.y),N,outIdx,outDistSqr);
356  }
357 
358  /** KD Tree-based search for the closest point (only ONE) to some given 3D coordinates.
359  * This method automatically build the "m_kdtree_data" structure when:
360  * - It is called for the first time
361  * - The map has changed
362  * - The KD-tree was build for 2D.
363  *
364  * \param x0 The X coordinate of the query.
365  * \param y0 The Y coordinate of the query.
366  * \param z0 The Z coordinate of the query.
367  * \param out_x The X coordinate of the found closest correspondence.
368  * \param out_y The Y coordinate of the found closest correspondence.
369  * \param out_z The Z coordinate of the found closest correspondence.
370  * \param out_dist_sqr The square distance between the query and the returned point.
371  *
372  * \return The index of the closest point in the map array.
373  * \sa kdTreeClosestPoint2D
374  */
375  inline size_t kdTreeClosestPoint3D(
376  float x0,
377  float y0,
378  float z0,
379  float &out_x,
380  float &out_y,
381  float &out_z,
382  float &out_dist_sqr
383  ) const
384  {
385  MRPT_START
386  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
387  if ( !m_kdtree3d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
388 
389  const size_t knn = 1; // Number of points to retrieve
390  size_t ret_index;
391  nanoflann::KNNResultSet<num_t> resultSet(knn);
392  resultSet.init(&ret_index, &out_dist_sqr );
393 
394  m_kdtree3d_data.query_point[0] = x0;
395  m_kdtree3d_data.query_point[1] = y0;
396  m_kdtree3d_data.query_point[2] = z0;
397  m_kdtree3d_data.index->findNeighbors(resultSet, &m_kdtree3d_data.query_point[0], nanoflann::SearchParams(kdtree_search_params.nChecks));
398 
399  // Copy output to user vars:
400  out_x = derived().kdtree_get_pt(ret_index,0);
401  out_y = derived().kdtree_get_pt(ret_index,1);
402  out_z = derived().kdtree_get_pt(ret_index,2);
403 
404  return ret_index;
405  MRPT_END
406  }
407 
408  /// \overload
409  inline size_t kdTreeClosestPoint3D(
410  float x0,
411  float y0,
412  float z0,
413  float &out_dist_sqr
414  ) const
415  {
416  MRPT_START
417  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
418  if ( !m_kdtree3d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
419 
420  const size_t knn = 1; // Number of points to retrieve
421  size_t ret_index;
422  nanoflann::KNNResultSet<num_t> resultSet(knn);
423  resultSet.init(&ret_index, &out_dist_sqr );
424 
425  m_kdtree3d_data.query_point[0] = x0;
426  m_kdtree3d_data.query_point[1] = y0;
427  m_kdtree3d_data.query_point[2] = z0;
428  m_kdtree3d_data.index->findNeighbors(resultSet, &m_kdtree3d_data.query_point[0], nanoflann::SearchParams(kdtree_search_params.nChecks));
429 
430  return ret_index;
431  MRPT_END
432  }
433 
434  /// \overload
435  inline size_t kdTreeClosestPoint3D(const TPoint3D &p0,TPoint3D &pOut,float &outDistSqr) const {
436  float dmy1,dmy2,dmy3;
437  size_t res=kdTreeClosestPoint3D(static_cast<float>(p0.x),static_cast<float>(p0.y),static_cast<float>(p0.z),dmy1,dmy2,dmy3,outDistSqr);
438  pOut.x=static_cast<double>(dmy1);
439  pOut.y=static_cast<double>(dmy2);
440  pOut.z=static_cast<double>(dmy3);
441  return res;
442  }
443 
444  /** KD Tree-based search for the N closest points to some given 3D coordinates.
445  * This method automatically build the "m_kdtree_data" structure when:
446  * - It is called for the first time
447  * - The map has changed
448  * - The KD-tree was build for 2D.
449  *
450  * \param x0 The X coordinate of the query.
451  * \param y0 The Y coordinate of the query.
452  * \param z0 The Z coordinate of the query.
453  * \param N The number of closest points to search.
454  * \param out_x The vector containing the X coordinates of the correspondences.
455  * \param out_y The vector containing the Y coordinates of the correspondences.
456  * \param out_z The vector containing the Z coordinates of the correspondences.
457  * \param out_dist_sqr The vector containing the square distance between the query and the returned points.
458  *
459  * \sa kdTreeNClosestPoint2D
460  */
461  inline void kdTreeNClosestPoint3D(
462  float x0,
463  float y0,
464  float z0,
465  size_t knn,
466  std::vector<float> &out_x,
467  std::vector<float> &out_y,
468  std::vector<float> &out_z,
469  std::vector<float> &out_dist_sqr ) const
470  {
471  MRPT_START
472  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
473  if ( !m_kdtree3d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
474 
475  std::vector<size_t> ret_indexes(knn);
476  out_x.resize(knn);
477  out_y.resize(knn);
478  out_z.resize(knn);
479  out_dist_sqr.resize(knn);
480 
481  nanoflann::KNNResultSet<num_t> resultSet(knn);
482  resultSet.init(&ret_indexes[0], &out_dist_sqr[0] );
483 
484  m_kdtree3d_data.query_point[0] = x0;
485  m_kdtree3d_data.query_point[1] = y0;
486  m_kdtree3d_data.query_point[2] = z0;
487  m_kdtree3d_data.index->findNeighbors(resultSet, &m_kdtree3d_data.query_point[0], nanoflann::SearchParams(kdtree_search_params.nChecks));
488 
489  for (size_t i=0;i<knn;i++)
490  {
491  out_x[i] = derived().kdtree_get_pt(ret_indexes[i],0);
492  out_y[i] = derived().kdtree_get_pt(ret_indexes[i],1);
493  out_z[i] = derived().kdtree_get_pt(ret_indexes[i],2);
494  }
495  MRPT_END
496  }
497 
498  /** KD Tree-based search for the N closest points to some given 3D coordinates.
499  * This method automatically build the "m_kdtree_data" structure when:
500  * - It is called for the first time
501  * - The map has changed
502  * - The KD-tree was build for 2D.
503  *
504  * \param x0 The X coordinate of the query.
505  * \param y0 The Y coordinate of the query.
506  * \param z0 The Z coordinate of the query.
507  * \param N The number of closest points to search.
508  * \param out_x The vector containing the X coordinates of the correspondences.
509  * \param out_y The vector containing the Y coordinates of the correspondences.
510  * \param out_z The vector containing the Z coordinates of the correspondences.
511  * \param out_idx The vector containing the indexes of the correspondences.
512  * \param out_dist_sqr The vector containing the square distance between the query and the returned points.
513  *
514  * \sa kdTreeNClosestPoint2D
515  */
516  inline void kdTreeNClosestPoint3DWithIdx(
517  float x0,
518  float y0,
519  float z0,
520  size_t knn,
521  std::vector<float> &out_x,
522  std::vector<float> &out_y,
523  std::vector<float> &out_z,
524  std::vector<size_t> &out_idx,
525  std::vector<float> &out_dist_sqr ) const
526  {
527  MRPT_START
528  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
529  if ( !m_kdtree3d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
530 
531  out_x.resize(knn);
532  out_y.resize(knn);
533  out_z.resize(knn);
534  out_idx.resize(knn);
535  out_dist_sqr.resize(knn);
536 
537  nanoflann::KNNResultSet<num_t> resultSet(knn);
538  resultSet.init(&out_idx[0], &out_dist_sqr[0] );
539 
540  m_kdtree3d_data.query_point[0] = x0;
541  m_kdtree3d_data.query_point[1] = y0;
542  m_kdtree3d_data.query_point[2] = z0;
543  m_kdtree3d_data.index->findNeighbors(resultSet, &m_kdtree3d_data.query_point[0], nanoflann::SearchParams(kdtree_search_params.nChecks));
544 
545  for (size_t i=0;i<knn;i++)
546  {
547  out_x[i] = derived().kdtree_get_pt(out_idx[i],0);
548  out_y[i] = derived().kdtree_get_pt(out_idx[i],1);
549  out_z[i] = derived().kdtree_get_pt(out_idx[i],2);
550  }
551  MRPT_END
552  }
553 
554  inline void kdTreeNClosestPoint3D(const TPoint3D &p0,size_t N,std::vector<TPoint3D> &pOut,std::vector<float> &outDistSqr) const {
555  std::vector<float> dmy1,dmy2,dmy3;
556  kdTreeNClosestPoint3D(static_cast<float>(p0.x),static_cast<float>(p0.y),static_cast<float>(p0.z),N,dmy1,dmy2,dmy3,outDistSqr);
557  pOut.resize(dmy1.size());
558  for (size_t i=0;i<dmy1.size();i++) {
559  pOut[i].x=static_cast<double>(dmy1[i]);
560  pOut[i].y=static_cast<double>(dmy2[i]);
561  pOut[i].z=static_cast<double>(dmy3[i]);
562  }
563  }
564 
565  /** KD Tree-based search for all the points within a given radius of some 3D point.
566  * This method automatically build the "m_kdtree_data" structure when:
567  * - It is called for the first time
568  * - The map has changed
569  * - The KD-tree was build for 2D.
570  *
571  * \param x0 The X coordinate of the query.
572  * \param y0 The Y coordinate of the query.
573  * \param z0 The Z coordinate of the query.
574  * \param maxRadius The search radius.
575  * \param out_indices_dist The output list, with pairs of indeces/squared distances for the found correspondences.
576  * \return Number of found points.
577  *
578  * \sa kdTreeRadiusSearch2D, kdTreeNClosestPoint3DIdx
579  */
580  inline size_t kdTreeRadiusSearch3D(
581  const num_t x0, const num_t y0, const num_t z0,
582  const num_t maxRadius,
583  std::vector<std::pair<size_t,num_t> >& out_indices_dist ) const
584  {
585  MRPT_START
586  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
587  out_indices_dist.clear();
588  if ( m_kdtree3d_data.m_num_points!=0 )
589  {
590  const num_t xyz[3] = {x0,y0,z0};
591  m_kdtree3d_data.index->radiusSearch(&xyz[0], maxRadius, out_indices_dist, nanoflann::SearchParams(kdtree_search_params.nChecks) );
592  }
593  return out_indices_dist.size();
594  MRPT_END
595  }
596 
597  /** KD Tree-based search for all the points within a given radius of some 2D point.
598  * This method automatically build the "m_kdtree_data" structure when:
599  * - It is called for the first time
600  * - The map has changed
601  * - The KD-tree was build for 3D.
602  *
603  * \param x0 The X coordinate of the query.
604  * \param y0 The Y coordinate of the query.
605  * \param maxRadius The search radius.
606  * \param out_indices_dist The output list, with pairs of indeces/squared distances for the found correspondences.
607  * \return Number of found points.
608  *
609  * \sa kdTreeRadiusSearch3D, kdTreeNClosestPoint2DIdx
610  */
611  inline size_t kdTreeRadiusSearch2D(
612  const num_t x0, const num_t y0,
613  const num_t maxRadius,
614  std::vector<std::pair<size_t,num_t> >& out_indices_dist ) const
615  {
616  MRPT_START
617  rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
618  out_indices_dist.clear();
619  if ( m_kdtree2d_data.m_num_points!=0 )
620  {
621  const num_t xyz[2] = {x0,y0};
622  m_kdtree2d_data.index->radiusSearch(&xyz[0], maxRadius, out_indices_dist, nanoflann::SearchParams(kdtree_search_params.nChecks) );
623  }
624  return out_indices_dist.size();
625  MRPT_END
626  }
627 
628  /** KD Tree-based search for the N closest point to some given 3D coordinates and returns their indexes.
629  * This method automatically build the "m_kdtree_data" structure when:
630  * - It is called for the first time
631  * - The map has changed
632  * - The KD-tree was build for 2D.
633  *
634  * \param x0 The X coordinate of the query.
635  * \param y0 The Y coordinate of the query.
636  * \param z0 The Z coordinate of the query.
637  * \param N The number of closest points to search.
638  * \param out_idx The indexes of the found closest correspondence.
639  * \param out_dist_sqr The square distance between the query and the returned point.
640  *
641  * \sa kdTreeClosestPoint2D, kdTreeRadiusSearch3D
642  */
643  inline void kdTreeNClosestPoint3DIdx(
644  float x0,
645  float y0,
646  float z0,
647  size_t knn,
648  std::vector<size_t> &out_idx,
649  std::vector<float> &out_dist_sqr ) const
650  {
651  MRPT_START
652  rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
653  if ( !m_kdtree3d_data.m_num_points ) THROW_EXCEPTION("There are no points in the KD-tree.")
654 
655  out_idx.resize(knn);
656  out_dist_sqr.resize(knn);
657  nanoflann::KNNResultSet<num_t> resultSet(knn);
658  resultSet.init(&out_idx[0], &out_dist_sqr[0] );
659 
660  m_kdtree3d_data.query_point[0] = x0;
661  m_kdtree3d_data.query_point[1] = y0;
662  m_kdtree3d_data.query_point[2] = z0;
663  m_kdtree3d_data.index->findNeighbors(resultSet, &m_kdtree3d_data.query_point[0], nanoflann::SearchParams(kdtree_search_params.nChecks));
664  MRPT_END
665  }
666 
667  inline void kdTreeNClosestPoint3DIdx(const TPoint3D &p0,size_t N,std::vector<size_t> &outIdx,std::vector<float> &outDistSqr) const {
668  kdTreeNClosestPoint3DIdx(static_cast<float>(p0.x),static_cast<float>(p0.y),static_cast<float>(p0.z),N,outIdx,outDistSqr);
669  }
670 
671  /* @} */
672 
673  protected:
674  /** To be called by child classes when KD tree data changes. */
675  inline void kdtree_mark_as_outdated() const { m_kdtree_is_uptodate = false; }
676 
677  private:
678  /** Internal structure with the KD-tree representation (mainly used to avoid copying pointers with the = operator) */
679  template <int _DIM = -1>
680  struct TKDTreeDataHolder
681  {
682  /** Init the pointer to NULL. */
683  inline TKDTreeDataHolder() : index(NULL),m_dim(_DIM), m_num_points(0) { }
684 
685  /** Copy constructor: It actually does NOT copy the kd-tree, a new object will be created if required! */
686  inline TKDTreeDataHolder(const TKDTreeDataHolder &) : index(NULL),m_dim(_DIM), m_num_points(0) { }
687 
688  /** Copy operator: It actually does NOT copy the kd-tree, a new object will be created if required! */
689  inline TKDTreeDataHolder& operator =(const TKDTreeDataHolder &o) {
690  if (&o!=this) clear();
691  return *this;
692  }
693 
694  /** Free memory (if allocated) */
695  inline ~TKDTreeDataHolder() { clear(); }
696 
697  /** Free memory (if allocated) */
698  inline void clear() { mrpt::utils::delete_safe( index ); }
699 
700  typedef nanoflann::KDTreeSingleIndexAdaptor<metric_t,Derived, _DIM> kdtree_index_t;
701 
702  kdtree_index_t *index; //!< NULL or the up-to-date index
703 
704  std::vector<num_t> query_point;
705  size_t m_dim; //!< Dimensionality. typ: 2,3
706  size_t m_num_points;
707  };
708 
709  mutable TKDTreeDataHolder<2> m_kdtree2d_data;
710  mutable TKDTreeDataHolder<3> m_kdtree3d_data;
711  mutable TKDTreeDataHolder<> m_kdtreeNd_data;
712  mutable bool m_kdtree_is_uptodate; //!< whether the KD tree needs to be rebuilt or not.
713 
714  /// Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the data points.
715  void rebuild_kdTree_2D() const
716  {
717  typedef typename TKDTreeDataHolder<2>::kdtree_index_t tree2d_t;
718 
719  if (!m_kdtree_is_uptodate) { m_kdtree2d_data.clear(); m_kdtree3d_data.clear(); m_kdtreeNd_data.clear(); }
720 
721  if (!m_kdtree2d_data.index)
722  {
723  // Erase previous tree:
724  m_kdtree2d_data.clear();
725  // And build new index:
726  const size_t N = derived().kdtree_get_point_count();
727  m_kdtree2d_data.m_num_points = N;
728  m_kdtree2d_data.m_dim = 2;
729  m_kdtree2d_data.query_point.resize(2);
730  if (N)
731  {
732  m_kdtree2d_data.index = new tree2d_t(2, derived(), nanoflann::KDTreeSingleIndexAdaptorParams(kdtree_search_params.leaf_max_size, 2 ) );
733  m_kdtree2d_data.index->buildIndex();
734  }
735  m_kdtree_is_uptodate = true;
736  }
737  }
738 
739  /// Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the data points.
740  void rebuild_kdTree_3D() const
741  {
742  typedef typename TKDTreeDataHolder<3>::kdtree_index_t tree3d_t;
743 
744  if (!m_kdtree_is_uptodate) { m_kdtree2d_data.clear(); m_kdtree3d_data.clear(); m_kdtreeNd_data.clear(); }
745 
746  if (!m_kdtree3d_data.index)
747  {
748  // Erase previous tree:
749  m_kdtree3d_data.clear();
750  // And build new index:
751  const size_t N = derived().kdtree_get_point_count();
752  m_kdtree3d_data.m_num_points = N;
753  m_kdtree3d_data.m_dim = 3;
754  m_kdtree3d_data.query_point.resize(3);
755  if (N)
756  {
757  m_kdtree3d_data.index = new tree3d_t(3, derived(), nanoflann::KDTreeSingleIndexAdaptorParams(kdtree_search_params.leaf_max_size, 3 ) );
758  m_kdtree3d_data.index->buildIndex();
759  }
760  m_kdtree_is_uptodate = true;
761  }
762  }
763 
764  }; // end of KDTreeCapable
765 
766  /** @} */ // end of grouping
767 
768  } // End of namespace
769 } // End of namespace
770 #endif
mrpt::math::KDTreeCapable::~KDTreeCapable
~KDTreeCapable()
Destructor (nothing needed to do here)
Definition: KDTreeCapable.h:78
mrpt::math::TPoint2D::y
double y
X,Y coordinates.
Definition: lightweight_geom_data.h:35
mrpt::math::KDTreeCapable::KDTreeCapable
KDTreeCapable()
Constructor.
Definition: KDTreeCapable.h:75
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:180
mrpt::math::KDTreeCapable::kdTreeClosestPoint2D
size_t kdTreeClosestPoint2D(float x0, float y0, float &out_dist_sqr) const
Definition: KDTreeCapable.h:147
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: maps/CColouredPointsMap.h:110
mrpt::math::KDTreeCapable::derived
const Derived & derived() const
CRTP helper method.
Definition: KDTreeCapable.h:81
nanoflann::KNNResultSet::init
void init(IndexType *indices_, DistanceType *dists_)
Definition: nanoflann.hpp:76
mrpt::math::KDTreeCapable::m_kdtree2d_data
TKDTreeDataHolder< 2 > m_kdtree2d_data
Definition: KDTreeCapable.h:709
mrpt::math::KDTreeCapable::TKDTreeDataHolder::m_dim
size_t m_dim
Dimensionality. typ: 2,3.
Definition: KDTreeCapable.h:705
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:307
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:354
mrpt::math::KDTreeCapable::kdTreeClosestPoint2DsqrError
float kdTreeClosestPoint2DsqrError(const TPoint2D &p0) const
Definition: KDTreeCapable.h:189
mrpt::math::TPoint3D::z
double z
X,Y,Z coordinates.
Definition: lightweight_geom_data.h:231
mrpt::math::KDTreeCapable::kdTreeTwoClosestPoint2D
void kdTreeTwoClosestPoint2D(const TPoint2D &p0, TPoint2D &pOut1, TPoint2D &pOut2, float &outDistSqr1, float &outDistSqr2) const
Definition: KDTreeCapable.h:247
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:740
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:680
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:516
mrpt::math::KDTreeCapable::kdTreeNClosestPoint3D
void kdTreeNClosestPoint3D(const TPoint3D &p0, size_t N, std::vector< TPoint3D > &pOut, std::vector< float > &outDistSqr) const
Definition: KDTreeCapable.h:554
mrpt::math::KDTreeCapable
A generic adaptor class for providing Approximate Nearest Neighbors (ANN) (via the nanoflann library)...
Definition: KDTreeCapable.h:67
mrpt::math::KDTreeCapable::m_kdtree3d_data
TKDTreeDataHolder< 3 > m_kdtree3d_data
Definition: KDTreeCapable.h:710
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:689
MRPT_END
#define MRPT_END
Definition: maps/CColouredPointsMap.h:352
mrpt::math::KDTreeCapable::kdTreeClosestPoint3D
size_t kdTreeClosestPoint3D(const TPoint3D &p0, TPoint3D &pOut, float &outDistSqr) const
Definition: KDTreeCapable.h:435
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:643
mrpt::math::KDTreeCapable::TKDTreeDataHolder::TKDTreeDataHolder
TKDTreeDataHolder()
Init the pointer to NULL.
Definition: KDTreeCapable.h:683
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:375
mrpt::math::KDTreeCapable::TKDTreeDataHolder::~TKDTreeDataHolder
~TKDTreeDataHolder()
Free memory (if allocated)
Definition: KDTreeCapable.h:695
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:675
mrpt::math::KDTreeCapable::TKDTreeDataHolder::kdtree_index_t
nanoflann::KDTreeSingleIndexAdaptor< metric_t, Derived, _DIM > kdtree_index_t
Definition: KDTreeCapable.h:700
MRPT_START
#define MRPT_START
Definition: maps/CColouredPointsMap.h:348
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:667
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:153
mrpt::math::KDTreeCapable::kdTreeRadiusSearch2D
size_t kdTreeRadiusSearch2D(const num_t x0, const num_t y0, const num_t maxRadius, 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:611
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:117
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:712
mrpt::math::KDTreeCapable::kdTreeClosestPoint3D
size_t kdTreeClosestPoint3D(float x0, float y0, float z0, float &out_dist_sqr) const
Definition: KDTreeCapable.h:409
lightweight_geom_data.h
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:461
mrpt::math::KDTreeCapable::TKDTreeSearchParams::leaf_max_size
size_t leaf_max_size
Max points per leaf.
Definition: KDTreeCapable.h:94
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:274
nanoflann::KDTreeSingleIndexAdaptorParams
Parameters (see http://code.google.com/p/nanoflann/ for help choosing the parameters) ...
Definition: nanoflann.hpp:397
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:210
mrpt::math::KDTreeCapable::TKDTreeDataHolder::clear
void clear()
Free memory (if allocated)
Definition: KDTreeCapable.h:698
mrpt::math::KDTreeCapable::TKDTreeDataHolder::index
kdtree_index_t * index
NULL or the up-to-date index.
Definition: KDTreeCapable.h:702
nanoflann::SearchParams
Search options for KDTreeSingleIndexAdaptor::findNeighbors()
Definition: nanoflann.hpp:408
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:715
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:332
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:686
mrpt::math::KDTreeCapable::m_kdtreeNd_data
TKDTreeDataHolder m_kdtreeNd_data
Definition: KDTreeCapable.h:711
nanoflann::KDTreeSingleIndexAdaptor::findNeighbors
void findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const
Find set of nearest neighbors to vec[0:dim-1].
Definition: nanoflann.hpp:935
mrpt::math::KDTreeCapable::kdtree_search_params
TKDTreeSearchParams kdtree_search_params
Parameters to tune the ANN searches.
Definition: KDTreeCapable.h:97
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:229
mrpt::math::TPoint2D
Lightweight 2D point.
Definition: lightweight_geom_data.h:33
mrpt::math::KDTreeCapable::kdTreeRadiusSearch3D
size_t kdTreeRadiusSearch3D(const num_t x0, const num_t y0, const num_t z0, const num_t maxRadius, 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:580
nanoflann::KDTreeSingleIndexAdaptor::radiusSearch
size_t radiusSearch(const ElementType *query_point, const DistanceType radius, std::vector< std::pair< IndexType, DistanceType > > &IndicesDists, const SearchParams &searchParams) const
Find all the neighbors to query_point[0:dim-1] within a maximum radius.
Definition: nanoflann.hpp:972
mrpt::math::KDTreeCapable::kdTreeClosestPoint2D
size_t kdTreeClosestPoint2D(const TPoint2D &p0, TPoint2D &pOut, float &outDistSqr) const
Definition: KDTreeCapable.h:170
mrpt::math::KDTreeCapable::TKDTreeSearchParams::nChecks
int nChecks
The number of checks for ANN (default: 32) - corresponds to FLANN's SearchParams::check.
Definition: KDTreeCapable.h:93


Page generated by Doxygen 1.8.9.1 for MRPT 1.3.0 SVN: at Sun Sep 13 03:55:12 UTC 2015