Main MRPT website > C++ reference for MRPT 1.4.0
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-2014 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
53namespace nanoflann
54{
55/** @addtogroup nanoflann_grp nanoflann C++ library for ANN
56 * @{ */
57
58 /** Library version: 0xMmP (M=Major,m=minor,P=patch) */
59 #define NANOFLANN_VERSION 0x119
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_) : indices(0), dists(0), 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::make_pair(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>
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>
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 (L2) 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:
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 * // [Only if using the metric_L2_Simple type] 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 DatasetAdaptor The user-provided adaptor (see comments above).
731 * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
732 * \tparam IndexType Will be typically size_t or int
733 */
734 template <typename Distance, class DatasetAdaptor,int DIM = -1, typename IndexType = size_t>
736 {
737 private:
738 /** Hidden copy constructor, to disallow copying indices (Not implemented) */
740 public:
741 typedef typename Distance::ElementType ElementType;
742 typedef typename Distance::DistanceType DistanceType;
743 protected:
744
745 /**
746 * Array of indices to vectors in the dataset.
747 */
748 std::vector<IndexType> vind;
749
751
752
753 /**
754 * The dataset used by this index
755 */
756 const DatasetAdaptor &dataset; //!< The source of our data
757
759
760 size_t m_size;
761 int dim; //!< Dimensionality of each data point
762
763
764 /*--------------------- Internal Data Structures --------------------------*/
765 struct Node
766 {
767 union {
768 struct
769 {
770 /**
771 * Indices of points in leaf node
772 */
773 IndexType left, right;
774 } lr;
775 struct
776 {
777 /**
778 * Dimension used for subdivision.
779 */
781 /**
782 * The values used for subdivision.
783 */
785 } sub;
786 };
787 /**
788 * The child nodes.
789 */
790 Node* child1, * child2;
791 };
792 typedef Node* NodePtr;
793
794
795 struct Interval
796 {
798 };
799
800 /** Define "BoundingBox" as a fixed-size or variable-size container depending on "DIM" */
802
803 /** Define "distance_vector_t" as a fixed-size or variable-size container depending on "DIM" */
805
806 /** This record represents a branch point when finding neighbors in
807 the tree. It contains a record of the minimum distance to the query
808 point, as well as the node at which the search resumes.
809 */
810 template <typename T, typename DistanceType>
812 {
813 T node; /* Tree node at which search resumes */
814 DistanceType mindist; /* Minimum distance to query for all nodes below. */
815
817 BranchStruct(const T& aNode, DistanceType dist) : node(aNode), mindist(dist) {}
818
819 inline bool operator<(const BranchStruct<T, DistanceType>& rhs) const
820 {
821 return mindist<rhs.mindist;
822 }
823 };
824
825 /**
826 * Array of k-d trees used to find neighbours.
827 */
830 typedef BranchSt* Branch;
831
833
834 /**
835 * Pooled memory allocator.
836 *
837 * Using a pooled memory allocator is more efficient
838 * than allocating memory directly when there is a large
839 * number small of memory allocations.
840 */
842
843 public:
844
845 Distance distance;
846
847 /**
848 * KDTree constructor
849 *
850 * Params:
851 * inputData = dataset with the input features
852 * params = parameters passed to the kdtree algorithm (see http://code.google.com/p/nanoflann/ for help choosing the parameters)
853 */
854 KDTreeSingleIndexAdaptor(const int dimensionality, const DatasetAdaptor& inputData, const KDTreeSingleIndexAdaptorParams& params = KDTreeSingleIndexAdaptorParams() ) :
855 dataset(inputData), index_params(params), root_node(NULL), distance(inputData)
856 {
857 m_size = dataset.kdtree_get_point_count();
858 dim = dimensionality;
859 if (DIM>0) dim=DIM;
860 else {
861 if (params.dim>0) dim = params.dim;
862 }
863 m_leaf_max_size = params.leaf_max_size;
864
865 // Create a permutable array of indices to the input vectors.
866 init_vind();
867 }
868
869 /**
870 * Standard destructor
871 */
873 {
874 }
875
876 /** Frees the previously-built index. Automatically called within buildIndex(). */
878 {
879 pool.free_all();
880 root_node=NULL;
881 }
882
883 /**
884 * Builds the index
885 */
887 {
888 init_vind();
889 freeIndex();
890 if(m_size == 0) return;
891 computeBoundingBox(root_bbox);
892 root_node = divideTree(0, m_size, root_bbox ); // construct the tree
893 }
894
895 /**
896 * Returns size of index.
897 */
898 size_t size() const
899 {
900 return m_size;
901 }
902
903 /**
904 * Returns the length of an index feature.
905 */
906 size_t veclen() const
907 {
908 return static_cast<size_t>(DIM>0 ? DIM : dim);
909 }
910
911 /**
912 * Computes the inde memory usage
913 * Returns: memory used by the index
914 */
915 size_t usedMemory() const
916 {
917 return pool.usedMemory+pool.wastedMemory+dataset.kdtree_get_point_count()*sizeof(IndexType); // pool memory and vind array memory
918 }
919
920 /** \name Query methods
921 * @{ */
922
923 /**
924 * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored inside
925 * the result object.
926 *
927 * Params:
928 * result = the result object in which the indices of the nearest-neighbors are stored
929 * vec = the vector for which to search the nearest neighbors
930 *
931 * \tparam RESULTSET Should be any ResultSet<DistanceType>
932 * \sa knnSearch, radiusSearch
933 */
934 template <typename RESULTSET>
935 void findNeighbors(RESULTSET& result, const ElementType* vec, const SearchParams& searchParams) const
936 {
937 assert(vec);
938 if (!root_node) throw std::runtime_error("[nanoflann] findNeighbors() called before building the index or no data points.");
939 float epsError = 1+searchParams.eps;
940
941 distance_vector_t dists; // fixed or variable-sized container (depending on DIM)
942 dists.assign((DIM>0 ? DIM : dim) ,0); // Fill it with zeros.
943 DistanceType distsq = computeInitialDistances(vec, dists);
944 searchLevel(result, vec, root_node, distsq, dists, epsError); // "count_leaf" parameter removed since was neither used nor returned to the user.
945 }
946
947 /**
948 * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. Their indices are stored inside
949 * the result object.
950 * \sa radiusSearch, findNeighbors
951 * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
952 */
953 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
954 {
956 resultSet.init(out_indices, out_distances_sq);
957 this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
958 }
959
960 /**
961 * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius.
962 * The output is given as a vector of pairs, of which the first element is a point index and the second the corresponding distance.
963 * Previous contents of \a IndicesDists are cleared.
964 *
965 * If searchParams.sorted==true, the output list is sorted by ascending distances.
966 *
967 * 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.
968 *
969 * \sa knnSearch, findNeighbors
970 * \return The number of points within the given radius (i.e. indices.size() or dists.size() )
971 */
972 size_t radiusSearch(const ElementType *query_point,const DistanceType radius, std::vector<std::pair<IndexType,DistanceType> >& IndicesDists, const SearchParams& searchParams) const
973 {
974 RadiusResultSet<DistanceType,IndexType> resultSet(radius,IndicesDists);
975 this->findNeighbors(resultSet, query_point, searchParams);
976
977 if (searchParams.sorted)
978 std::sort(IndicesDists.begin(),IndicesDists.end(), IndexDist_Sorter() );
979
980 return resultSet.size();
981 }
982
983 /** @} */
984
985 private:
986 /** Make sure the auxiliary list \a vind has the same size than the current dataset, and re-generate if size has changed. */
988 {
989 // Create a permutable array of indices to the input vectors.
990 m_size = dataset.kdtree_get_point_count();
991 if (vind.size()!=m_size) vind.resize(m_size);
992 for (size_t i = 0; i < m_size; i++) vind[i] = i;
993 }
994
995 /// Helper accessor to the dataset points:
996 inline ElementType dataset_get(size_t idx, int component) const {
997 return dataset.kdtree_get_pt(idx,component);
998 }
999
1000
1001 void save_tree(FILE* stream, NodePtr tree)
1002 {
1003 save_value(stream, *tree);
1004 if (tree->child1!=NULL) {
1005 save_tree(stream, tree->child1);
1006 }
1007 if (tree->child2!=NULL) {
1008 save_tree(stream, tree->child2);
1009 }
1010 }
1011
1012
1013 void load_tree(FILE* stream, NodePtr& tree)
1014 {
1015 tree = pool.allocate<Node>();
1016 load_value(stream, *tree);
1017 if (tree->child1!=NULL) {
1018 load_tree(stream, tree->child1);
1019 }
1020 if (tree->child2!=NULL) {
1021 load_tree(stream, tree->child2);
1022 }
1023 }
1024
1025
1027 {
1028 bbox.resize((DIM>0 ? DIM : dim));
1029 if (dataset.kdtree_get_bbox(bbox))
1030 {
1031 // Done! It was implemented in derived class
1032 }
1033 else
1034 {
1035 const size_t N = dataset.kdtree_get_point_count();
1036 if (!N) throw std::runtime_error("[nanoflann] computeBoundingBox() called but no data points found.");
1037 for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1038 bbox[i].low =
1039 bbox[i].high = dataset_get(0,i);
1040 }
1041 for (size_t k=1; k<N; ++k) {
1042 for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1043 if (dataset_get(k,i)<bbox[i].low) bbox[i].low = dataset_get(k,i);
1044 if (dataset_get(k,i)>bbox[i].high) bbox[i].high = dataset_get(k,i);
1045 }
1046 }
1047 }
1048 }
1049
1050
1051 /**
1052 * Create a tree node that subdivides the list of vecs from vind[first]
1053 * to vind[last]. The routine is called recursively on each sublist.
1054 * Place a pointer to this new tree node in the location pTree.
1055 *
1056 * Params: pTree = the new node to create
1057 * first = index of the first vector
1058 * last = index of the last vector
1059 */
1060 NodePtr divideTree(const IndexType left, const IndexType right, BoundingBox& bbox)
1061 {
1062 NodePtr node = pool.allocate<Node>(); // allocate memory
1063
1064 /* If too few exemplars remain, then make this a leaf node. */
1065 if ( (right-left) <= m_leaf_max_size) {
1066 node->child1 = node->child2 = NULL; /* Mark as leaf node. */
1067 node->lr.left = left;
1068 node->lr.right = right;
1069
1070 // compute bounding-box of leaf points
1071 for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1072 bbox[i].low = dataset_get(vind[left],i);
1073 bbox[i].high = dataset_get(vind[left],i);
1074 }
1075 for (IndexType k=left+1; k<right; ++k) {
1076 for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1077 if (bbox[i].low>dataset_get(vind[k],i)) bbox[i].low=dataset_get(vind[k],i);
1078 if (bbox[i].high<dataset_get(vind[k],i)) bbox[i].high=dataset_get(vind[k],i);
1079 }
1080 }
1081 }
1082 else {
1083 IndexType idx;
1084 int cutfeat;
1085 DistanceType cutval;
1086 middleSplit_(&vind[0]+left, right-left, idx, cutfeat, cutval, bbox);
1087
1088 node->sub.divfeat = cutfeat;
1089
1090 BoundingBox left_bbox(bbox);
1091 left_bbox[cutfeat].high = cutval;
1092 node->child1 = divideTree(left, left+idx, left_bbox);
1093
1094 BoundingBox right_bbox(bbox);
1095 right_bbox[cutfeat].low = cutval;
1096 node->child2 = divideTree(left+idx, right, right_bbox);
1097
1098 node->sub.divlow = left_bbox[cutfeat].high;
1099 node->sub.divhigh = right_bbox[cutfeat].low;
1100
1101 for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1102 bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
1103 bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
1104 }
1105 }
1106
1107 return node;
1108 }
1109
1110 void computeMinMax(IndexType* ind, IndexType count, int element, ElementType& min_elem, ElementType& max_elem)
1111 {
1112 min_elem = dataset_get(ind[0],element);
1113 max_elem = dataset_get(ind[0],element);
1114 for (IndexType i=1; i<count; ++i) {
1115 ElementType val = dataset_get(ind[i],element);
1116 if (val<min_elem) min_elem = val;
1117 if (val>max_elem) max_elem = val;
1118 }
1119 }
1120
1121 void middleSplit_(IndexType* ind, IndexType count, IndexType& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
1122 {
1123 const DistanceType EPS=static_cast<DistanceType>(0.00001);
1124 ElementType max_span = bbox[0].high-bbox[0].low;
1125 for (int i=1; i<(DIM>0 ? DIM : dim); ++i) {
1126 ElementType span = bbox[i].high-bbox[i].low;
1127 if (span>max_span) {
1128 max_span = span;
1129 }
1130 }
1131 ElementType max_spread = -1;
1132 cutfeat = 0;
1133 for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1134 ElementType span = bbox[i].high-bbox[i].low;
1135 if (span>(1-EPS)*max_span) {
1136 ElementType min_elem, max_elem;
1137 computeMinMax(ind, count, cutfeat, min_elem, max_elem);
1138 ElementType spread = max_elem-min_elem;;
1139 if (spread>max_spread) {
1140 cutfeat = i;
1141 max_spread = spread;
1142 }
1143 }
1144 }
1145 // split in the middle
1146 DistanceType split_val = (bbox[cutfeat].low+bbox[cutfeat].high)/2;
1147 ElementType min_elem, max_elem;
1148 computeMinMax(ind, count, cutfeat, min_elem, max_elem);
1149
1150 if (split_val<min_elem) cutval = min_elem;
1151 else if (split_val>max_elem) cutval = max_elem;
1152 else cutval = split_val;
1153
1154 IndexType lim1, lim2;
1155 planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
1156
1157 if (lim1>count/2) index = lim1;
1158 else if (lim2<count/2) index = lim2;
1159 else index = count/2;
1160 }
1161
1162
1163 /**
1164 * Subdivide the list of points by a plane perpendicular on axe corresponding
1165 * to the 'cutfeat' dimension at 'cutval' position.
1166 *
1167 * On return:
1168 * dataset[ind[0..lim1-1]][cutfeat]<cutval
1169 * dataset[ind[lim1..lim2-1]][cutfeat]==cutval
1170 * dataset[ind[lim2..count]][cutfeat]>cutval
1171 */
1172 void planeSplit(IndexType* ind, const IndexType count, int cutfeat, DistanceType cutval, IndexType& lim1, IndexType& lim2)
1173 {
1174 /* Move vector indices for left subtree to front of list. */
1175 IndexType left = 0;
1176 IndexType right = count-1;
1177 for (;; ) {
1178 while (left<=right && dataset_get(ind[left],cutfeat)<cutval) ++left;
1179 while (right && left<=right && dataset_get(ind[right],cutfeat)>=cutval) --right;
1180 if (left>right || !right) break; // "!right" was added to support unsigned Index types
1181 std::swap(ind[left], ind[right]);
1182 ++left;
1183 --right;
1184 }
1185 /* If either list is empty, it means that all remaining features
1186 * are identical. Split in the middle to maintain a balanced tree.
1187 */
1188 lim1 = left;
1189 right = count-1;
1190 for (;; ) {
1191 while (left<=right && dataset_get(ind[left],cutfeat)<=cutval) ++left;
1192 while (right && left<=right && dataset_get(ind[right],cutfeat)>cutval) --right;
1193 if (left>right || !right) break; // "!right" was added to support unsigned Index types
1194 std::swap(ind[left], ind[right]);
1195 ++left;
1196 --right;
1197 }
1198 lim2 = left;
1199 }
1200
1202 {
1203 assert(vec);
1204 DistanceType distsq = 0.0;
1205
1206 for (int i = 0; i < (DIM>0 ? DIM : dim); ++i) {
1207 if (vec[i] < root_bbox[i].low) {
1208 dists[i] = distance.accum_dist(vec[i], root_bbox[i].low, i);
1209 distsq += dists[i];
1210 }
1211 if (vec[i] > root_bbox[i].high) {
1212 dists[i] = distance.accum_dist(vec[i], root_bbox[i].high, i);
1213 distsq += dists[i];
1214 }
1215 }
1216
1217 return distsq;
1218 }
1219
1220 /**
1221 * Performs an exact search in the tree starting from a node.
1222 * \tparam RESULTSET Should be any ResultSet<DistanceType>
1223 */
1224 template <class RESULTSET>
1225 void searchLevel(RESULTSET& result_set, const ElementType* vec, const NodePtr node, DistanceType mindistsq,
1226 distance_vector_t& dists, const float epsError) const
1227 {
1228 /* If this is a leaf node, then do check and return. */
1229 if ((node->child1 == NULL)&&(node->child2 == NULL)) {
1230 //count_leaf += (node->lr.right-node->lr.left); // Removed since was neither used nor returned to the user.
1231 DistanceType worst_dist = result_set.worstDist();
1232 for (IndexType i=node->lr.left; i<node->lr.right; ++i) {
1233 const IndexType index = vind[i];// reorder... : i;
1234 DistanceType dist = distance(vec, index, (DIM>0 ? DIM : dim));
1235 if (dist<worst_dist) {
1236 result_set.addPoint(dist,vind[i]);
1237 }
1238 }
1239 return;
1240 }
1241
1242 /* Which child branch should be taken first? */
1243 int idx = node->sub.divfeat;
1244 ElementType val = vec[idx];
1245 DistanceType diff1 = val - node->sub.divlow;
1246 DistanceType diff2 = val - node->sub.divhigh;
1247
1248 NodePtr bestChild;
1249 NodePtr otherChild;
1250 DistanceType cut_dist;
1251 if ((diff1+diff2)<0) {
1252 bestChild = node->child1;
1253 otherChild = node->child2;
1254 cut_dist = distance.accum_dist(val, node->sub.divhigh, idx);
1255 }
1256 else {
1257 bestChild = node->child2;
1258 otherChild = node->child1;
1259 cut_dist = distance.accum_dist( val, node->sub.divlow, idx);
1260 }
1261
1262 /* Call recursively to search next level down. */
1263 searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError);
1264
1265 DistanceType dst = dists[idx];
1266 mindistsq = mindistsq + cut_dist - dst;
1267 dists[idx] = cut_dist;
1268 if (mindistsq*epsError<=result_set.worstDist()) {
1269 searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError);
1270 }
1271 dists[idx] = dst;
1272 }
1273
1274 public:
1275 /** Stores the index in a binary file.
1276 * 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.
1277 * See the example: examples/saveload_example.cpp
1278 * \sa loadIndex */
1279 void saveIndex(FILE* stream)
1280 {
1281 save_value(stream, m_size);
1282 save_value(stream, dim);
1283 save_value(stream, root_bbox);
1284 save_value(stream, m_leaf_max_size);
1285 save_value(stream, vind);
1286 save_tree(stream, root_node);
1287 }
1288
1289 /** Loads a previous index from a binary file.
1290 * 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.
1291 * See the example: examples/saveload_example.cpp
1292 * \sa loadIndex */
1293 void loadIndex(FILE* stream)
1294 {
1295 load_value(stream, m_size);
1296 load_value(stream, dim);
1297 load_value(stream, root_bbox);
1298 load_value(stream, m_leaf_max_size);
1299 load_value(stream, vind);
1300 load_tree(stream, root_node);
1301 }
1302
1303 }; // class KDTree
1304
1305
1306 /** An L2-metric KD-tree adaptor for working with data directly stored in an Eigen Matrix, without duplicating the data storage.
1307 * Each row in the matrix represents a point in the state space.
1308 *
1309 * Example of usage:
1310 * \code
1311 * Eigen::Matrix<num_t,Dynamic,Dynamic> mat;
1312 * // Fill out "mat"...
1313 *
1314 * typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix<num_t,Dynamic,Dynamic> > my_kd_tree_t;
1315 * const int max_leaf = 10;
1316 * my_kd_tree_t mat_index(dimdim, mat, max_leaf );
1317 * mat_index.index->buildIndex();
1318 * mat_index.index->...
1319 * \endcode
1320 *
1321 * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations.
1322 * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
1323 * \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int)
1324 */
1325 template <class MatrixType, int DIM = -1, class Distance = nanoflann::metric_L2, typename IndexType = size_t>
1327 {
1329 typedef typename MatrixType::Scalar num_t;
1330 typedef typename Distance::template traits<num_t,self_t>::distance_t metric_t;
1332
1333 index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index.
1334
1335 /// Constructor: takes a const ref to the matrix object with the data points
1336 KDTreeEigenMatrixAdaptor(const int dimensionality, const MatrixType &mat, const int leaf_max_size = 10) : m_data_matrix(mat)
1337 {
1338 const size_t dims = mat.cols();
1339 if (DIM>0 && static_cast<int>(dims)!=DIM)
1340 throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
1341 index = new index_t( dims, *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size, dims ) );
1342 index->buildIndex();
1343 }
1344 private:
1345 /** Hidden copy constructor, to disallow copying this class (Not implemented) */
1347 public:
1348
1350 delete index;
1351 }
1352
1353 const MatrixType &m_data_matrix;
1354
1355 /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]).
1356 * Note that this is a short-cut method for index->findNeighbors().
1357 * The user can also call index->... methods as desired.
1358 * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
1359 */
1360 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
1361 {
1363 resultSet.init(out_indices, out_distances_sq);
1364 index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
1365 }
1366
1367 /** @name Interface expected by KDTreeSingleIndexAdaptor
1368 * @{ */
1369
1370 const self_t & derived() const {
1371 return *this;
1372 }
1374 return *this;
1375 }
1376
1377 // Must return the number of data points
1378 inline size_t kdtree_get_point_count() const {
1379 return m_data_matrix.rows();
1380 }
1381
1382 // Returns the L2 distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
1383 inline num_t kdtree_distance(const num_t *p1, const size_t idx_p2,size_t size) const
1384 {
1385 num_t s=0;
1386 for (size_t i=0; i<size; i++) {
1387 const num_t d= p1[i]-m_data_matrix.coeff(idx_p2,i);
1388 s+=d*d;
1389 }
1390 return s;
1391 }
1392
1393 // Returns the dim'th component of the idx'th point in the class:
1394 inline num_t kdtree_get_pt(const size_t idx, int dim) const {
1395 return m_data_matrix.coeff(idx,dim);
1396 }
1397
1398 // Optional bounding-box computation: return false to default to a standard bbox computation loop.
1399 // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
1400 // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
1401 template <class BBOX>
1402 bool kdtree_get_bbox(BBOX &bb) const {
1403 return false;
1404 }
1405
1406 /** @} */
1407
1408 }; // end of KDTreeEigenMatrixAdaptor
1409 /** @} */
1410
1411/** @} */ // end of grouping
1412} // end of NS
1413
1414
1415#endif /* NANOFLANN_HPP_ */
A STL container (as wrapper) for arrays of constant size defined at compile time (class imported from...
reverse_iterator rend()
const T * const_iterator
static void rangecheck(size_type i)
reference front()
void assign(const size_t n, const T &value)
const_reference front() const
std::size_t size_type
const_reference operator[](size_type i) const
reference back()
void swap(CArray< T, N > &y)
reference operator[](size_type i)
const_reference at(size_type i) const
static bool empty()
reference at(size_type i)
std::ptrdiff_t difference_type
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...
const_reference back() const
static size_type max_size()
const_reverse_iterator rend() const
std::reverse_iterator< iterator > reverse_iterator
static size_type size()
const_reverse_iterator rbegin() const
const T & const_reference
const_iterator begin() const
const T * data() const
void assign(const T &value)
reverse_iterator rbegin()
std::reverse_iterator< const_iterator > const_reverse_iterator
const_iterator end() const
size_t size() const
Returns size of index.
const KDTreeSingleIndexAdaptorParams index_params
KDTreeSingleIndexAdaptor(const KDTreeSingleIndexAdaptor< Distance, DatasetAdaptor, DIM, IndexType > &)
Hidden copy constructor, to disallow copying indices (Not implemented)
~KDTreeSingleIndexAdaptor()
Standard destructor.
int dim
Dimensionality of each data point.
NodePtr root_node
Array of k-d trees used to find neighbours.
void saveIndex(FILE *stream)
Stores the index in a binary file.
Distance::ElementType ElementType
void knnSearch(const ElementType *query_point, const size_t num_closest, IndexType *out_indices, DistanceType *out_distances_sq, const int=10) const
Find the "num_closest" nearest neighbors to the query_point[0:dim-1].
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 'cutfeat' dimension...
size_t usedMemory() const
Computes the inde memory usage Returns: memory used by the index.
void save_tree(FILE *stream, NodePtr tree)
size_t veclen() const
Returns the length of an index feature.
KDTreeSingleIndexAdaptor(const int dimensionality, const DatasetAdaptor &inputData, const KDTreeSingleIndexAdaptorParams &params=KDTreeSingleIndexAdaptorParams())
KDTree constructor.
void computeMinMax(IndexType *ind, IndexType count, int element, ElementType &min_elem, ElementType &max_elem)
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.
void middleSplit_(IndexType *ind, IndexType count, IndexType &index, int &cutfeat, DistanceType &cutval, const BoundingBox &bbox)
std::vector< IndexType > vind
Array of indices to vectors in the dataset.
void freeIndex()
Frees the previously-built index.
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].
void init_vind()
Make sure the auxiliary list vind has the same size than the current dataset, and re-generate if size...
const DatasetAdaptor & dataset
The dataset used by this index.
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.
ElementType dataset_get(size_t idx, int component) const
Helper accessor to the dataset points:
DistanceType computeInitialDistances(const ElementType *vec, distance_vector_t &dists) const
void loadIndex(FILE *stream)
Loads a previous index from a binary file.
BranchStruct< NodePtr, DistanceType > BranchSt
Distance::DistanceType DistanceType
void buildIndex()
Builds the index.
PooledAllocator pool
Pooled memory allocator.
void computeBoundingBox(BoundingBox &bbox)
array_or_vector_selector< DIM, Interval >::container_t BoundingBox
Define "BoundingBox" as a fixed-size or variable-size container depending on "DIM".
void findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const
Find set of nearest neighbors to vec[0:dim-1].
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".
void load_tree(FILE *stream, NodePtr &tree)
void init(IndexType *indices_, DistanceType *dists_)
Definition nanoflann.hpp:76
void addPoint(DistanceType dist, IndexType index)
Definition nanoflann.hpp:95
DistanceType * dists
Definition nanoflann.hpp:67
CountType size() const
Definition nanoflann.hpp:84
KNNResultSet(CountType capacity_)
Definition nanoflann.hpp:72
DistanceType worstDist() const
PooledAllocator(const size_t blocksize_=BLOCKSIZE)
Default constructor.
void free_all()
Frees all allocated memory chunks.
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.
T * allocate(const size_t count=1)
Allocates (using this pool) a generic type T.
A result-set class used when performing a radius based search.
std::pair< IndexType, DistanceType > worst_item() const
Find the worst result (furtherest neighbor) without copying or sorting Pre-conditions: size() > 0.
void addPoint(DistanceType dist, IndexType index)
const DistanceType radius
DistanceType worstDist() const
RadiusResultSet(DistanceType radius_, std::vector< std::pair< IndexType, DistanceType > > &indices_dists)
std::vector< std::pair< IndexType, DistanceType > > & m_indices_dists
void set_radius_and_clear(const DistanceType r)
Clears the result set and adjusts the search radius.
Scalar * iterator
const Scalar * const_iterator
@ static_size
EIGEN_STRONG_INLINE iterator begin()
EIGEN_STRONG_INLINE iterator end()
void load_value(FILE *stream, T &value, size_t count=1)
void save_value(FILE *stream, const T &value, size_t count=1)
const size_t WORDSIZE
Pooled storage allocator.
T * allocate(size_t count=1)
Allocates (using C's malloc) a generic type T.
const size_t BLOCKSIZE
long double abs< long double >(long double x)
float abs< float >(float x)
double abs< double >(double x)
int abs< int >(int x)
T abs(T x)
operator "<" for std::sort()
bool operator()(const PairType &p1, const PairType &p2) const
PairType will be typically: std::pair<IndexType,DistanceType>
An L2-metric KD-tree adaptor for working with data directly stored in an Eigen Matrix,...
void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int=10) const
Query for the num_closest closest points to a given point (entered as query_point[0:dim-1]).
bool kdtree_get_bbox(BBOX &bb) const
num_t kdtree_get_pt(const size_t idx, int dim) const
KDTreeEigenMatrixAdaptor(const self_t &)
Hidden copy constructor, to disallow copying this class (Not implemented)
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.
Distance::template traits< num_t, self_t >::distance_t metric_t
KDTreeSingleIndexAdaptor< metric_t, self_t, DIM, IndexType > index_t
KDTreeEigenMatrixAdaptor< MatrixType, DIM, Distance, IndexType > self_t
num_t kdtree_distance(const num_t *p1, const size_t idx_p2, size_t size) const
This record represents a branch point when finding neighbors in the tree.
bool operator<(const BranchStruct< T, DistanceType > &rhs) const
BranchStruct(const T &aNode, DistanceType dist)
IndexType left
Indices of points in leaf node.
DistanceType divlow
The values used for subdivision.
struct nanoflann::KDTreeSingleIndexAdaptor::Node::@10::@13 sub
struct nanoflann::KDTreeSingleIndexAdaptor::Node::@10::@12 lr
int divfeat
Dimension used for subdivision.
Parameters (see http://code.google.com/p/nanoflann/ for help choosing the parameters)
KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size=10, int dim_=-1)
Manhattan distance functor (generic version, optimized for high-dimensionality data sets).
_DistanceType DistanceType
L1_Adaptor(const DataSource &_data_source)
DistanceType operator()(const T *a, const size_t b_idx, size_t size, DistanceType worst_dist=-1) const
DistanceType accum_dist(const U a, const V b, int) const
const DataSource & data_source
Squared Euclidean distance functor (generic version, optimized for high-dimensionality data sets).
_DistanceType DistanceType
DistanceType operator()(const T *a, const size_t b_idx, size_t size, DistanceType worst_dist=-1) const
L2_Adaptor(const DataSource &_data_source)
DistanceType accum_dist(const U a, const V b, int) const
const DataSource & data_source
Squared Euclidean (L2) distance functor (suitable for low-dimensionality datasets,...
DistanceType accum_dist(const U a, const V b, int) const
const DataSource & data_source
L2_Simple_Adaptor(const DataSource &_data_source)
DistanceType operator()(const T *a, const size_t b_idx, size_t size) const
Search options for KDTreeSingleIndexAdaptor::findNeighbors()
bool sorted
only for radius search, require neighbours sorted by distance (default: true)
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...
float eps
search for eps-approximate neighbours (default: 0)
int checks
Ignored parameter (Kept for compatibility with the FLANN interface).
Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors when DIM=-1.
L1_Adaptor< T, DataSource > distance_t
Metaprogramming helper traits class for the L1 (Manhattan) metric.
L2_Adaptor< T, DataSource > distance_t
L2_Simple_Adaptor< T, DataSource > distance_t
Metaprogramming helper traits class for the L2_simple (Euclidean) metric.
Metaprogramming helper traits class for the L2 (Euclidean) metric.



Page generated by Doxygen 1.9.7 for MRPT 1.4.0 SVN: at Tue Jun 13 13:56:43 UTC 2023