Main MRPT website > C++ reference
MRPT logo
nanoflann.hpp
Go to the documentation of this file.
1 /***********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
5  * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
6  * Copyright 2011-2013 Jose Luis Blanco (joseluisblancoc@gmail.com).
7  * All rights reserved.
8  *
9  * THE BSD LICENSE
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * 1. Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * 2. Redistributions in binary form must reproduce the above copyright
18  * notice, this list of conditions and the following disclaimer in the
19  * documentation and/or other materials provided with the distribution.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
22  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
23  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
24  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
26  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
27  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
28  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
29  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
30  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  *************************************************************************/
32 
33 #ifndef NANOFLANN_HPP_
34 #define NANOFLANN_HPP_
35 
36 #include <vector>
37 #include <cassert>
38 #include <algorithm>
39 #include <stdexcept>
40 #include <cstdio> // for fwrite()
41 #include <cmath> // for fabs(),...
42 #include <limits>
43 
44 // Avoid conflicting declaration of min/max macros in windows headers
45 #if !defined(NOMINMAX) && (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64))
46 # define NOMINMAX
47 # ifdef max
48 # undef max
49 # undef min
50 # endif
51 #endif
52 
53 namespace nanoflann
54 {
55 /** @addtogroup nanoflann_grp nanoflann C++ library for ANN
56  * @{ */
57 
58  /** Library version: 0xMmP (M=Major,m=minor,P=path) */
59  #define NANOFLANN_VERSION 0x117
60 
61  /** @addtogroup result_sets_grp Result set classes
62  * @{ */
63  template <typename DistanceType, typename IndexType = size_t, typename CountType = size_t>
65  {
66  IndexType * indices;
67  DistanceType* dists;
68  CountType capacity;
69  CountType count;
70 
71  public:
72  inline KNNResultSet(CountType capacity_) : capacity(capacity_), count(0)
73  {
74  }
75 
76  inline void init(IndexType* indices_, DistanceType* dists_)
77  {
78  indices = indices_;
79  dists = dists_;
80  count = 0;
81  dists[capacity-1] = (std::numeric_limits<DistanceType>::max)();
82  }
83 
84  inline CountType size() const
85  {
86  return count;
87  }
88 
89  inline bool full() const
90  {
91  return count == capacity;
92  }
93 
94 
95  inline void addPoint(DistanceType dist, IndexType index)
96  {
97  CountType i;
98  for (i=count; i>0; --i) {
99 #ifdef NANOFLANN_FIRST_MATCH // If defined and two poins have the same distance, the one with the lowest-index will be returned first.
100  if ( (dists[i-1]>dist) || ((dist==dists[i-1])&&(indices[i-1]>index)) ) {
101 #else
102  if (dists[i-1]>dist) {
103 #endif
104  if (i<capacity) {
105  dists[i] = dists[i-1];
106  indices[i] = indices[i-1];
107  }
108  }
109  else break;
110  }
111  if (i<capacity) {
112  dists[i] = dist;
113  indices[i] = index;
114  }
115  if (count<capacity) count++;
116  }
117 
118  inline DistanceType worstDist() const
119  {
120  return dists[capacity-1];
121  }
122  };
123 
124 
125  /**
126  * A result-set class used when performing a radius based search.
127  */
128  template <typename DistanceType, typename IndexType = size_t>
130  {
131  public:
132  const DistanceType radius;
133 
134  std::vector<std::pair<IndexType,DistanceType> >& m_indices_dists;
135 
136  inline RadiusResultSet(DistanceType radius_, std::vector<std::pair<IndexType,DistanceType> >& indices_dists) : radius(radius_), m_indices_dists(indices_dists)
137  {
138  init();
139  }
140 
141  inline ~RadiusResultSet() { }
142 
143  inline void init() { clear(); }
144  inline void clear() { m_indices_dists.clear(); }
145 
146  inline size_t size() const { return m_indices_dists.size(); }
147 
148  inline bool full() const { return true; }
149 
150  inline void addPoint(DistanceType dist, IndexType index)
151  {
152  if (dist<radius)
153  m_indices_dists.push_back(std::pair<IndexType,DistanceType>(index,dist));
154  }
155 
156  inline DistanceType worstDist() const { return radius; }
157 
158  /** Clears the result set and adjusts the search radius. */
159  inline void set_radius_and_clear( const DistanceType r )
160  {
161  radius = r;
162  clear();
163  }
164 
165  /**
166  * Find the worst result (furtherest neighbor) without copying or sorting
167  * Pre-conditions: size() > 0
168  */
169  std::pair<IndexType,DistanceType> worst_item() const
170  {
171  if (m_indices_dists.empty()) throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on an empty list of results.");
172  typedef typename std::vector<std::pair<IndexType,DistanceType> >::const_iterator DistIt;
173  DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end());
174  return *it;
175  }
176  };
177 
178  /** operator "<" for std::sort() */
180  {
181  /** PairType will be typically: std::pair<IndexType,DistanceType> */
182  template <typename PairType>
183  inline bool operator()(const PairType &p1, const PairType &p2) const {
184  return p1.second < p2.second;
185  }
186  };
187 
188  /** @} */
189 
190 
191  /** @addtogroup loadsave_grp Load/save auxiliary functions
192  * @{ */
193  template<typename T>
194  void save_value(FILE* stream, const T& value, size_t count = 1)
195  {
196  fwrite(&value, sizeof(value),count, stream);
197  }
198 
199  template<typename T>
200  void save_value(FILE* stream, const std::vector<T>& value)
201  {
202  size_t size = value.size();
203  fwrite(&size, sizeof(size_t), 1, stream);
204  fwrite(&value[0], sizeof(T), size, stream);
205  }
206 
207  template<typename T>
208  void load_value(FILE* stream, T& value, size_t count = 1)
209  {
210  size_t read_cnt = fread(&value, sizeof(value), count, stream);
211  if (read_cnt != count) {
212  throw std::runtime_error("Cannot read from file");
213  }
214  }
215 
216 
217  template<typename T>
218  void load_value(FILE* stream, std::vector<T>& value)
219  {
220  size_t size;
221  size_t read_cnt = fread(&size, sizeof(size_t), 1, stream);
222  if (read_cnt!=1) {
223  throw std::runtime_error("Cannot read from file");
224  }
225  value.resize(size);
226  read_cnt = fread(&value[0], sizeof(T), size, stream);
227  if (read_cnt!=size) {
228  throw std::runtime_error("Cannot read from file");
229  }
230  }
231  /** @} */
232 
233 
234  /** @addtogroup metric_grp Metric (distance) classes
235  * @{ */
236 
237  template<typename T> inline T abs(T x) { return (x<0) ? -x : x; }
238  template<> inline int abs<int>(int x) { return ::abs(x); }
239  template<> inline float abs<float>(float x) { return fabsf(x); }
240  template<> inline double abs<double>(double x) { return fabs(x); }
241  template<> inline long double abs<long double>(long double x) { return fabsl(x); }
242 
243  /** Manhattan distance functor (generic version, optimized for high-dimensionality data sets).
244  * Corresponding distance traits: nanoflann::metric_L1
245  * \tparam T Type of the elements (e.g. double, float, uint8_t)
246  * \tparam DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t)
247  */
248  template<class T, class DataSource, typename _DistanceType = T>
249  struct L1_Adaptor
250  {
251  typedef T ElementType;
252  typedef _DistanceType DistanceType;
253 
254  const DataSource &data_source;
255 
256  L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) { }
257 
258  inline DistanceType operator()(const T* a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const
259  {
260  DistanceType result = DistanceType();
261  const T* last = a + size;
262  const T* lastgroup = last - 3;
263  size_t d = 0;
264 
265  /* Process 4 items with each loop for efficiency. */
266  while (a < lastgroup) {
267  const DistanceType diff0 = nanoflann::abs(a[0] - data_source.kdtree_get_pt(b_idx,d++));
268  const DistanceType diff1 = nanoflann::abs(a[1] - data_source.kdtree_get_pt(b_idx,d++));
269  const DistanceType diff2 = nanoflann::abs(a[2] - data_source.kdtree_get_pt(b_idx,d++));
270  const DistanceType diff3 = nanoflann::abs(a[3] - data_source.kdtree_get_pt(b_idx,d++));
271  result += diff0 + diff1 + diff2 + diff3;
272  a += 4;
273  if ((worst_dist>0)&&(result>worst_dist)) {
274  return result;
275  }
276  }
277  /* Process last 0-3 components. Not needed for standard vector lengths. */
278  while (a < last) {
279  result += nanoflann::abs( *a++ - data_source.kdtree_get_pt(b_idx,d++) );
280  }
281  return result;
282  }
283 
284  template <typename U, typename V>
285  inline DistanceType accum_dist(const U a, const V b, int ) const
286  {
287  return nanoflann::abs(a-b);
288  }
289  };
290 
291  /** Squared Euclidean distance functor (generic version, optimized for high-dimensionality data sets).
292  * Corresponding distance traits: nanoflann::metric_L2
293  * \tparam T Type of the elements (e.g. double, float, uint8_t)
294  * \tparam DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t)
295  */
296  template<class T, class DataSource, typename _DistanceType = T>
297  struct L2_Adaptor
298  {
299  typedef T ElementType;
300  typedef _DistanceType DistanceType;
301 
302  const DataSource &data_source;
303 
304  L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) { }
305 
306  inline DistanceType operator()(const T* a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const
307  {
308  DistanceType result = DistanceType();
309  const T* last = a + size;
310  const T* lastgroup = last - 3;
311  size_t d = 0;
312 
313  /* Process 4 items with each loop for efficiency. */
314  while (a < lastgroup) {
315  const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx,d++);
316  const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx,d++);
317  const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx,d++);
318  const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx,d++);
319  result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
320  a += 4;
321  if ((worst_dist>0)&&(result>worst_dist)) {
322  return result;
323  }
324  }
325  /* Process last 0-3 components. Not needed for standard vector lengths. */
326  while (a < last) {
327  const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx,d++);
328  result += diff0 * diff0;
329  }
330  return result;
331  }
332 
333  template <typename U, typename V>
334  inline DistanceType accum_dist(const U a, const V b, int ) const
335  {
336  return (a-b)*(a-b);
337  }
338  };
339 
340  /** Squared Euclidean distance functor (suitable for low-dimensionality datasets, like 2D or 3D point clouds)
341  * Corresponding distance traits: nanoflann::metric_L2_Simple
342  * \tparam T Type of the elements (e.g. double, float, uint8_t)
343  * \tparam DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t)
344  */
345  template<class T, class DataSource, typename _DistanceType = T>
347  {
348  typedef T ElementType;
349  typedef _DistanceType DistanceType;
350 
351  const DataSource &data_source;
352 
353  L2_Simple_Adaptor(const DataSource &_data_source) : data_source(_data_source) { }
354 
355  inline DistanceType operator()(const T* a, const size_t b_idx, size_t size) const {
356  return data_source.kdtree_distance(a,b_idx,size);
357  }
358 
359  template <typename U, typename V>
360  inline DistanceType accum_dist(const U a, const V b, int ) const
361  {
362  return (a-b)*(a-b);
363  }
364  };
365 
366  /** Metaprogramming helper traits class for the L1 (Manhattan) metric */
367  struct metric_L1 {
368  template<class T, class DataSource>
369  struct traits {
371  };
372  };
373  /** Metaprogramming helper traits class for the L2 (Euclidean) metric */
374  struct metric_L2 {
375  template<class T, class DataSource>
376  struct traits {
378  };
379  };
380  /** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */
382  template<class T, class DataSource>
383  struct traits {
385  };
386  };
387 
388  /** @} */
389 
390 
391 
392  /** @addtogroup param_grp Parameter structs
393  * @{ */
394 
395  /** Parameters (see http://code.google.com/p/nanoflann/ for help choosing the parameters)
396  */
398  {
399  KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10, int dim_ = -1) :
400  leaf_max_size(_leaf_max_size), dim(dim_)
401  {}
402 
404  int dim;
405  };
406 
407  /** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */
409  {
410  /** Note: The first argument (checks_IGNORED_) is ignored, but kept for compatibility with the FLANN interface */
411  SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true ) :
412  checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {}
413 
414  int checks; //!< Ignored parameter (Kept for compatibility with the FLANN interface).
415  float eps; //!< search for eps-approximate neighbours (default: 0)
416  bool sorted; //!< only for radius search, require neighbours sorted by distance (default: true)
417  };
418  /** @} */
419 
420 
421  /** @addtogroup memalloc_grp Memory allocation
422  * @{ */
423 
424  /**
425  * Allocates (using C's malloc) a generic type T.
426  *
427  * Params:
428  * count = number of instances to allocate.
429  * Returns: pointer (of type T*) to memory buffer
430  */
431  template <typename T>
432  inline T* allocate(size_t count = 1)
433  {
434  T* mem = (T*) ::malloc(sizeof(T)*count);
435  return mem;
436  }
437 
438 
439  /**
440  * Pooled storage allocator
441  *
442  * The following routines allow for the efficient allocation of storage in
443  * small chunks from a specified pool. Rather than allowing each structure
444  * to be freed individually, an entire pool of storage is freed at once.
445  * This method has two advantages over just using malloc() and free(). First,
446  * it is far more efficient for allocating small objects, as there is
447  * no overhead for remembering all the information needed to free each
448  * object or consolidating fragmented memory. Second, the decision about
449  * how long to keep an object is made at the time of allocation, and there
450  * is no need to track down all the objects to free them.
451  *
452  */
453 
454  const size_t WORDSIZE=16;
455  const size_t BLOCKSIZE=8192;
456 
458  {
459  /* We maintain memory alignment to word boundaries by requiring that all
460  allocations be in multiples of the machine wordsize. */
461  /* Size of machine word in bytes. Must be power of 2. */
462  /* Minimum number of bytes requested at a time from the system. Must be multiple of WORDSIZE. */
463 
464 
465  size_t remaining; /* Number of bytes left in current block of storage. */
466  void* base; /* Pointer to base of current block of storage. */
467  void* loc; /* Current location in block to next allocate memory. */
468  size_t blocksize;
469 
471  {
472  remaining = 0;
473  base = NULL;
474  usedMemory = 0;
475  wastedMemory = 0;
476  }
477 
478  public:
479  size_t usedMemory;
480  size_t wastedMemory;
481 
482  /**
483  Default constructor. Initializes a new pool.
484  */
485  PooledAllocator(const size_t blocksize_ = BLOCKSIZE) : blocksize(blocksize_) {
486  internal_init();
487  }
488 
489  /**
490  * Destructor. Frees all the memory allocated in this pool.
491  */
493  free_all();
494  }
495 
496  /** Frees all allocated memory chunks */
497  void free_all()
498  {
499  while (base != NULL) {
500  void *prev = *((void**) base); /* Get pointer to prev block. */
501  ::free(base);
502  base = prev;
503  }
504  internal_init();
505  }
506 
507  /**
508  * Returns a pointer to a piece of new memory of the given size in bytes
509  * allocated from the pool.
510  */
511  void* malloc(const size_t req_size)
512  {
513  /* Round size up to a multiple of wordsize. The following expression
514  only works for WORDSIZE that is a power of 2, by masking last bits of
515  incremented size to zero.
516  */
517  const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);
518 
519  /* Check whether a new block must be allocated. Note that the first word
520  of a block is reserved for a pointer to the previous block.
521  */
522  if (size > remaining) {
523 
524  wastedMemory += remaining;
525 
526  /* Allocate new storage. */
527  const size_t blocksize = (size + sizeof(void*) + (WORDSIZE-1) > BLOCKSIZE) ?
528  size + sizeof(void*) + (WORDSIZE-1) : BLOCKSIZE;
529 
530  // use the standard C malloc to allocate memory
531  void* m = ::malloc(blocksize);
532  if (!m) {
533  fprintf(stderr,"Failed to allocate memory.\n");
534  return NULL;
535  }
536 
537  /* Fill first word of new block with pointer to previous block. */
538  ((void**) m)[0] = base;
539  base = m;
540 
541  size_t shift = 0;
542  //int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & (WORDSIZE-1))) & (WORDSIZE-1);
543 
544  remaining = blocksize - sizeof(void*) - shift;
545  loc = ((char*)m + sizeof(void*) + shift);
546  }
547  void* rloc = loc;
548  loc = (char*)loc + size;
549  remaining -= size;
550 
551  usedMemory += size;
552 
553  return rloc;
554  }
555 
556  /**
557  * Allocates (using this pool) a generic type T.
558  *
559  * Params:
560  * count = number of instances to allocate.
561  * Returns: pointer (of type T*) to memory buffer
562  */
563  template <typename T>
564  T* allocate(const size_t count = 1)
565  {
566  T* mem = (T*) this->malloc(sizeof(T)*count);
567  return mem;
568  }
569 
570  };
571  /** @} */
572 
573  /** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff
574  * @{ */
575 
576  // ---------------- CArray -------------------------
577  /** A STL container (as wrapper) for arrays of constant size defined at compile time (class imported from the MRPT project)
578  * This code is an adapted version from Boost, modifed for its integration
579  * within MRPT (JLBC, Dec/2009) (Renamed array -> CArray to avoid possible potential conflicts).
580  * See
581  * http://www.josuttis.com/cppcode
582  * for details and the latest version.
583  * See
584  * http://www.boost.org/libs/array for Documentation.
585  * for documentation.
586  *
587  * (C) Copyright Nicolai M. Josuttis 2001.
588  * Permission to copy, use, modify, sell and distribute this software
589  * is granted provided this copyright notice appears in all copies.
590  * This software is provided "as is" without express or implied
591  * warranty, and with no claim as to its suitability for any purpose.
592  *
593  * 29 Jan 2004 - minor fixes (Nico Josuttis)
594  * 04 Dec 2003 - update to synch with library TR1 (Alisdair Meredith)
595  * 23 Aug 2002 - fix for Non-MSVC compilers combined with MSVC libraries.
596  * 05 Aug 2001 - minor update (Nico Josuttis)
597  * 20 Jan 2001 - STLport fix (Beman Dawes)
598  * 29 Sep 2000 - Initial Revision (Nico Josuttis)
599  *
600  * Jan 30, 2004
601  */
602  template <typename T, std::size_t N>
603  class CArray {
604  public:
605  T elems[N]; // fixed-size array of elements of type T
606 
607  public:
608  // type definitions
609  typedef T value_type;
610  typedef T* iterator;
611  typedef const T* const_iterator;
612  typedef T& reference;
613  typedef const T& const_reference;
614  typedef std::size_t size_type;
615  typedef std::ptrdiff_t difference_type;
616 
617  // iterator support
618  inline iterator begin() { return elems; }
619  inline const_iterator begin() const { return elems; }
620  inline iterator end() { return elems+N; }
621  inline const_iterator end() const { return elems+N; }
622 
623  // reverse iterator support
624 #if !defined(BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) && !defined(BOOST_MSVC_STD_ITERATOR) && !defined(BOOST_NO_STD_ITERATOR_TRAITS)
625  typedef std::reverse_iterator<iterator> reverse_iterator;
626  typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
627 #elif defined(_MSC_VER) && (_MSC_VER == 1300) && defined(BOOST_DINKUMWARE_STDLIB) && (BOOST_DINKUMWARE_STDLIB == 310)
628  // workaround for broken reverse_iterator in VC7
629  typedef std::reverse_iterator<std::_Ptrit<value_type, difference_type, iterator,
631  typedef std::reverse_iterator<std::_Ptrit<value_type, difference_type, const_iterator,
633 #else
634  // workaround for broken reverse_iterator implementations
635  typedef std::reverse_iterator<iterator,T> reverse_iterator;
636  typedef std::reverse_iterator<const_iterator,T> const_reverse_iterator;
637 #endif
638 
643  // operator[]
644  inline reference operator[](size_type i) { return elems[i]; }
645  inline const_reference operator[](size_type i) const { return elems[i]; }
646  // at() with range check
647  reference at(size_type i) { rangecheck(i); return elems[i]; }
648  const_reference at(size_type i) const { rangecheck(i); return elems[i]; }
649  // front() and back()
650  reference front() { return elems[0]; }
651  const_reference front() const { return elems[0]; }
652  reference back() { return elems[N-1]; }
653  const_reference back() const { return elems[N-1]; }
654  // size is constant
655  static inline size_type size() { return N; }
656  static bool empty() { return false; }
657  static size_type max_size() { return N; }
658  enum { static_size = N };
659  /** This method has no effects in this class, but raises an exception if the expected size does not match */
660  inline void resize(const size_t nElements) { if (nElements!=N) throw std::logic_error("Try to change the size of a CArray."); }
661  // swap (note: linear complexity in N, constant for given instantiation)
662  void swap (CArray<T,N>& y) { std::swap_ranges(begin(),end(),y.begin()); }
663  // direct access to data (read-only)
664  const T* data() const { return elems; }
665  // use array as C array (direct read/write access to data)
666  T* data() { return elems; }
667  // assignment with type conversion
668  template <typename T2> CArray<T,N>& operator= (const CArray<T2,N>& rhs) {
669  std::copy(rhs.begin(),rhs.end(), begin());
670  return *this;
671  }
672  // assign one value to all elements
673  inline void assign (const T& value) { for (size_t i=0;i<N;i++) elems[i]=value; }
674  // assign (compatible with std::vector's one) (by JLBC for MRPT)
675  void assign (const size_t n, const T& value) { assert(N==n); for (size_t i=0;i<N;i++) elems[i]=value; }
676  private:
677  // check range (may be private because it is static)
678  static void rangecheck (size_type i) { if (i >= size()) { throw std::out_of_range("CArray<>: index out of range"); } }
679  }; // end of CArray
680 
681  /** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors when DIM=-1.
682  * Fixed size version for a generic DIM:
683  */
684  template <int DIM, typename T>
686  {
688  };
689  /** Dynamic size version */
690  template <typename T>
692  typedef std::vector<T> container_t;
693  };
694  /** @} */
695 
696  /** @addtogroup kdtrees_grp KD-tree classes and adaptors
697  * @{ */
698 
699  /** kd-tree index
700  *
701  * Contains the k-d trees and other information for indexing a set of points
702  * for nearest-neighbor matching.
703  *
704  * The class "DatasetAdaptor" must provide the following interface (can be non-virtual, inlined methods):
705  *
706  * \code
707  * // Must return the number of data points
708  * inline size_t kdtree_get_point_count() const { ... }
709  *
710  * // Must return the Euclidean (L2) distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
711  * inline DistanceType kdtree_distance(const T *p1, const size_t idx_p2,size_t size) const { ... }
712  *
713  * // Must return the dim'th component of the idx'th point in the class:
714  * inline T kdtree_get_pt(const size_t idx, int dim) const { ... }
715  *
716  * // Optional bounding-box computation: return false to default to a standard bbox computation loop.
717  * // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
718  * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
719  * template <class BBOX>
720  * bool kdtree_get_bbox(BBOX &bb) const
721  * {
722  * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits
723  * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits
724  * ...
725  * return true;
726  * }
727  *
728  * \endcode
729  *
730  * \tparam IndexType Will be typically size_t or int
731  */
732  template <typename Distance, class DatasetAdaptor,int DIM = -1, typename IndexType = size_t>
734  {
735  public:
736  typedef typename Distance::ElementType ElementType;
737  typedef typename Distance::DistanceType DistanceType;
738  protected:
739 
740  /**
741  * Array of indices to vectors in the dataset.
742  */
743  std::vector<IndexType> vind;
744 
746 
747 
748  /**
749  * The dataset used by this index
750  */
751  const DatasetAdaptor &dataset; //!< The source of our data
752 
754 
755  size_t m_size;
756  int dim; //!< Dimensionality of each data point
757 
758 
759  /*--------------------- Internal Data Structures --------------------------*/
760  struct Node
761  {
762  union {
763  struct
764  {
765  /**
766  * Indices of points in leaf node
767  */
768  IndexType left, right;
769  } lr;
770  struct
771  {
772  /**
773  * Dimension used for subdivision.
774  */
775  int divfeat;
776  /**
777  * The values used for subdivision.
778  */
780  } sub;
781  };
782  /**
783  * The child nodes.
784  */
785  Node* child1, * child2;
786  };
787  typedef Node* NodePtr;
788 
789 
790  struct Interval
791  {
793  };
794 
795  /** Define "BoundingBox" as a fixed-size or variable-size container depending on "DIM" */
797 
798  /** Define "distance_vector_t" as a fixed-size or variable-size container depending on "DIM" */
800 
801  /** This record represents a branch point when finding neighbors in
802  the tree. It contains a record of the minimum distance to the query
803  point, as well as the node at which the search resumes.
804  */
805  template <typename T, typename DistanceType>
807  {
808  T node; /* Tree node at which search resumes */
809  DistanceType mindist; /* Minimum distance to query for all nodes below. */
810 
812  BranchStruct(const T& aNode, DistanceType dist) : node(aNode), mindist(dist) {}
813 
814  inline bool operator<(const BranchStruct<T, DistanceType>& rhs) const
815  {
816  return mindist<rhs.mindist;
817  }
818  };
819 
820  /**
821  * Array of k-d trees used to find neighbours.
822  */
825  typedef BranchSt* Branch;
826 
828 
829  /**
830  * Pooled memory allocator.
831  *
832  * Using a pooled memory allocator is more efficient
833  * than allocating memory directly when there is a large
834  * number small of memory allocations.
835  */
837 
838  public:
839 
840  Distance distance;
841 
842  /**
843  * KDTree constructor
844  *
845  * Params:
846  * inputData = dataset with the input features
847  * params = parameters passed to the kdtree algorithm (see http://code.google.com/p/nanoflann/ for help choosing the parameters)
848  */
849  KDTreeSingleIndexAdaptor(const int dimensionality, const DatasetAdaptor& inputData, const KDTreeSingleIndexAdaptorParams& params = KDTreeSingleIndexAdaptorParams() ) :
850  dataset(inputData), index_params(params), root_node(NULL), distance(inputData)
851  {
852  m_size = dataset.kdtree_get_point_count();
853  dim = dimensionality;
854  if (DIM>0) dim=DIM;
855  else {
856  if (params.dim>0) dim = params.dim;
857  }
858  m_leaf_max_size = params.leaf_max_size;
859 
860  // Create a permutable array of indices to the input vectors.
861  init_vind();
862  }
863 
864  /**
865  * Standard destructor
866  */
868  {
869  }
870 
871  /** Frees the previously-built index. Automatically called within buildIndex(). */
872  void freeIndex()
873  {
874  pool.free_all();
875  root_node=NULL;
876  }
877 
878  /**
879  * Builds the index
880  */
881  void buildIndex()
882  {
883  init_vind();
884  computeBoundingBox(root_bbox);
885  freeIndex();
886  root_node = divideTree(0, m_size, root_bbox ); // construct the tree
887  }
888 
889  /**
890  * Returns size of index.
891  */
892  size_t size() const
893  {
894  return m_size;
895  }
896 
897  /**
898  * Returns the length of an index feature.
899  */
900  size_t veclen() const
901  {
902  return static_cast<size_t>(DIM>0 ? DIM : dim);
903  }
904 
905  /**
906  * Computes the inde memory usage
907  * Returns: memory used by the index
908  */
909  size_t usedMemory() const
910  {
911  return pool.usedMemory+pool.wastedMemory+dataset.kdtree_get_point_count()*sizeof(IndexType); // pool memory and vind array memory
912  }
913 
914  /** \name Query methods
915  * @{ */
916 
917  /**
918  * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored inside
919  * the result object.
920  *
921  * Params:
922  * result = the result object in which the indices of the nearest-neighbors are stored
923  * vec = the vector for which to search the nearest neighbors
924  *
925  * \tparam RESULTSET Should be any ResultSet<DistanceType>
926  * \sa knnSearch, radiusSearch
927  */
928  template <typename RESULTSET>
929  void findNeighbors(RESULTSET& result, const ElementType* vec, const SearchParams& searchParams) const
930  {
931  assert(vec);
932  if (!root_node) throw std::runtime_error("[nanoflann] findNeighbors() called before building the index.");
933  float epsError = 1+searchParams.eps;
934 
935  distance_vector_t dists; // fixed or variable-sized container (depending on DIM)
936  dists.assign((DIM>0 ? DIM : dim) ,0); // Fill it with zeros.
937  DistanceType distsq = computeInitialDistances(vec, dists);
938  searchLevel(result, vec, root_node, distsq, dists, epsError); // "count_leaf" parameter removed since was neither used nor returned to the user.
939  }
940 
941  /**
942  * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. Their indices are stored inside
943  * the result object.
944  * \sa radiusSearch, findNeighbors
945  * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
946  */
947  inline void knnSearch(const ElementType *query_point, const size_t num_closest, IndexType *out_indices, DistanceType *out_distances_sq, const int nChecks_IGNORED = 10) const
948  {
950  resultSet.init(out_indices, out_distances_sq);
951  this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
952  }
953 
954  /**
955  * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius.
956  * The output is given as a vector of pairs, of which the first element is a point index and the second the corresponding distance.
957  * Previous contents of \a IndicesDists are cleared.
958  *
959  * If searchParams.sorted==true, the output list is sorted by ascending distances.
960  *
961  * For a better performance, it is advisable to do a .reserve() on the vector if you have any wild guess about the number of expected matches.
962  *
963  * \sa knnSearch, findNeighbors
964  * \return The number of points within the given radius (i.e. indices.size() or dists.size() )
965  */
966  size_t radiusSearch(const ElementType *query_point,const DistanceType radius, std::vector<std::pair<IndexType,DistanceType> >& IndicesDists, const SearchParams& searchParams) const
967  {
968  RadiusResultSet<DistanceType,IndexType> resultSet(radius,IndicesDists);
969  this->findNeighbors(resultSet, query_point, searchParams);
970 
971  if (searchParams.sorted)
972  std::sort(IndicesDists.begin(),IndicesDists.end(), IndexDist_Sorter() );
973 
974  return resultSet.size();
975  }
976 
977  /** @} */
978 
979  private:
980  /** Make sure the auxiliary list \a vind has the same size than the current dataset, and re-generate if size has changed. */
981  void init_vind()
982  {
983  // Create a permutable array of indices to the input vectors.
984  m_size = dataset.kdtree_get_point_count();
985  if (vind.size()!=m_size) vind.resize(m_size);
986  for (size_t i = 0; i < m_size; i++) vind[i] = i;
987  }
988 
989  /// Helper accessor to the dataset points:
990  inline ElementType dataset_get(size_t idx, int component) const {
991  return dataset.kdtree_get_pt(idx,component);
992  }
993 
994 
995  void save_tree(FILE* stream, NodePtr tree)
996  {
997  save_value(stream, *tree);
998  if (tree->child1!=NULL) {
999  save_tree(stream, tree->child1);
1000  }
1001  if (tree->child2!=NULL) {
1002  save_tree(stream, tree->child2);
1003  }
1004  }
1005 
1006 
1007  void load_tree(FILE* stream, NodePtr& tree)
1008  {
1009  tree = pool.allocate<Node>();
1010  load_value(stream, *tree);
1011  if (tree->child1!=NULL) {
1012  load_tree(stream, tree->child1);
1013  }
1014  if (tree->child2!=NULL) {
1015  load_tree(stream, tree->child2);
1016  }
1017  }
1018 
1019 
1021  {
1022  bbox.resize((DIM>0 ? DIM : dim));
1023  if (dataset.kdtree_get_bbox(bbox))
1024  {
1025  // Done! It was implemented in derived class
1026  }
1027  else
1028  {
1029  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1030  bbox[i].low =
1031  bbox[i].high = dataset_get(0,i);
1032  }
1033  const size_t N = dataset.kdtree_get_point_count();
1034  for (size_t k=1; k<N; ++k) {
1035  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1036  if (dataset_get(k,i)<bbox[i].low) bbox[i].low = dataset_get(k,i);
1037  if (dataset_get(k,i)>bbox[i].high) bbox[i].high = dataset_get(k,i);
1038  }
1039  }
1040  }
1041  }
1042 
1043 
1044  /**
1045  * Create a tree node that subdivides the list of vecs from vind[first]
1046  * to vind[last]. The routine is called recursively on each sublist.
1047  * Place a pointer to this new tree node in the location pTree.
1048  *
1049  * Params: pTree = the new node to create
1050  * first = index of the first vector
1051  * last = index of the last vector
1052  */
1053  NodePtr divideTree(const IndexType left, const IndexType right, BoundingBox& bbox)
1054  {
1055  NodePtr node = pool.allocate<Node>(); // allocate memory
1056 
1057  /* If too few exemplars remain, then make this a leaf node. */
1058  if ( (right-left) <= m_leaf_max_size) {
1059  node->child1 = node->child2 = NULL; /* Mark as leaf node. */
1060  node->lr.left = left;
1061  node->lr.right = right;
1062 
1063  // compute bounding-box of leaf points
1064  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1065  bbox[i].low = dataset_get(vind[left],i);
1066  bbox[i].high = dataset_get(vind[left],i);
1067  }
1068  for (IndexType k=left+1; k<right; ++k) {
1069  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1070  if (bbox[i].low>dataset_get(vind[k],i)) bbox[i].low=dataset_get(vind[k],i);
1071  if (bbox[i].high<dataset_get(vind[k],i)) bbox[i].high=dataset_get(vind[k],i);
1072  }
1073  }
1074  }
1075  else {
1076  IndexType idx;
1077  int cutfeat;
1078  DistanceType cutval;
1079  middleSplit_(&vind[0]+left, right-left, idx, cutfeat, cutval, bbox);
1080 
1081  node->sub.divfeat = cutfeat;
1082 
1083  BoundingBox left_bbox(bbox);
1084  left_bbox[cutfeat].high = cutval;
1085  node->child1 = divideTree(left, left+idx, left_bbox);
1086 
1087  BoundingBox right_bbox(bbox);
1088  right_bbox[cutfeat].low = cutval;
1089  node->child2 = divideTree(left+idx, right, right_bbox);
1090 
1091  node->sub.divlow = left_bbox[cutfeat].high;
1092  node->sub.divhigh = right_bbox[cutfeat].low;
1093 
1094  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1095  bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
1096  bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
1097  }
1098  }
1099 
1100  return node;
1101  }
1102 
1103  void computeMinMax(IndexType* ind, IndexType count, int element, ElementType& min_elem, ElementType& max_elem)
1104  {
1105  min_elem = dataset_get(ind[0],element);
1106  max_elem = dataset_get(ind[0],element);
1107  for (IndexType i=1; i<count; ++i) {
1108  ElementType val = dataset_get(ind[i],element);
1109  if (val<min_elem) min_elem = val;
1110  if (val>max_elem) max_elem = val;
1111  }
1112  }
1113 
1114  void middleSplit(IndexType* ind, IndexType count, IndexType& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
1115  {
1116  // find the largest span from the approximate bounding box
1117  ElementType max_span = bbox[0].high-bbox[0].low;
1118  cutfeat = 0;
1119  cutval = (bbox[0].high+bbox[0].low)/2;
1120  for (int i=1; i<(DIM>0 ? DIM : dim); ++i) {
1121  ElementType span = bbox[i].low-bbox[i].low;
1122  if (span>max_span) {
1123  max_span = span;
1124  cutfeat = i;
1125  cutval = (bbox[i].high+bbox[i].low)/2;
1126  }
1127  }
1128 
1129  // compute exact span on the found dimension
1130  ElementType min_elem, max_elem;
1131  computeMinMax(ind, count, cutfeat, min_elem, max_elem);
1132  cutval = (min_elem+max_elem)/2;
1133  max_span = max_elem - min_elem;
1134 
1135  // check if a dimension of a largest span exists
1136  size_t k = cutfeat;
1137  for (size_t i=0; i<(DIM>0 ? DIM : dim); ++i) {
1138  if (i==k) continue;
1139  ElementType span = bbox[i].high-bbox[i].low;
1140  if (span>max_span) {
1141  computeMinMax(ind, count, i, min_elem, max_elem);
1142  span = max_elem - min_elem;
1143  if (span>max_span) {
1144  max_span = span;
1145  cutfeat = i;
1146  cutval = (min_elem+max_elem)/2;
1147  }
1148  }
1149  }
1150  IndexType lim1, lim2;
1151  planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
1152 
1153  if (lim1>count/2) index = lim1;
1154  else if (lim2<count/2) index = lim2;
1155  else index = count/2;
1156  }
1157 
1158 
1159  void middleSplit_(IndexType* ind, IndexType count, IndexType& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
1160  {
1161  const DistanceType EPS=static_cast<DistanceType>(0.00001);
1162  ElementType max_span = bbox[0].high-bbox[0].low;
1163  for (int i=1; i<(DIM>0 ? DIM : dim); ++i) {
1164  ElementType span = bbox[i].high-bbox[i].low;
1165  if (span>max_span) {
1166  max_span = span;
1167  }
1168  }
1169  ElementType max_spread = -1;
1170  cutfeat = 0;
1171  for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1172  ElementType span = bbox[i].high-bbox[i].low;
1173  if (span>(1-EPS)*max_span) {
1174  ElementType min_elem, max_elem;
1175  computeMinMax(ind, count, cutfeat, min_elem, max_elem);
1176  ElementType spread = max_elem-min_elem;;
1177  if (spread>max_spread) {
1178  cutfeat = i;
1179  max_spread = spread;
1180  }
1181  }
1182  }
1183  // split in the middle
1184  DistanceType split_val = (bbox[cutfeat].low+bbox[cutfeat].high)/2;
1185  ElementType min_elem, max_elem;
1186  computeMinMax(ind, count, cutfeat, min_elem, max_elem);
1187 
1188  if (split_val<min_elem) cutval = min_elem;
1189  else if (split_val>max_elem) cutval = max_elem;
1190  else cutval = split_val;
1191 
1192  IndexType lim1, lim2;
1193  planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
1194 
1195  if (lim1>count/2) index = lim1;
1196  else if (lim2<count/2) index = lim2;
1197  else index = count/2;
1198  }
1199 
1200 
1201  /**
1202  * Subdivide the list of points by a plane perpendicular on axe corresponding
1203  * to the 'cutfeat' dimension at 'cutval' position.
1204  *
1205  * On return:
1206  * dataset[ind[0..lim1-1]][cutfeat]<cutval
1207  * dataset[ind[lim1..lim2-1]][cutfeat]==cutval
1208  * dataset[ind[lim2..count]][cutfeat]>cutval
1209  */
1210  void planeSplit(IndexType* ind, const IndexType count, int cutfeat, DistanceType cutval, IndexType& lim1, IndexType& lim2)
1211  {
1212  /* Move vector indices for left subtree to front of list. */
1213  IndexType left = 0;
1214  IndexType right = count-1;
1215  for (;; ) {
1216  while (left<=right && dataset_get(ind[left],cutfeat)<cutval) ++left;
1217  while (right && left<=right && dataset_get(ind[right],cutfeat)>=cutval) --right;
1218  if (left>right || !right) break; // "!right" was added to support unsigned Index types
1219  std::swap(ind[left], ind[right]);
1220  ++left;
1221  --right;
1222  }
1223  /* If either list is empty, it means that all remaining features
1224  * are identical. Split in the middle to maintain a balanced tree.
1225  */
1226  lim1 = left;
1227  right = count-1;
1228  for (;; ) {
1229  while (left<=right && dataset_get(ind[left],cutfeat)<=cutval) ++left;
1230  while (right && left<=right && dataset_get(ind[right],cutfeat)>cutval) --right;
1231  if (left>right || !right) break; // "!right" was added to support unsigned Index types
1232  std::swap(ind[left], ind[right]);
1233  ++left;
1234  --right;
1235  }
1236  lim2 = left;
1237  }
1238 
1240  {
1241  assert(vec);
1242  DistanceType distsq = 0.0;
1243 
1244  for (int i = 0; i < (DIM>0 ? DIM : dim); ++i) {
1245  if (vec[i] < root_bbox[i].low) {
1246  dists[i] = distance.accum_dist(vec[i], root_bbox[i].low, i);
1247  distsq += dists[i];
1248  }
1249  if (vec[i] > root_bbox[i].high) {
1250  dists[i] = distance.accum_dist(vec[i], root_bbox[i].high, i);
1251  distsq += dists[i];
1252  }
1253  }
1254 
1255  return distsq;
1256  }
1257 
1258  /**
1259  * Performs an exact search in the tree starting from a node.
1260  * \tparam RESULTSET Should be any ResultSet<DistanceType>
1261  */
1262  template <class RESULTSET>
1263  void searchLevel(RESULTSET& result_set, const ElementType* vec, const NodePtr node, DistanceType mindistsq,
1264  distance_vector_t& dists, const float epsError) const
1265  {
1266  /* If this is a leaf node, then do check and return. */
1267  if ((node->child1 == NULL)&&(node->child2 == NULL)) {
1268  //count_leaf += (node->lr.right-node->lr.left); // Removed since was neither used nor returned to the user.
1269  DistanceType worst_dist = result_set.worstDist();
1270  for (IndexType i=node->lr.left; i<node->lr.right; ++i) {
1271  const IndexType index = vind[i];// reorder... : i;
1272  DistanceType dist = distance(vec, index, (DIM>0 ? DIM : dim));
1273  if (dist<worst_dist) {
1274  result_set.addPoint(dist,vind[i]);
1275  }
1276  }
1277  return;
1278  }
1279 
1280  /* Which child branch should be taken first? */
1281  int idx = node->sub.divfeat;
1282  ElementType val = vec[idx];
1283  DistanceType diff1 = val - node->sub.divlow;
1284  DistanceType diff2 = val - node->sub.divhigh;
1285 
1286  NodePtr bestChild;
1287  NodePtr otherChild;
1288  DistanceType cut_dist;
1289  if ((diff1+diff2)<0) {
1290  bestChild = node->child1;
1291  otherChild = node->child2;
1292  cut_dist = distance.accum_dist(val, node->sub.divhigh, idx);
1293  }
1294  else {
1295  bestChild = node->child2;
1296  otherChild = node->child1;
1297  cut_dist = distance.accum_dist( val, node->sub.divlow, idx);
1298  }
1299 
1300  /* Call recursively to search next level down. */
1301  searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError);
1302 
1303  DistanceType dst = dists[idx];
1304  mindistsq = mindistsq + cut_dist - dst;
1305  dists[idx] = cut_dist;
1306  if (mindistsq*epsError<=result_set.worstDist()) {
1307  searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError);
1308  }
1309  dists[idx] = dst;
1310  }
1311 
1312  public:
1313  /** Stores the index in a binary file.
1314  * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when loading the index object it must be constructed associated to the same source of data points used while building it.
1315  * See the example: examples/saveload_example.cpp
1316  * \sa loadIndex */
1317  void saveIndex(FILE* stream)
1318  {
1319  save_value(stream, m_size);
1320  save_value(stream, dim);
1321  save_value(stream, root_bbox);
1322  save_value(stream, m_leaf_max_size);
1323  save_value(stream, vind);
1324  save_tree(stream, root_node);
1325  }
1326 
1327  /** Loads a previous index from a binary file.
1328  * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the index object must be constructed associated to the same source of data points used while building the index.
1329  * See the example: examples/saveload_example.cpp
1330  * \sa loadIndex */
1331  void loadIndex(FILE* stream)
1332  {
1333  load_value(stream, m_size);
1334  load_value(stream, dim);
1335  load_value(stream, root_bbox);
1336  load_value(stream, m_leaf_max_size);
1337  load_value(stream, vind);
1338  load_tree(stream, root_node);
1339  }
1340 
1341  }; // class KDTree
1342 
1343 
1344  /** A simple KD-tree adaptor for working with data directly stored in an Eigen Matrix, without duplicating the data storage.
1345  * Each row in the matrix represents a point in the state space.
1346  *
1347  * Example of usage:
1348  * \code
1349  * Eigen::Matrix<num_t,Dynamic,Dynamic> mat;
1350  * // Fill out "mat"...
1351  *
1352  * typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix<num_t,Dynamic,Dynamic> > my_kd_tree_t;
1353  * const int max_leaf = 10;
1354  * my_kd_tree_t mat_index(dimdim, mat, max_leaf );
1355  * mat_index.index->buildIndex();
1356  * mat_index.index->...
1357  * \endcode
1358  *
1359  * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations.
1360  * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
1361  * \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int)
1362  */
1363  template <class MatrixType, int DIM = -1, class Distance = nanoflann::metric_L2, typename IndexType = size_t>
1365  {
1367  typedef typename MatrixType::Scalar num_t;
1368  typedef typename Distance::template traits<num_t,self_t>::distance_t metric_t;
1370 
1371  index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index.
1372 
1373  /// Constructor: takes a const ref to the matrix object with the data points
1374  KDTreeEigenMatrixAdaptor(const int dimensionality, const MatrixType &mat, const int leaf_max_size = 10) : m_data_matrix(mat)
1375  {
1376  const size_t dims = mat.cols();
1377  if (DIM>0 && static_cast<int>(dims)!=DIM)
1378  throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
1379  index = new index_t( dims, *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size, dims ) );
1380  index->buildIndex();
1381  }
1382 
1384  delete index;
1385  }
1386 
1387  const MatrixType &m_data_matrix;
1388 
1389  /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]).
1390  * Note that this is a short-cut method for index->findNeighbors().
1391  * The user can also call index->... methods as desired.
1392  * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
1393  */
1394  inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const
1395  {
1397  resultSet.init(out_indices, out_distances_sq);
1398  index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
1399  }
1400 
1401  /** @name Interface expected by KDTreeSingleIndexAdaptor
1402  * @{ */
1403 
1404  const self_t & derived() const {
1405  return *this;
1406  }
1408  return *this;
1409  }
1410 
1411  // Must return the number of data points
1412  inline size_t kdtree_get_point_count() const {
1413  return m_data_matrix.rows();
1414  }
1415 
1416  // Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
1417  inline num_t kdtree_distance(const num_t *p1, const size_t idx_p2,size_t size) const
1418  {
1419  num_t s=0;
1420  for (size_t i=0; i<size; i++) {
1421  const num_t d= p1[i]-m_data_matrix.coeff(idx_p2,i);
1422  s+=d*d;
1423  }
1424  return s;
1425  }
1426 
1427  // Returns the dim'th component of the idx'th point in the class:
1428  inline num_t kdtree_get_pt(const size_t idx, int dim) const {
1429  return m_data_matrix.coeff(idx,dim);
1430  }
1431 
1432  // Optional bounding-box computation: return false to default to a standard bbox computation loop.
1433  // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
1434  // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
1435  template <class BBOX>
1436  bool kdtree_get_bbox(BBOX &bb) const {
1437  return false;
1438  }
1439 
1440  /** @} */
1441 
1442  }; // end of KDTreeEigenMatrixAdaptor
1443  /** @} */
1444 
1445 /** @} */ // end of grouping
1446 } // end of NS
1447 
1448 
1449 #endif /* NANOFLANN_HPP_ */
A simple KD-tree adaptor for working with data directly stored in an Eigen Matrix, without duplicating the data storage.
Definition: nanoflann.hpp:1364
std::pair< IndexType, DistanceType > worst_item() const
Find the worst result (furtherest neighbor) without copying or sorting Pre-conditions: size() > 0...
Definition: nanoflann.hpp:169
long double abs< long double >(long double x)
Definition: nanoflann.hpp:241
DistanceType accum_dist(const U a, const V b, int) const
Definition: nanoflann.hpp:334
std::reverse_iterator< iterator > reverse_iterator
Definition: nanoflann.hpp:625
size_t size() const
Returns size of index.
Definition: nanoflann.hpp:892
PooledAllocator(const size_t blocksize_=BLOCKSIZE)
Default constructor.
Definition: nanoflann.hpp:485
void knnSearch(const ElementType *query_point, const size_t num_closest, IndexType *out_indices, DistanceType *out_distances_sq, const int nChecks_IGNORED=10) const
Find the "num_closest" nearest neighbors to the query_point[0:dim-1].
Definition: nanoflann.hpp:947
struct nanoflann::KDTreeSingleIndexAdaptor::Node::@6::@8 lr
KDTreeEigenMatrixAdaptor(const int dimensionality, const MatrixType &mat, const int leaf_max_size=10)
The kd-tree index for the user to call its methods as usual with any other FLANN index.
Definition: nanoflann.hpp:1374
void freeIndex()
Frees the previously-built index.
Definition: nanoflann.hpp:872
EIGEN_STRONG_INLINE iterator end()
Definition: eigen_plugins.h:53
void swap(CArray< T, N > &y)
Definition: nanoflann.hpp:662
const DataSource & data_source
Definition: nanoflann.hpp:254
const T * data() const
Definition: nanoflann.hpp:664
Metaprogramming helper traits class for the L2 (Euclidean) metric.
Definition: nanoflann.hpp:374
std::vector< IndexType > vind
Array of indices to vectors in the dataset.
Definition: nanoflann.hpp:743
Manhattan distance functor (generic version, optimized for high-dimensionality data sets)...
Definition: nanoflann.hpp:249
iterator begin()
Definition: nanoflann.hpp:618
void loadIndex(FILE *stream)
Loads a previous index from a binary file.
Definition: nanoflann.hpp:1331
struct nanoflann::KDTreeSingleIndexAdaptor::Node::@6::@9 sub
Squared Euclidean distance functor (suitable for low-dimensionality datasets, like 2D or 3D point clo...
Definition: nanoflann.hpp:346
BranchStruct(const T &aNode, DistanceType dist)
Definition: nanoflann.hpp:812
const DistanceType radius
Definition: nanoflann.hpp:132
size_t usedMemory() const
Computes the inde memory usage Returns: memory used by the index.
Definition: nanoflann.hpp:909
~PooledAllocator()
Destructor.
Definition: nanoflann.hpp:492
double abs< double >(double x)
Definition: nanoflann.hpp:240
std::ptrdiff_t difference_type
Definition: nanoflann.hpp:615
RadiusResultSet(DistanceType radius_, std::vector< std::pair< IndexType, DistanceType > > &indices_dists)
Definition: nanoflann.hpp:136
static size_type size()
Definition: nanoflann.hpp:655
~KDTreeSingleIndexAdaptor()
Standard destructor.
Definition: nanoflann.hpp:867
const KDTreeSingleIndexAdaptorParams index_params
Definition: nanoflann.hpp:753
static void rangecheck(size_type i)
Definition: nanoflann.hpp:678
reverse_iterator rbegin()
Definition: nanoflann.hpp:639
Scalar * iterator
Definition: eigen_plugins.h:49
void init(IndexType *indices_, DistanceType *dists_)
Definition: nanoflann.hpp:76
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:52
const_reference at(size_type i) const
Definition: nanoflann.hpp:648
DistanceType * dists
Definition: nanoflann.hpp:67
DistanceType accum_dist(const U a, const V b, int) const
Definition: nanoflann.hpp:285
DistanceType operator()(const T *a, const size_t b_idx, size_t size) const
Definition: nanoflann.hpp:355
iterator end()
Definition: nanoflann.hpp:620
num_t kdtree_get_pt(const size_t idx, int dim) const
Definition: nanoflann.hpp:1428
const Scalar * const_iterator
Definition: eigen_plugins.h:50
L2_Simple_Adaptor< T, DataSource > distance_t
Definition: nanoflann.hpp:384
float eps
search for eps-approximate neighbours (default: 0)
Definition: nanoflann.hpp:415
const_reference front() const
Definition: nanoflann.hpp:651
void resize(const size_t nElements)
This method has no effects in this class, but raises an exception if the expected size does not match...
Definition: nanoflann.hpp:660
L2_Adaptor< T, DataSource > distance_t
Definition: nanoflann.hpp:377
DistanceType computeInitialDistances(const ElementType *vec, distance_vector_t &dists) const
Definition: nanoflann.hpp:1239
DistanceType operator()(const T *a, const size_t b_idx, size_t size, DistanceType worst_dist=-1) const
Definition: nanoflann.hpp:258
L1_Adaptor< T, DataSource > distance_t
Definition: nanoflann.hpp:370
void load_value(FILE *stream, T &value, size_t count=1)
Definition: nanoflann.hpp:208
Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors when DIM=-1.
Definition: nanoflann.hpp:685
const DataSource & data_source
Definition: nanoflann.hpp:302
bool operator()(const PairType &p1, const PairType &p2) const
PairType will be typically: std::pair<IndexType,DistanceType>
Definition: nanoflann.hpp:183
reference back()
Definition: nanoflann.hpp:652
void searchLevel(RESULTSET &result_set, const ElementType *vec, const NodePtr node, DistanceType mindistsq, distance_vector_t &dists, const float epsError) const
Performs an exact search in the tree starting from a node.
Definition: nanoflann.hpp:1263
reverse_iterator rend()
Definition: nanoflann.hpp:641
void init_vind()
Make sure the auxiliary list vind has the same size than the current dataset, and re-generate if size...
Definition: nanoflann.hpp:981
DistanceType accum_dist(const U a, const V b, int) const
Definition: nanoflann.hpp:360
bool kdtree_get_bbox(BBOX &bb) const
Definition: nanoflann.hpp:1436
L2_Adaptor(const DataSource &_data_source)
Definition: nanoflann.hpp:304
void addPoint(DistanceType dist, IndexType index)
Definition: nanoflann.hpp:150
const size_t BLOCKSIZE
Definition: nanoflann.hpp:455
array_or_vector_selector< DIM, DistanceType >::container_t distance_vector_t
Define "distance_vector_t" as a fixed-size or variable-size container depending on "DIM"...
Definition: nanoflann.hpp:799
static size_type max_size()
Definition: nanoflann.hpp:657
bool sorted
only for radius search, require neighbours sorted by distance (default: true)
Definition: nanoflann.hpp:416
const DatasetAdaptor & dataset
The dataset used by this index.
Definition: nanoflann.hpp:751
operator "<" for std::sort()
Definition: nanoflann.hpp:179
void middleSplit(IndexType *ind, IndexType count, IndexType &index, int &cutfeat, DistanceType &cutval, const BoundingBox &bbox)
Definition: nanoflann.hpp:1114
reference operator[](size_type i)
Definition: nanoflann.hpp:644
Distance::DistanceType DistanceType
Definition: nanoflann.hpp:737
void middleSplit_(IndexType *ind, IndexType count, IndexType &index, int &cutfeat, DistanceType &cutval, const BoundingBox &bbox)
Definition: nanoflann.hpp:1159
const T * const_iterator
Definition: nanoflann.hpp:611
CountType size() const
Definition: nanoflann.hpp:84
KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size=10, int dim_=-1)
Definition: nanoflann.hpp:399
void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED=10) const
Query for the num_closest closest points to a given point (entered as query_point[0:dim-1]).
Definition: nanoflann.hpp:1394
DistanceType operator()(const T *a, const size_t b_idx, size_t size, DistanceType worst_dist=-1) const
Definition: nanoflann.hpp:306
std::vector< std::pair< IndexType, DistanceType > > & m_indices_dists
Definition: nanoflann.hpp:134
T abs(T x)
Definition: nanoflann.hpp:237
T * allocate(size_t count=1)
Allocates (using C&#39;s malloc) a generic type T.
Definition: nanoflann.hpp:432
Distance::ElementType ElementType
Definition: nanoflann.hpp:736
reference at(size_type i)
Definition: nanoflann.hpp:647
std::size_t size_type
Definition: nanoflann.hpp:614
void findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const
Find set of nearest neighbors to vec[0:dim-1].
Definition: nanoflann.hpp:929
KNNResultSet(CountType capacity_)
Definition: nanoflann.hpp:72
void saveIndex(FILE *stream)
Stores the index in a binary file.
Definition: nanoflann.hpp:1317
A result-set class used when performing a radius based search.
Definition: nanoflann.hpp:129
ElementType dataset_get(size_t idx, int component) const
Helper accessor to the dataset points:
Definition: nanoflann.hpp:990
DistanceType worstDist() const
Definition: nanoflann.hpp:156
KDTreeSingleIndexAdaptor(const int dimensionality, const DatasetAdaptor &inputData, const KDTreeSingleIndexAdaptorParams &params=KDTreeSingleIndexAdaptorParams())
KDTree constructor.
Definition: nanoflann.hpp:849
const_reverse_iterator rend() const
Definition: nanoflann.hpp:642
T * allocate(const size_t count=1)
Allocates (using this pool) a generic type T.
Definition: nanoflann.hpp:564
A STL container (as wrapper) for arrays of constant size defined at compile time (class imported from...
Definition: nanoflann.hpp:603
int checks
Ignored parameter (Kept for compatibility with the FLANN interface).
Definition: nanoflann.hpp:414
L1_Adaptor(const DataSource &_data_source)
Definition: nanoflann.hpp:256
void addPoint(DistanceType dist, IndexType index)
Definition: nanoflann.hpp:95
const_reference back() const
Definition: nanoflann.hpp:653
void free_all()
Frees all allocated memory chunks.
Definition: nanoflann.hpp:497
KDTreeEigenMatrixAdaptor< MatrixType, DIM, Distance, IndexType > self_t
Definition: nanoflann.hpp:1366
void planeSplit(IndexType *ind, const IndexType count, int cutfeat, DistanceType cutval, IndexType &lim1, IndexType &lim2)
Subdivide the list of points by a plane perpendicular on axe corresponding to the &#39;cutfeat&#39; dimension...
Definition: nanoflann.hpp:1210
size_t veclen() const
Returns the length of an index feature.
Definition: nanoflann.hpp:900
This record represents a branch point when finding neighbors in the tree.
Definition: nanoflann.hpp:806
_DistanceType DistanceType
Definition: nanoflann.hpp:252
const_reference operator[](size_type i) const
Definition: nanoflann.hpp:645
array_or_vector_selector< DIM, Interval >::container_t BoundingBox
Define "BoundingBox" as a fixed-size or variable-size container depending on "DIM".
Definition: nanoflann.hpp:796
Squared Euclidean distance functor (generic version, optimized for high-dimensionality data sets)...
Definition: nanoflann.hpp:297
DistanceType divlow
The values used for subdivision.
Definition: nanoflann.hpp:779
NodePtr root_node
Array of k-d trees used to find neighbours.
Definition: nanoflann.hpp:823
const DataSource & data_source
Definition: nanoflann.hpp:351
void assign(const T &value)
Definition: nanoflann.hpp:673
Parameters (see http://code.google.com/p/nanoflann/ for help choosing the parameters) ...
Definition: nanoflann.hpp:397
const self_t & derived() const
Definition: nanoflann.hpp:1404
DistanceType worstDist() const
Definition: nanoflann.hpp:118
static bool empty()
Definition: nanoflann.hpp:656
void computeMinMax(IndexType *ind, IndexType count, int element, ElementType &min_elem, ElementType &max_elem)
Definition: nanoflann.hpp:1103
NodePtr divideTree(const IndexType left, const IndexType right, BoundingBox &bbox)
Create a tree node that subdivides the list of vecs from vind[first] to vind[last].
Definition: nanoflann.hpp:1053
L2_Simple_Adaptor(const DataSource &_data_source)
Definition: nanoflann.hpp:353
void assign(const size_t n, const T &value)
Definition: nanoflann.hpp:675
Distance::template traits< num_t, self_t >::distance_t metric_t
Definition: nanoflann.hpp:1368
void save_tree(FILE *stream, NodePtr tree)
Definition: nanoflann.hpp:995
int dim
Dimensionality of each data point.
Definition: nanoflann.hpp:756
BranchStruct< NodePtr, DistanceType > BranchSt
Definition: nanoflann.hpp:824
Search options for KDTreeSingleIndexAdaptor::findNeighbors()
Definition: nanoflann.hpp:408
std::reverse_iterator< const_iterator > const_reverse_iterator
Definition: nanoflann.hpp:626
const size_t WORDSIZE
Pooled storage allocator.
Definition: nanoflann.hpp:454
int abs< int >(int x)
Definition: nanoflann.hpp:238
void computeBoundingBox(BoundingBox &bbox)
Definition: nanoflann.hpp:1020
void save_value(FILE *stream, const T &value, size_t count=1)
Definition: nanoflann.hpp:194
void load_tree(FILE *stream, NodePtr &tree)
Definition: nanoflann.hpp:1007
PooledAllocator pool
Pooled memory allocator.
Definition: nanoflann.hpp:836
const_iterator begin() const
Definition: nanoflann.hpp:619
int divfeat
Dimension used for subdivision.
Definition: nanoflann.hpp:775
Metaprogramming helper traits class for the L1 (Manhattan) metric.
Definition: nanoflann.hpp:367
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:966
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
const_reverse_iterator rbegin() const
Definition: nanoflann.hpp:640
void * malloc(const size_t req_size)
Returns a pointer to a piece of new memory of the given size in bytes allocated from the pool...
Definition: nanoflann.hpp:511
SearchParams(int checks_IGNORED_=32, float eps_=0, bool sorted_=true)
Note: The first argument (checks_IGNORED_) is ignored, but kept for compatibility with the FLANN inte...
Definition: nanoflann.hpp:411
Metaprogramming helper traits class for the L2_simple (Euclidean) metric.
Definition: nanoflann.hpp:381
float abs< float >(float x)
Definition: nanoflann.hpp:239
_DistanceType DistanceType
Definition: nanoflann.hpp:300
void set_radius_and_clear(const DistanceType r)
Clears the result set and adjusts the search radius.
Definition: nanoflann.hpp:159
num_t kdtree_distance(const num_t *p1, const size_t idx_p2, size_t size) const
Definition: nanoflann.hpp:1417
KDTreeSingleIndexAdaptor< metric_t, self_t, DIM, IndexType > index_t
Definition: nanoflann.hpp:1369
const_iterator end() const
Definition: nanoflann.hpp:621
reference front()
Definition: nanoflann.hpp:650
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
void buildIndex()
Builds the index.
Definition: nanoflann.hpp:881
const T & const_reference
Definition: nanoflann.hpp:613



Page generated by Doxygen 1.8.14 for MRPT 1.0.2 SVN: at lun oct 28 00:52:41 CET 2019 Hosted on:
SourceForge.net Logo