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


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013