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-2021 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 /** \mainpage nanoflann C++ API documentation
34 * nanoflann is a C++ header-only library for building KD-Trees, mostly
35 * optimized for 2D or 3D point clouds.
36 *
37 * nanoflann does not require compiling or installing, just an
38 * #include <nanoflann.hpp> in your code.
39 *
40 * See:
41 * - <a href="modules.html" >C++ API organized by modules</a>
42 * - <a href="https://github.com/jlblancoc/nanoflann" >Online README</a>
43 * - <a href="http://jlblancoc.github.io/nanoflann/" >Doxygen
44 * documentation</a>
45 */
46
47 #ifndef NANOFLANN_HPP_
48 #define NANOFLANN_HPP_
49
50 // Uwe Schulzweida
51 #define NANOFLANN_FIRST_MATCH 1
52
53 #include <algorithm>
54 #include <array>
55 #include <cassert>
56 #include <cmath> // for abs()
57 #include <cstdio> // for fwrite()
58 #include <cstdlib> // for abs()
59 #include <functional>
60 #include <limits> // std::reference_wrapper
61 #include <stdexcept>
62 #include <vector>
63
64 /** Library version: 0xMmP (M=Major,m=minor,P=patch) */
65 #define NANOFLANN_VERSION 0x132
66
67 // Avoid conflicting declaration of min/max macros in windows headers
68 #if !defined(NOMINMAX) && \
69 (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64))
70 #define NOMINMAX
71 #ifdef max
72 #undef max
73 #undef min
74 #endif
75 #endif
76
77 namespace nanoflann {
78 /** @addtogroup nanoflann_grp nanoflann C++ library for ANN
79 * @{ */
80
81 /** the PI constant (required to avoid MSVC missing symbols) */
pi_const()82 template <typename T> T pi_const() {
83 return static_cast<T>(3.14159265358979323846);
84 }
85
86 /**
87 * Traits if object is resizable and assignable (typically has a resize | assign
88 * method)
89 */
90 template <typename T, typename = int> struct has_resize : std::false_type {};
91
92 template <typename T>
93 struct has_resize<T, decltype((void)std::declval<T>().resize(1), 0)>
94 : std::true_type {};
95
96 template <typename T, typename = int> struct has_assign : std::false_type {};
97
98 template <typename T>
99 struct has_assign<T, decltype((void)std::declval<T>().assign(1, 0), 0)>
100 : std::true_type {};
101
102 /**
103 * Free function to resize a resizable object
104 */
105 template <typename Container>
106 inline typename std::enable_if<has_resize<Container>::value, void>::type
resize(Container & c,const size_t nElements)107 resize(Container &c, const size_t nElements) {
108 c.resize(nElements);
109 }
110
111 /**
112 * Free function that has no effects on non resizable containers (e.g.
113 * std::array) It raises an exception if the expected size does not match
114 */
115 template <typename Container>
116 inline typename std::enable_if<!has_resize<Container>::value, void>::type
resize(Container & c,const size_t nElements)117 resize(Container &c, const size_t nElements) {
118 if (nElements != c.size())
119 throw std::logic_error("Try to change the size of a std::array.");
120 }
121
122 /**
123 * Free function to assign to a container
124 */
125 template <typename Container, typename T>
126 inline typename std::enable_if<has_assign<Container>::value, void>::type
assign(Container & c,const size_t nElements,const T & value)127 assign(Container &c, const size_t nElements, const T &value) {
128 c.assign(nElements, value);
129 }
130
131 /**
132 * Free function to assign to a std::array
133 */
134 template <typename Container, typename T>
135 inline typename std::enable_if<!has_assign<Container>::value, void>::type
assign(Container & c,const size_t nElements,const T & value)136 assign(Container &c, const size_t nElements, const T &value) {
137 for (size_t i = 0; i < nElements; i++)
138 c[i] = value;
139 }
140
141 /** @addtogroup result_sets_grp Result set classes
142 * @{ */
143 template <typename _DistanceType, typename _IndexType = size_t,
144 typename _CountType = size_t>
145 class KNNResultSet {
146 public:
147 typedef _DistanceType DistanceType;
148 typedef _IndexType IndexType;
149 typedef _CountType CountType;
150
151 private:
152 IndexType *indices;
153 DistanceType *dists;
154 // Uwe Schulzweida
155 DistanceType radius;
156 CountType capacity;
157 CountType count;
158
159 public:
KNNResultSet(CountType capacity_)160 inline KNNResultSet(CountType capacity_)
161 : indices(0), dists(0), capacity(capacity_), count(0) {}
162 // Uwe Schulzweida
KNNResultSet(DistanceType radius_,CountType capacity_)163 inline KNNResultSet(DistanceType radius_, CountType capacity_)
164 : indices(0), dists(0), radius(radius_), capacity(capacity_), count(0) {}
165
init(IndexType * indices_,DistanceType * dists_)166 inline void init(IndexType *indices_, DistanceType *dists_) {
167 indices = indices_;
168 dists = dists_;
169 count = 0;
170 // Uwe Schulzweida
171 if (capacity) dists[capacity - 1] = radius;
172 if (capacity) indices[capacity - 1] = (std::numeric_limits<IndexType>::max)();
173 }
174
size() const175 inline CountType size() const { return count; }
176
full() const177 inline bool full() const { return count == capacity; }
178
179 /**
180 * Called during search to add an element matching the criteria.
181 * @return true if the search should be continued, false if the results are
182 * sufficient
183 */
addPoint(DistanceType dist,IndexType index)184 inline bool addPoint(DistanceType dist, IndexType index) {
185 CountType i;
186 for (i = count; i > 0; --i) {
187 #ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same
188 // distance, the one with the lowest-index will be
189 // returned first.
190 // if ((dists[i - 1] > dist) || ((dist == dists[i - 1]) && (indices[i - 1] > index))) {
191 // Uwe Schulzweida
192 if ((dists[i - 1] > dist) || ((dist <= dists[i - 1]) && (indices[i - 1] > index))) {
193 #else
194 if (dists[i - 1] > dist) {
195 #endif
196 if (i < capacity) {
197 dists[i] = dists[i - 1];
198 indices[i] = indices[i - 1];
199 }
200 } else
201 break;
202 }
203 if (i < capacity) {
204 dists[i] = dist;
205 indices[i] = index;
206 }
207 if (count < capacity)
208 count++;
209
210 // tell caller that the search shall continue
211 return true;
212 }
213
214 inline DistanceType worstDist() const { return dists[capacity - 1]; }
215 // Uwe Schulzweida
216 inline IndexType worstIndex() const { return indices[capacity - 1]; }
217
218 };
219
220 /** operator "<" for std::sort() */
221 struct IndexDist_Sorter {
222 /** PairType will be typically: std::pair<IndexType,DistanceType> */
223 template <typename PairType>
operator ()nanoflann::IndexDist_Sorter224 inline bool operator()(const PairType &p1, const PairType &p2) const {
225 return p1.second < p2.second;
226 }
227 };
228
229 /**
230 * A result-set class used when performing a radius based search.
231 */
232 template <typename _DistanceType, typename _IndexType = size_t>
233 class RadiusResultSet {
234 public:
235 typedef _DistanceType DistanceType;
236 typedef _IndexType IndexType;
237
238 public:
239 const DistanceType radius;
240
241 std::vector<std::pair<IndexType, DistanceType>> &m_indices_dists;
242
RadiusResultSet(DistanceType radius_,std::vector<std::pair<IndexType,DistanceType>> & indices_dists)243 inline RadiusResultSet(
244 DistanceType radius_,
245 std::vector<std::pair<IndexType, DistanceType>> &indices_dists)
246 : radius(radius_), m_indices_dists(indices_dists) {
247 init();
248 }
249
init()250 inline void init() { clear(); }
clear()251 inline void clear() { m_indices_dists.clear(); }
252
size() const253 inline size_t size() const { return m_indices_dists.size(); }
254
full() const255 inline bool full() const { return true; }
256
257 /**
258 * Called during search to add an element matching the criteria.
259 * @return true if the search should be continued, false if the results are
260 * sufficient
261 */
addPoint(DistanceType dist,IndexType index)262 inline bool addPoint(DistanceType dist, IndexType index) {
263 if (dist < radius)
264 m_indices_dists.push_back(std::make_pair(index, dist));
265 return true;
266 }
267
worstDist() const268 inline DistanceType worstDist() const { return radius; }
269
270 /**
271 * Find the worst result (furtherest neighbor) without copying or sorting
272 * Pre-conditions: size() > 0
273 */
worst_item() const274 std::pair<IndexType, DistanceType> worst_item() const {
275 if (m_indices_dists.empty())
276 throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on "
277 "an empty list of results.");
278 typedef
279 typename std::vector<std::pair<IndexType, DistanceType>>::const_iterator
280 DistIt;
281 DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end(),
282 IndexDist_Sorter());
283 return *it;
284 }
285 };
286
287 /** @} */
288
289 /** @addtogroup loadsave_grp Load/save auxiliary functions
290 * @{ */
291 template <typename T>
save_value(FILE * stream,const T & value,size_t count=1)292 void save_value(FILE *stream, const T &value, size_t count = 1) {
293 fwrite(&value, sizeof(value), count, stream);
294 }
295
296 template <typename T>
save_value(FILE * stream,const std::vector<T> & value)297 void save_value(FILE *stream, const std::vector<T> &value) {
298 size_t size = value.size();
299 fwrite(&size, sizeof(size_t), 1, stream);
300 fwrite(&value[0], sizeof(T), size, stream);
301 }
302
303 template <typename T>
load_value(FILE * stream,T & value,size_t count=1)304 void load_value(FILE *stream, T &value, size_t count = 1) {
305 size_t read_cnt = fread(&value, sizeof(value), count, stream);
306 if (read_cnt != count) {
307 throw std::runtime_error("Cannot read from file");
308 }
309 }
310
load_value(FILE * stream,std::vector<T> & value)311 template <typename T> void load_value(FILE *stream, std::vector<T> &value) {
312 size_t size;
313 size_t read_cnt = fread(&size, sizeof(size_t), 1, stream);
314 if (read_cnt != 1) {
315 throw std::runtime_error("Cannot read from file");
316 }
317 value.resize(size);
318 read_cnt = fread(&value[0], sizeof(T), size, stream);
319 if (read_cnt != size) {
320 throw std::runtime_error("Cannot read from file");
321 }
322 }
323 /** @} */
324
325 /** @addtogroup metric_grp Metric (distance) classes
326 * @{ */
327
328 struct Metric {};
329
330 /** Manhattan distance functor (generic version, optimized for
331 * high-dimensionality data sets). Corresponding distance traits:
332 * nanoflann::metric_L1 \tparam T Type of the elements (e.g. double, float,
333 * uint8_t) \tparam _DistanceType Type of distance variables (must be signed)
334 * (e.g. float, double, int64_t)
335 */
336 template <class T, class DataSource, typename _DistanceType = T>
337 struct L1_Adaptor {
338 typedef T ElementType;
339 typedef _DistanceType DistanceType;
340
341 const DataSource &data_source;
342
L1_Adaptornanoflann::L1_Adaptor343 L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) {}
344
evalMetricnanoflann::L1_Adaptor345 inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size,
346 DistanceType worst_dist = -1) const {
347 DistanceType result = DistanceType();
348 const T *last = a + size;
349 const T *lastgroup = last - 3;
350 size_t d = 0;
351
352 /* Process 4 items with each loop for efficiency. */
353 while (a < lastgroup) {
354 const DistanceType diff0 =
355 std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++));
356 const DistanceType diff1 =
357 std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++));
358 const DistanceType diff2 =
359 std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++));
360 const DistanceType diff3 =
361 std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++));
362 result += diff0 + diff1 + diff2 + diff3;
363 a += 4;
364 if ((worst_dist > 0) && (result > worst_dist)) {
365 return result;
366 }
367 }
368 /* Process last 0-3 components. Not needed for standard vector lengths. */
369 while (a < last) {
370 result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++));
371 }
372 return result;
373 }
374
375 template <typename U, typename V>
accum_distnanoflann::L1_Adaptor376 inline DistanceType accum_dist(const U a, const V b, const size_t) const {
377 return std::abs(a - b);
378 }
379 };
380
381 /** Squared Euclidean distance functor (generic version, optimized for
382 * high-dimensionality data sets). Corresponding distance traits:
383 * nanoflann::metric_L2 \tparam T Type of the elements (e.g. double, float,
384 * uint8_t) \tparam _DistanceType Type of distance variables (must be signed)
385 * (e.g. float, double, int64_t)
386 */
387 template <class T, class DataSource, typename _DistanceType = T>
388 struct L2_Adaptor {
389 typedef T ElementType;
390 typedef _DistanceType DistanceType;
391
392 const DataSource &data_source;
393
L2_Adaptornanoflann::L2_Adaptor394 L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {}
395
evalMetricnanoflann::L2_Adaptor396 inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size,
397 DistanceType worst_dist = -1) const {
398 DistanceType result = DistanceType();
399 const T *last = a + size;
400 const T *lastgroup = last - 3;
401 size_t d = 0;
402
403 /* Process 4 items with each loop for efficiency. */
404 while (a < lastgroup) {
405 const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx, d++);
406 const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx, d++);
407 const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx, d++);
408 const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx, d++);
409 result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
410 a += 4;
411 if ((worst_dist > 0) && (result > worst_dist)) {
412 return result;
413 }
414 }
415 /* Process last 0-3 components. Not needed for standard vector lengths. */
416 while (a < last) {
417 const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx, d++);
418 result += diff0 * diff0;
419 }
420 return result;
421 }
422
423 template <typename U, typename V>
accum_distnanoflann::L2_Adaptor424 inline DistanceType accum_dist(const U a, const V b, const size_t) const {
425 return (a - b) * (a - b);
426 }
427 };
428
429 /** Squared Euclidean (L2) distance functor (suitable for low-dimensionality
430 * datasets, like 2D or 3D point clouds) Corresponding distance traits:
431 * nanoflann::metric_L2_Simple \tparam T Type of the elements (e.g. double,
432 * float, uint8_t) \tparam _DistanceType Type of distance variables (must be
433 * signed) (e.g. float, double, int64_t)
434 */
435 template <class T, class DataSource, typename _DistanceType = T>
436 struct L2_Simple_Adaptor {
437 typedef T ElementType;
438 typedef _DistanceType DistanceType;
439
440 const DataSource &data_source;
441
L2_Simple_Adaptornanoflann::L2_Simple_Adaptor442 L2_Simple_Adaptor(const DataSource &_data_source)
443 : data_source(_data_source) {}
444
evalMetricnanoflann::L2_Simple_Adaptor445 inline DistanceType evalMetric(const T *a, const size_t b_idx,
446 size_t size) const {
447 DistanceType result = DistanceType();
448 for (size_t i = 0; i < size; ++i) {
449 const DistanceType diff = a[i] - data_source.kdtree_get_pt(b_idx, i);
450 result += diff * diff;
451 }
452 return result;
453 }
454
455 template <typename U, typename V>
accum_distnanoflann::L2_Simple_Adaptor456 inline DistanceType accum_dist(const U a, const V b, const size_t) const {
457 return (a - b) * (a - b);
458 }
459 };
460
461 /** SO2 distance functor
462 * Corresponding distance traits: nanoflann::metric_SO2
463 * \tparam T Type of the elements (e.g. double, float)
464 * \tparam _DistanceType Type of distance variables (must be signed) (e.g.
465 * float, double) orientation is constrained to be in [-pi, pi]
466 */
467 template <class T, class DataSource, typename _DistanceType = T>
468 struct SO2_Adaptor {
469 typedef T ElementType;
470 typedef _DistanceType DistanceType;
471
472 const DataSource &data_source;
473
SO2_Adaptornanoflann::SO2_Adaptor474 SO2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {}
475
evalMetricnanoflann::SO2_Adaptor476 inline DistanceType evalMetric(const T *a, const size_t b_idx,
477 size_t size) const {
478 return accum_dist(a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1),
479 size - 1);
480 }
481
482 /** Note: this assumes that input angles are already in the range [-pi,pi] */
483 template <typename U, typename V>
accum_distnanoflann::SO2_Adaptor484 inline DistanceType accum_dist(const U a, const V b, const size_t) const {
485 DistanceType result = DistanceType();
486 DistanceType PI = pi_const<DistanceType>();
487 result = b - a;
488 if (result > PI)
489 result -= 2 * PI;
490 else if (result < -PI)
491 result += 2 * PI;
492 return result;
493 }
494 };
495
496 /** SO3 distance functor (Uses L2_Simple)
497 * Corresponding distance traits: nanoflann::metric_SO3
498 * \tparam T Type of the elements (e.g. double, float)
499 * \tparam _DistanceType Type of distance variables (must be signed) (e.g.
500 * float, double)
501 */
502 template <class T, class DataSource, typename _DistanceType = T>
503 struct SO3_Adaptor {
504 typedef T ElementType;
505 typedef _DistanceType DistanceType;
506
507 L2_Simple_Adaptor<T, DataSource> distance_L2_Simple;
508
SO3_Adaptornanoflann::SO3_Adaptor509 SO3_Adaptor(const DataSource &_data_source)
510 : distance_L2_Simple(_data_source) {}
511
evalMetricnanoflann::SO3_Adaptor512 inline DistanceType evalMetric(const T *a, const size_t b_idx,
513 size_t size) const {
514 return distance_L2_Simple.evalMetric(a, b_idx, size);
515 }
516
517 template <typename U, typename V>
accum_distnanoflann::SO3_Adaptor518 inline DistanceType accum_dist(const U a, const V b, const size_t idx) const {
519 return distance_L2_Simple.accum_dist(a, b, idx);
520 }
521 };
522
523 /** Metaprogramming helper traits class for the L1 (Manhattan) metric */
524 struct metric_L1 : public Metric {
525 template <class T, class DataSource> struct traits {
526 typedef L1_Adaptor<T, DataSource> distance_t;
527 };
528 };
529 /** Metaprogramming helper traits class for the L2 (Euclidean) metric */
530 struct metric_L2 : public Metric {
531 template <class T, class DataSource> struct traits {
532 typedef L2_Adaptor<T, DataSource> distance_t;
533 };
534 };
535 /** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */
536 struct metric_L2_Simple : public Metric {
537 template <class T, class DataSource> struct traits {
538 typedef L2_Simple_Adaptor<T, DataSource> distance_t;
539 };
540 };
541 /** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */
542 struct metric_SO2 : public Metric {
543 template <class T, class DataSource> struct traits {
544 typedef SO2_Adaptor<T, DataSource> distance_t;
545 };
546 };
547 /** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */
548 struct metric_SO3 : public Metric {
549 template <class T, class DataSource> struct traits {
550 typedef SO3_Adaptor<T, DataSource> distance_t;
551 };
552 };
553
554 /** @} */
555
556 /** @addtogroup param_grp Parameter structs
557 * @{ */
558
559 /** Parameters (see README.md) */
560 struct KDTreeSingleIndexAdaptorParams {
KDTreeSingleIndexAdaptorParamsnanoflann::KDTreeSingleIndexAdaptorParams561 KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10)
562 : leaf_max_size(_leaf_max_size) {}
563
564 size_t leaf_max_size;
565 };
566
567 /** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */
568 struct SearchParams {
569 /** Note: The first argument (checks_IGNORED_) is ignored, but kept for
570 * compatibility with the FLANN interface */
SearchParamsnanoflann::SearchParams571 SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true)
572 : checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {}
573
574 int checks; //!< Ignored parameter (Kept for compatibility with the FLANN
575 //!< interface).
576 float eps; //!< search for eps-approximate neighbours (default: 0)
577 bool sorted; //!< only for radius search, require neighbours sorted by
578 //!< distance (default: true)
579 };
580 /** @} */
581
582 /** @addtogroup memalloc_grp Memory allocation
583 * @{ */
584
585 /**
586 * Allocates (using C's malloc) a generic type T.
587 *
588 * Params:
589 * count = number of instances to allocate.
590 * Returns: pointer (of type T*) to memory buffer
591 */
allocate(size_t count=1)592 template <typename T> inline T *allocate(size_t count = 1) {
593 T *mem = static_cast<T *>(::malloc(sizeof(T) * count));
594 return mem;
595 }
596
597 /**
598 * Pooled storage allocator
599 *
600 * The following routines allow for the efficient allocation of storage in
601 * small chunks from a specified pool. Rather than allowing each structure
602 * to be freed individually, an entire pool of storage is freed at once.
603 * This method has two advantages over just using malloc() and free(). First,
604 * it is far more efficient for allocating small objects, as there is
605 * no overhead for remembering all the information needed to free each
606 * object or consolidating fragmented memory. Second, the decision about
607 * how long to keep an object is made at the time of allocation, and there
608 * is no need to track down all the objects to free them.
609 *
610 */
611
612 const size_t WORDSIZE = 16;
613 const size_t BLOCKSIZE = 8192;
614
615 class PooledAllocator {
616 /* We maintain memory alignment to word boundaries by requiring that all
617 allocations be in multiples of the machine wordsize. */
618 /* Size of machine word in bytes. Must be power of 2. */
619 /* Minimum number of bytes requested at a time from the system. Must be
620 * multiple of WORDSIZE. */
621
622 size_t remaining; /* Number of bytes left in current block of storage. */
623 void *base; /* Pointer to base of current block of storage. */
624 void *loc; /* Current location in block to next allocate memory. */
625
internal_init()626 void internal_init() {
627 remaining = 0;
628 base = NULL;
629 usedMemory = 0;
630 wastedMemory = 0;
631 }
632
633 public:
634 size_t usedMemory;
635 size_t wastedMemory;
636
637 /**
638 Default constructor. Initializes a new pool.
639 */
PooledAllocator()640 PooledAllocator() { internal_init(); }
641
642 /**
643 * Destructor. Frees all the memory allocated in this pool.
644 */
~PooledAllocator()645 ~PooledAllocator() { free_all(); }
646
647 /** Frees all allocated memory chunks */
free_all()648 void free_all() {
649 while (base != NULL) {
650 void *prev =
651 *(static_cast<void **>(base)); /* Get pointer to prev block. */
652 ::free(base);
653 base = prev;
654 }
655 internal_init();
656 }
657
658 /**
659 * Returns a pointer to a piece of new memory of the given size in bytes
660 * allocated from the pool.
661 */
malloc(const size_t req_size)662 void *malloc(const size_t req_size) {
663 /* Round size up to a multiple of wordsize. The following expression
664 only works for WORDSIZE that is a power of 2, by masking last bits of
665 incremented size to zero.
666 */
667 const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);
668
669 /* Check whether a new block must be allocated. Note that the first word
670 of a block is reserved for a pointer to the previous block.
671 */
672 if (size > remaining) {
673
674 wastedMemory += remaining;
675
676 /* Allocate new storage. */
677 const size_t blocksize =
678 (size + sizeof(void *) + (WORDSIZE - 1) > BLOCKSIZE)
679 ? size + sizeof(void *) + (WORDSIZE - 1)
680 : BLOCKSIZE;
681
682 // use the standard C malloc to allocate memory
683 void *m = ::malloc(blocksize);
684 if (!m) {
685 fprintf(stderr, "Failed to allocate memory.\n");
686 throw std::bad_alloc();
687 }
688
689 /* Fill first word of new block with pointer to previous block. */
690 static_cast<void **>(m)[0] = base;
691 base = m;
692
693 size_t shift = 0;
694 // int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) &
695 // (WORDSIZE-1))) & (WORDSIZE-1);
696
697 remaining = blocksize - sizeof(void *) - shift;
698 loc = (static_cast<char *>(m) + sizeof(void *) + shift);
699 }
700 void *rloc = loc;
701 loc = static_cast<char *>(loc) + size;
702 remaining -= size;
703
704 usedMemory += size;
705
706 return rloc;
707 }
708
709 /**
710 * Allocates (using this pool) a generic type T.
711 *
712 * Params:
713 * count = number of instances to allocate.
714 * Returns: pointer (of type T*) to memory buffer
715 */
allocate(const size_t count=1)716 template <typename T> T *allocate(const size_t count = 1) {
717 T *mem = static_cast<T *>(this->malloc(sizeof(T) * count));
718 return mem;
719 }
720 };
721 /** @} */
722
723 /** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff
724 * @{ */
725
726 /** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors
727 * when DIM=-1. Fixed size version for a generic DIM:
728 */
729 template <int DIM, typename T> struct array_or_vector_selector {
730 typedef std::array<T, DIM> container_t;
731 };
732 /** Dynamic size version */
733 template <typename T> struct array_or_vector_selector<-1, T> {
734 typedef std::vector<T> container_t;
735 };
736
737 /** @} */
738
739 /** kd-tree base-class
740 *
741 * Contains the member functions common to the classes KDTreeSingleIndexAdaptor
742 * and KDTreeSingleIndexDynamicAdaptor_.
743 *
744 * \tparam Derived The name of the class which inherits this class.
745 * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
746 * \tparam Distance The distance metric to use, these are all classes derived
747 * from nanoflann::Metric \tparam DIM Dimensionality of data points (e.g. 3 for
748 * 3D points) \tparam IndexType Will be typically size_t or int
749 */
750
751 template <class Derived, typename Distance, class DatasetAdaptor, int DIM = -1,
752 typename IndexType = size_t>
753 class KDTreeBaseClass {
754
755 public:
756 /** Frees the previously-built index. Automatically called within
757 * buildIndex(). */
freeIndex(Derived & obj)758 void freeIndex(Derived &obj) {
759 obj.pool.free_all();
760 obj.root_node = NULL;
761 obj.m_size_at_index_build = 0;
762 }
763
764 typedef typename Distance::ElementType ElementType;
765 typedef typename Distance::DistanceType DistanceType;
766
767 /*--------------------- Internal Data Structures --------------------------*/
768 struct Node {
769 /** Union used because a node can be either a LEAF node or a non-leaf node,
770 * so both data fields are never used simultaneously */
771 union {
772 struct leaf {
773 IndexType left, right; //!< Indices of points in leaf node
774 } lr;
775 struct nonleaf {
776 int divfeat; //!< Dimension used for subdivision.
777 DistanceType divlow, divhigh; //!< The values used for subdivision.
778 } sub;
779 } node_type;
780 Node *child1, *child2; //!< Child nodes (both=NULL mean its a leaf node)
781 };
782
783 typedef Node *NodePtr;
784
785 struct Interval {
786 ElementType low, high;
787 };
788
789 /**
790 * Array of indices to vectors in the dataset.
791 */
792 std::vector<IndexType> vind;
793
794 NodePtr root_node;
795
796 size_t m_leaf_max_size;
797
798 size_t m_size; //!< Number of current points in the dataset
799 size_t m_size_at_index_build; //!< Number of points in the dataset when the
800 //!< index was built
801 int dim; //!< Dimensionality of each data point
802
803 /** Define "BoundingBox" as a fixed-size or variable-size container depending
804 * on "DIM" */
805 typedef
806 typename array_or_vector_selector<DIM, Interval>::container_t BoundingBox;
807
808 /** Define "distance_vector_t" as a fixed-size or variable-size container
809 * depending on "DIM" */
810 typedef typename array_or_vector_selector<DIM, DistanceType>::container_t
811 distance_vector_t;
812
813 /** The KD-tree used to find neighbours */
814
815 BoundingBox root_bbox;
816
817 /**
818 * Pooled memory allocator.
819 *
820 * Using a pooled memory allocator is more efficient
821 * than allocating memory directly when there is a large
822 * number small of memory allocations.
823 */
824 PooledAllocator pool;
825
826 /** Returns number of points in dataset */
size(const Derived & obj) const827 size_t size(const Derived &obj) const { return obj.m_size; }
828
829 /** Returns the length of each point in the dataset */
veclen(const Derived & obj)830 size_t veclen(const Derived &obj) {
831 return static_cast<size_t>(DIM > 0 ? DIM : obj.dim);
832 }
833
834 /// Helper accessor to the dataset points:
dataset_get(const Derived & obj,size_t idx,int component) const835 inline ElementType dataset_get(const Derived &obj, size_t idx,
836 int component) const {
837 return obj.dataset.kdtree_get_pt(idx, component);
838 }
839
840 /**
841 * Computes the inde memory usage
842 * Returns: memory used by the index
843 */
usedMemory(Derived & obj)844 size_t usedMemory(Derived &obj) {
845 return obj.pool.usedMemory + obj.pool.wastedMemory +
846 obj.dataset.kdtree_get_point_count() *
847 sizeof(IndexType); // pool memory and vind array memory
848 }
849
computeMinMax(const Derived & obj,IndexType * ind,IndexType count,int element,ElementType & min_elem,ElementType & max_elem)850 void computeMinMax(const Derived &obj, IndexType *ind, IndexType count,
851 int element, ElementType &min_elem,
852 ElementType &max_elem) {
853 min_elem = dataset_get(obj, ind[0], element);
854 max_elem = dataset_get(obj, ind[0], element);
855 for (IndexType i = 1; i < count; ++i) {
856 ElementType val = dataset_get(obj, ind[i], element);
857 if (val < min_elem)
858 min_elem = val;
859 if (val > max_elem)
860 max_elem = val;
861 }
862 }
863
864 /**
865 * Create a tree node that subdivides the list of vecs from vind[first]
866 * to vind[last]. The routine is called recursively on each sublist.
867 *
868 * @param left index of the first vector
869 * @param right index of the last vector
870 */
divideTree(Derived & obj,const IndexType left,const IndexType right,BoundingBox & bbox)871 NodePtr divideTree(Derived &obj, const IndexType left, const IndexType right,
872 BoundingBox &bbox) {
873 NodePtr node = obj.pool.template allocate<Node>(); // allocate memory
874
875 /* If too few exemplars remain, then make this a leaf node. */
876 if ((right - left) <= static_cast<IndexType>(obj.m_leaf_max_size)) {
877 node->child1 = node->child2 = NULL; /* Mark as leaf node. */
878 node->node_type.lr.left = left;
879 node->node_type.lr.right = right;
880
881 // compute bounding-box of leaf points
882 for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
883 bbox[i].low = dataset_get(obj, obj.vind[left], i);
884 bbox[i].high = dataset_get(obj, obj.vind[left], i);
885 }
886 for (IndexType k = left + 1; k < right; ++k) {
887 for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
888 if (bbox[i].low > dataset_get(obj, obj.vind[k], i))
889 bbox[i].low = dataset_get(obj, obj.vind[k], i);
890 if (bbox[i].high < dataset_get(obj, obj.vind[k], i))
891 bbox[i].high = dataset_get(obj, obj.vind[k], i);
892 }
893 }
894 } else {
895 IndexType idx;
896 int cutfeat;
897 DistanceType cutval;
898 middleSplit_(obj, &obj.vind[0] + left, right - left, idx, cutfeat, cutval,
899 bbox);
900
901 node->node_type.sub.divfeat = cutfeat;
902
903 BoundingBox left_bbox(bbox);
904 left_bbox[cutfeat].high = cutval;
905 node->child1 = divideTree(obj, left, left + idx, left_bbox);
906
907 BoundingBox right_bbox(bbox);
908 right_bbox[cutfeat].low = cutval;
909 node->child2 = divideTree(obj, left + idx, right, right_bbox);
910
911 node->node_type.sub.divlow = left_bbox[cutfeat].high;
912 node->node_type.sub.divhigh = right_bbox[cutfeat].low;
913
914 for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
915 bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
916 bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
917 }
918 }
919
920 return node;
921 }
922
middleSplit_(Derived & obj,IndexType * ind,IndexType count,IndexType & index,int & cutfeat,DistanceType & cutval,const BoundingBox & bbox)923 void middleSplit_(Derived &obj, IndexType *ind, IndexType count,
924 IndexType &index, int &cutfeat, DistanceType &cutval,
925 const BoundingBox &bbox) {
926 const DistanceType EPS = static_cast<DistanceType>(0.00001);
927 ElementType max_span = bbox[0].high - bbox[0].low;
928 for (int i = 1; i < (DIM > 0 ? DIM : obj.dim); ++i) {
929 ElementType span = bbox[i].high - bbox[i].low;
930 if (span > max_span) {
931 max_span = span;
932 }
933 }
934 ElementType max_spread = -1;
935 cutfeat = 0;
936 for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
937 ElementType span = bbox[i].high - bbox[i].low;
938 if (span > (1 - EPS) * max_span) {
939 ElementType min_elem, max_elem;
940 computeMinMax(obj, ind, count, i, min_elem, max_elem);
941 ElementType spread = max_elem - min_elem;
942 ;
943 if (spread > max_spread) {
944 cutfeat = i;
945 max_spread = spread;
946 }
947 }
948 }
949 // split in the middle
950 DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2;
951 ElementType min_elem, max_elem;
952 computeMinMax(obj, ind, count, cutfeat, min_elem, max_elem);
953
954 if (split_val < min_elem)
955 cutval = min_elem;
956 else if (split_val > max_elem)
957 cutval = max_elem;
958 else
959 cutval = split_val;
960
961 IndexType lim1, lim2;
962 planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2);
963
964 if (lim1 > count / 2)
965 index = lim1;
966 else if (lim2 < count / 2)
967 index = lim2;
968 else
969 index = count / 2;
970 }
971
972 /**
973 * Subdivide the list of points by a plane perpendicular on axe corresponding
974 * to the 'cutfeat' dimension at 'cutval' position.
975 *
976 * On return:
977 * dataset[ind[0..lim1-1]][cutfeat]<cutval
978 * dataset[ind[lim1..lim2-1]][cutfeat]==cutval
979 * dataset[ind[lim2..count]][cutfeat]>cutval
980 */
planeSplit(Derived & obj,IndexType * ind,const IndexType count,int cutfeat,DistanceType & cutval,IndexType & lim1,IndexType & lim2)981 void planeSplit(Derived &obj, IndexType *ind, const IndexType count,
982 int cutfeat, DistanceType &cutval, IndexType &lim1,
983 IndexType &lim2) {
984 /* Move vector indices for left subtree to front of list. */
985 IndexType left = 0;
986 IndexType right = count - 1;
987 for (;;) {
988 while (left <= right && dataset_get(obj, ind[left], cutfeat) < cutval)
989 ++left;
990 while (right && left <= right &&
991 dataset_get(obj, ind[right], cutfeat) >= cutval)
992 --right;
993 if (left > right || !right)
994 break; // "!right" was added to support unsigned Index types
995 std::swap(ind[left], ind[right]);
996 ++left;
997 --right;
998 }
999 /* If either list is empty, it means that all remaining features
1000 * are identical. Split in the middle to maintain a balanced tree.
1001 */
1002 lim1 = left;
1003 right = count - 1;
1004 for (;;) {
1005 while (left <= right && dataset_get(obj, ind[left], cutfeat) <= cutval)
1006 ++left;
1007 while (right && left <= right &&
1008 dataset_get(obj, ind[right], cutfeat) > cutval)
1009 --right;
1010 if (left > right || !right)
1011 break; // "!right" was added to support unsigned Index types
1012 std::swap(ind[left], ind[right]);
1013 ++left;
1014 --right;
1015 }
1016 lim2 = left;
1017 }
1018
computeInitialDistances(const Derived & obj,const ElementType * vec,distance_vector_t & dists) const1019 DistanceType computeInitialDistances(const Derived &obj,
1020 const ElementType *vec,
1021 distance_vector_t &dists) const {
1022 assert(vec);
1023 DistanceType distsq = DistanceType();
1024
1025 for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
1026 if (vec[i] < obj.root_bbox[i].low) {
1027 dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].low, i);
1028 distsq += dists[i];
1029 }
1030 if (vec[i] > obj.root_bbox[i].high) {
1031 dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].high, i);
1032 distsq += dists[i];
1033 }
1034 }
1035 return distsq;
1036 }
1037
save_tree(Derived & obj,FILE * stream,NodePtr tree)1038 void save_tree(Derived &obj, FILE *stream, NodePtr tree) {
1039 save_value(stream, *tree);
1040 if (tree->child1 != NULL) {
1041 save_tree(obj, stream, tree->child1);
1042 }
1043 if (tree->child2 != NULL) {
1044 save_tree(obj, stream, tree->child2);
1045 }
1046 }
1047
load_tree(Derived & obj,FILE * stream,NodePtr & tree)1048 void load_tree(Derived &obj, FILE *stream, NodePtr &tree) {
1049 tree = obj.pool.template allocate<Node>();
1050 load_value(stream, *tree);
1051 if (tree->child1 != NULL) {
1052 load_tree(obj, stream, tree->child1);
1053 }
1054 if (tree->child2 != NULL) {
1055 load_tree(obj, stream, tree->child2);
1056 }
1057 }
1058
1059 /** Stores the index in a binary file.
1060 * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when
1061 * loading the index object it must be constructed associated to the same
1062 * source of data points used while building it. See the example:
1063 * examples/saveload_example.cpp \sa loadIndex */
saveIndex_(Derived & obj,FILE * stream)1064 void saveIndex_(Derived &obj, FILE *stream) {
1065 save_value(stream, obj.m_size);
1066 save_value(stream, obj.dim);
1067 save_value(stream, obj.root_bbox);
1068 save_value(stream, obj.m_leaf_max_size);
1069 save_value(stream, obj.vind);
1070 save_tree(obj, stream, obj.root_node);
1071 }
1072
1073 /** Loads a previous index from a binary file.
1074 * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the
1075 * index object must be constructed associated to the same source of data
1076 * points used while building the index. See the example:
1077 * examples/saveload_example.cpp \sa loadIndex */
loadIndex_(Derived & obj,FILE * stream)1078 void loadIndex_(Derived &obj, FILE *stream) {
1079 load_value(stream, obj.m_size);
1080 load_value(stream, obj.dim);
1081 load_value(stream, obj.root_bbox);
1082 load_value(stream, obj.m_leaf_max_size);
1083 load_value(stream, obj.vind);
1084 load_tree(obj, stream, obj.root_node);
1085 }
1086 };
1087
1088 /** @addtogroup kdtrees_grp KD-tree classes and adaptors
1089 * @{ */
1090
1091 /** kd-tree static index
1092 *
1093 * Contains the k-d trees and other information for indexing a set of points
1094 * for nearest-neighbor matching.
1095 *
1096 * The class "DatasetAdaptor" must provide the following interface (can be
1097 * non-virtual, inlined methods):
1098 *
1099 * \code
1100 * // Must return the number of data poins
1101 * inline size_t kdtree_get_point_count() const { ... }
1102 *
1103 *
1104 * // Must return the dim'th component of the idx'th point in the class:
1105 * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... }
1106 *
1107 * // Optional bounding-box computation: return false to default to a standard
1108 * bbox computation loop.
1109 * // Return true if the BBOX was already computed by the class and returned
1110 * in "bb" so it can be avoided to redo it again.
1111 * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3
1112 * for point clouds) template <class BBOX> bool kdtree_get_bbox(BBOX &bb) const
1113 * {
1114 * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits
1115 * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits
1116 * ...
1117 * return true;
1118 * }
1119 *
1120 * \endcode
1121 *
1122 * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
1123 * \tparam Distance The distance metric to use: nanoflann::metric_L1,
1124 * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM
1125 * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will
1126 * be typically size_t or int
1127 */
1128 template <typename Distance, class DatasetAdaptor, int DIM = -1,
1129 typename IndexType = size_t>
1130 class KDTreeSingleIndexAdaptor
1131 : public KDTreeBaseClass<
1132 KDTreeSingleIndexAdaptor<Distance, DatasetAdaptor, DIM, IndexType>,
1133 Distance, DatasetAdaptor, DIM, IndexType> {
1134 public:
1135 /** Deleted copy constructor*/
1136 KDTreeSingleIndexAdaptor(
1137 const KDTreeSingleIndexAdaptor<Distance, DatasetAdaptor, DIM, IndexType>
1138 &) = delete;
1139
1140 /**
1141 * The dataset used by this index
1142 */
1143 const DatasetAdaptor &dataset; //!< The source of our data
1144
1145 const KDTreeSingleIndexAdaptorParams index_params;
1146
1147 Distance distance;
1148
1149 typedef typename nanoflann::KDTreeBaseClass<
1150 nanoflann::KDTreeSingleIndexAdaptor<Distance, DatasetAdaptor, DIM,
1151 IndexType>,
1152 Distance, DatasetAdaptor, DIM, IndexType>
1153 BaseClassRef;
1154
1155 typedef typename BaseClassRef::ElementType ElementType;
1156 typedef typename BaseClassRef::DistanceType DistanceType;
1157
1158 typedef typename BaseClassRef::Node Node;
1159 typedef Node *NodePtr;
1160
1161 typedef typename BaseClassRef::Interval Interval;
1162 /** Define "BoundingBox" as a fixed-size or variable-size container depending
1163 * on "DIM" */
1164 typedef typename BaseClassRef::BoundingBox BoundingBox;
1165
1166 /** Define "distance_vector_t" as a fixed-size or variable-size container
1167 * depending on "DIM" */
1168 typedef typename BaseClassRef::distance_vector_t distance_vector_t;
1169
1170 /**
1171 * KDTree constructor
1172 *
1173 * Refer to docs in README.md or online in
1174 * https://github.com/jlblancoc/nanoflann
1175 *
1176 * The KD-Tree point dimension (the length of each point in the datase, e.g. 3
1177 * for 3D points) is determined by means of:
1178 * - The \a DIM template parameter if >0 (highest priority)
1179 * - Otherwise, the \a dimensionality parameter of this constructor.
1180 *
1181 * @param inputData Dataset with the input features
1182 * @param params Basically, the maximum leaf node size
1183 */
KDTreeSingleIndexAdaptor(const int dimensionality,const DatasetAdaptor & inputData,const KDTreeSingleIndexAdaptorParams & params=KDTreeSingleIndexAdaptorParams ())1184 KDTreeSingleIndexAdaptor(const int dimensionality,
1185 const DatasetAdaptor &inputData,
1186 const KDTreeSingleIndexAdaptorParams ¶ms =
1187 KDTreeSingleIndexAdaptorParams())
1188 : dataset(inputData), index_params(params), distance(inputData) {
1189 BaseClassRef::root_node = NULL;
1190 BaseClassRef::m_size = dataset.kdtree_get_point_count();
1191 BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
1192 BaseClassRef::dim = dimensionality;
1193 if (DIM > 0)
1194 BaseClassRef::dim = DIM;
1195 BaseClassRef::m_leaf_max_size = params.leaf_max_size;
1196
1197 // Create a permutable array of indices to the input vectors.
1198 init_vind();
1199 }
1200
1201 /**
1202 * Builds the index
1203 */
buildIndex()1204 void buildIndex() {
1205 BaseClassRef::m_size = dataset.kdtree_get_point_count();
1206 BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
1207 init_vind();
1208 this->freeIndex(*this);
1209 BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
1210 if (BaseClassRef::m_size == 0)
1211 return;
1212 computeBoundingBox(BaseClassRef::root_bbox);
1213 BaseClassRef::root_node =
1214 this->divideTree(*this, 0, BaseClassRef::m_size,
1215 BaseClassRef::root_bbox); // construct the tree
1216 }
1217
1218 /** \name Query methods
1219 * @{ */
1220
1221 /**
1222 * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored
1223 * inside the result object.
1224 *
1225 * Params:
1226 * result = the result object in which the indices of the
1227 * nearest-neighbors are stored vec = the vector for which to search the
1228 * nearest neighbors
1229 *
1230 * \tparam RESULTSET Should be any ResultSet<DistanceType>
1231 * \return True if the requested neighbors could be found.
1232 * \sa knnSearch, radiusSearch
1233 */
1234 template <typename RESULTSET>
findNeighbors(RESULTSET & result,const ElementType * vec,const SearchParams & searchParams) const1235 bool findNeighbors(RESULTSET &result, const ElementType *vec,
1236 const SearchParams &searchParams) const {
1237 assert(vec);
1238 if (this->size(*this) == 0)
1239 return false;
1240 if (!BaseClassRef::root_node)
1241 throw std::runtime_error(
1242 "[nanoflann] findNeighbors() called before building the index.");
1243 float epsError = 1 + searchParams.eps;
1244
1245 distance_vector_t
1246 dists; // fixed or variable-sized container (depending on DIM)
1247 auto zero = static_cast<decltype(result.worstDist())>(0);
1248 assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim),
1249 zero); // Fill it with zeros.
1250 DistanceType distsq = this->computeInitialDistances(*this, vec, dists);
1251 searchLevel(result, vec, BaseClassRef::root_node, distsq, dists,
1252 epsError); // "count_leaf" parameter removed since was neither
1253 // used nor returned to the user.
1254 return result.full();
1255 }
1256
1257 /**
1258 * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1].
1259 * Their indices are stored inside the result object. \sa radiusSearch,
1260 * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility
1261 * with the original FLANN interface. \return Number `N` of valid points in
1262 * the result set. Only the first `N` entries in `out_indices` and
1263 * `out_distances_sq` will be valid. Return may be less than `num_closest`
1264 * only if the number of elements in the tree is less than `num_closest`.
1265 */
knnSearch(const ElementType * query_point,const size_t num_closest,IndexType * out_indices,DistanceType * out_distances_sq,const int=10) const1266 size_t knnSearch(const ElementType *query_point, const size_t num_closest,
1267 IndexType *out_indices, DistanceType *out_distances_sq,
1268 const int /* nChecks_IGNORED */ = 10) const {
1269 nanoflann::KNNResultSet<DistanceType, IndexType> resultSet(num_closest);
1270 resultSet.init(out_indices, out_distances_sq);
1271 this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
1272 return resultSet.size();
1273 }
1274 // Uwe Schulzweida
knnRangeSearch(const ElementType * query_point,const DistanceType radius,const size_t num_closest,IndexType * out_indices,DistanceType * out_distances_sq,const int=10) const1275 size_t knnRangeSearch(const ElementType *query_point, const DistanceType radius, const size_t num_closest,
1276 IndexType *out_indices, DistanceType *out_distances_sq,
1277 const int /* nChecks_IGNORED */ = 10) const {
1278 nanoflann::KNNResultSet<DistanceType,IndexType> resultSet(radius, num_closest);
1279 resultSet.init(out_indices, out_distances_sq);
1280 this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
1281 return resultSet.size();
1282 }
1283
1284 /**
1285 * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius.
1286 * The output is given as a vector of pairs, of which the first element is a
1287 * point index and the second the corresponding distance. Previous contents of
1288 * \a IndicesDists are cleared.
1289 *
1290 * If searchParams.sorted==true, the output list is sorted by ascending
1291 * distances.
1292 *
1293 * For a better performance, it is advisable to do a .reserve() on the vector
1294 * if you have any wild guess about the number of expected matches.
1295 *
1296 * \sa knnSearch, findNeighbors, radiusSearchCustomCallback
1297 * \return The number of points within the given radius (i.e. indices.size()
1298 * or dists.size() )
1299 */
1300 size_t
radiusSearch(const ElementType * query_point,const DistanceType & radius,std::vector<std::pair<IndexType,DistanceType>> & IndicesDists,const SearchParams & searchParams) const1301 radiusSearch(const ElementType *query_point, const DistanceType &radius,
1302 std::vector<std::pair<IndexType, DistanceType>> &IndicesDists,
1303 const SearchParams &searchParams) const {
1304 RadiusResultSet<DistanceType, IndexType> resultSet(radius, IndicesDists);
1305 const size_t nFound =
1306 radiusSearchCustomCallback(query_point, resultSet, searchParams);
1307 if (searchParams.sorted)
1308 std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter());
1309 return nFound;
1310 }
1311
1312 /**
1313 * Just like radiusSearch() but with a custom callback class for each point
1314 * found in the radius of the query. See the source of RadiusResultSet<> as a
1315 * start point for your own classes. \sa radiusSearch
1316 */
1317 template <class SEARCH_CALLBACK>
radiusSearchCustomCallback(const ElementType * query_point,SEARCH_CALLBACK & resultSet,const SearchParams & searchParams=SearchParams ()) const1318 size_t radiusSearchCustomCallback(
1319 const ElementType *query_point, SEARCH_CALLBACK &resultSet,
1320 const SearchParams &searchParams = SearchParams()) const {
1321 this->findNeighbors(resultSet, query_point, searchParams);
1322 return resultSet.size();
1323 }
1324
1325 /** @} */
1326
1327 public:
1328 /** Make sure the auxiliary list \a vind has the same size than the current
1329 * dataset, and re-generate if size has changed. */
init_vind()1330 void init_vind() {
1331 // Create a permutable array of indices to the input vectors.
1332 BaseClassRef::m_size = dataset.kdtree_get_point_count();
1333 if (BaseClassRef::vind.size() != BaseClassRef::m_size)
1334 BaseClassRef::vind.resize(BaseClassRef::m_size);
1335 for (size_t i = 0; i < BaseClassRef::m_size; i++)
1336 BaseClassRef::vind[i] = i;
1337 }
1338
computeBoundingBox(BoundingBox & bbox)1339 void computeBoundingBox(BoundingBox &bbox) {
1340 resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim));
1341 if (dataset.kdtree_get_bbox(bbox)) {
1342 // Done! It was implemented in derived class
1343 } else {
1344 const size_t N = dataset.kdtree_get_point_count();
1345 if (!N)
1346 throw std::runtime_error("[nanoflann] computeBoundingBox() called but "
1347 "no data points found.");
1348 for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {
1349 bbox[i].low = bbox[i].high = this->dataset_get(*this, 0, i);
1350 }
1351 for (size_t k = 1; k < N; ++k) {
1352 for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {
1353 if (this->dataset_get(*this, k, i) < bbox[i].low)
1354 bbox[i].low = this->dataset_get(*this, k, i);
1355 if (this->dataset_get(*this, k, i) > bbox[i].high)
1356 bbox[i].high = this->dataset_get(*this, k, i);
1357 }
1358 }
1359 }
1360 }
1361
1362 /**
1363 * Performs an exact search in the tree starting from a node.
1364 * \tparam RESULTSET Should be any ResultSet<DistanceType>
1365 * \return true if the search should be continued, false if the results are
1366 * sufficient
1367 */
1368 template <class RESULTSET>
searchLevel(RESULTSET & result_set,const ElementType * vec,const NodePtr node,DistanceType mindistsq,distance_vector_t & dists,const float epsError) const1369 bool searchLevel(RESULTSET &result_set, const ElementType *vec,
1370 const NodePtr node, DistanceType mindistsq,
1371 distance_vector_t &dists, const float epsError) const {
1372 /* If this is a leaf node, then do check and return. */
1373 if ((node->child1 == NULL) && (node->child2 == NULL)) {
1374 // count_leaf += (node->lr.right-node->lr.left); // Removed since was
1375 // neither used nor returned to the user.
1376 DistanceType worst_dist = result_set.worstDist();
1377 // Uwe Schulzweida
1378 const IndexType worst_index = result_set.worstIndex();
1379 for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right;
1380 ++i) {
1381 const IndexType index = BaseClassRef::vind[i]; // reorder... : i;
1382 DistanceType dist = distance.evalMetric(
1383 vec, index, (DIM > 0 ? DIM : BaseClassRef::dim));
1384 #ifdef NANOFLANN_FIRST_MATCH // Uwe Schulzweida
1385 dist = (float) dist;
1386 if (dist < worst_dist || (dist <= worst_dist && index < worst_index) ) {
1387 #else
1388 if (dist < worst_dist) {
1389 #endif
1390 if (!result_set.addPoint(dist, BaseClassRef::vind[i])) {
1391 // the resultset doesn't want to receive any more points, we're done
1392 // searching!
1393 return false;
1394 }
1395 }
1396 }
1397 return true;
1398 }
1399
1400 /* Which child branch should be taken first? */
1401 int idx = node->node_type.sub.divfeat;
1402 ElementType val = vec[idx];
1403 DistanceType diff1 = val - node->node_type.sub.divlow;
1404 DistanceType diff2 = val - node->node_type.sub.divhigh;
1405
1406 NodePtr bestChild;
1407 NodePtr otherChild;
1408 DistanceType cut_dist;
1409 if ((diff1 + diff2) < 0) {
1410 bestChild = node->child1;
1411 otherChild = node->child2;
1412 cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx);
1413 } else {
1414 bestChild = node->child2;
1415 otherChild = node->child1;
1416 cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx);
1417 }
1418
1419 /* Call recursively to search next level down. */
1420 if (!searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError)) {
1421 // the resultset doesn't want to receive any more points, we're done
1422 // searching!
1423 return false;
1424 }
1425
1426 DistanceType dst = dists[idx];
1427 mindistsq = mindistsq + cut_dist - dst;
1428 dists[idx] = cut_dist;
1429 if (mindistsq * epsError <= result_set.worstDist()) {
1430 if (!searchLevel(result_set, vec, otherChild, mindistsq, dists,
1431 epsError)) {
1432 // the resultset doesn't want to receive any more points, we're done
1433 // searching!
1434 return false;
1435 }
1436 }
1437 dists[idx] = dst;
1438 return true;
1439 }
1440
1441 public:
1442 /** Stores the index in a binary file.
1443 * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when
1444 * loading the index object it must be constructed associated to the same
1445 * source of data points used while building it. See the example:
1446 * examples/saveload_example.cpp \sa loadIndex */
1447 void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); }
1448
1449 /** Loads a previous index from a binary file.
1450 * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the
1451 * index object must be constructed associated to the same source of data
1452 * points used while building the index. See the example:
1453 * examples/saveload_example.cpp \sa loadIndex */
1454 void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); }
1455
1456 }; // class KDTree
1457
1458 /** kd-tree dynamic index
1459 *
1460 * Contains the k-d trees and other information for indexing a set of points
1461 * for nearest-neighbor matching.
1462 *
1463 * The class "DatasetAdaptor" must provide the following interface (can be
1464 * non-virtual, inlined methods):
1465 *
1466 * \code
1467 * // Must return the number of data poins
1468 * inline size_t kdtree_get_point_count() const { ... }
1469 *
1470 * // Must return the dim'th component of the idx'th point in the class:
1471 * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... }
1472 *
1473 * // Optional bounding-box computation: return false to default to a standard
1474 * bbox computation loop.
1475 * // Return true if the BBOX was already computed by the class and returned
1476 * in "bb" so it can be avoided to redo it again.
1477 * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3
1478 * for point clouds) template <class BBOX> bool kdtree_get_bbox(BBOX &bb) const
1479 * {
1480 * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits
1481 * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits
1482 * ...
1483 * return true;
1484 * }
1485 *
1486 * \endcode
1487 *
1488 * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
1489 * \tparam Distance The distance metric to use: nanoflann::metric_L1,
1490 * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM
1491 * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will
1492 * be typically size_t or int
1493 */
1494 template <typename Distance, class DatasetAdaptor, int DIM = -1,
1495 typename IndexType = size_t>
1496 class KDTreeSingleIndexDynamicAdaptor_
1497 : public KDTreeBaseClass<KDTreeSingleIndexDynamicAdaptor_<
1498 Distance, DatasetAdaptor, DIM, IndexType>,
1499 Distance, DatasetAdaptor, DIM, IndexType> {
1500 public:
1501 /**
1502 * The dataset used by this index
1503 */
1504 const DatasetAdaptor &dataset; //!< The source of our data
1505
1506 KDTreeSingleIndexAdaptorParams index_params;
1507
1508 std::vector<int> &treeIndex;
1509
1510 Distance distance;
1511
1512 typedef typename nanoflann::KDTreeBaseClass<
1513 nanoflann::KDTreeSingleIndexDynamicAdaptor_<Distance, DatasetAdaptor, DIM,
1514 IndexType>,
1515 Distance, DatasetAdaptor, DIM, IndexType>
1516 BaseClassRef;
1517
1518 typedef typename BaseClassRef::ElementType ElementType;
1519 typedef typename BaseClassRef::DistanceType DistanceType;
1520
1521 typedef typename BaseClassRef::Node Node;
1522 typedef Node *NodePtr;
1523
1524 typedef typename BaseClassRef::Interval Interval;
1525 /** Define "BoundingBox" as a fixed-size or variable-size container depending
1526 * on "DIM" */
1527 typedef typename BaseClassRef::BoundingBox BoundingBox;
1528
1529 /** Define "distance_vector_t" as a fixed-size or variable-size container
1530 * depending on "DIM" */
1531 typedef typename BaseClassRef::distance_vector_t distance_vector_t;
1532
1533 /**
1534 * KDTree constructor
1535 *
1536 * Refer to docs in README.md or online in
1537 * https://github.com/jlblancoc/nanoflann
1538 *
1539 * The KD-Tree point dimension (the length of each point in the datase, e.g. 3
1540 * for 3D points) is determined by means of:
1541 * - The \a DIM template parameter if >0 (highest priority)
1542 * - Otherwise, the \a dimensionality parameter of this constructor.
1543 *
1544 * @param inputData Dataset with the input features
1545 * @param params Basically, the maximum leaf node size
1546 */
KDTreeSingleIndexDynamicAdaptor_(const int dimensionality,const DatasetAdaptor & inputData,std::vector<int> & treeIndex_,const KDTreeSingleIndexAdaptorParams & params=KDTreeSingleIndexAdaptorParams ())1547 KDTreeSingleIndexDynamicAdaptor_(
1548 const int dimensionality, const DatasetAdaptor &inputData,
1549 std::vector<int> &treeIndex_,
1550 const KDTreeSingleIndexAdaptorParams ¶ms =
1551 KDTreeSingleIndexAdaptorParams())
1552 : dataset(inputData), index_params(params), treeIndex(treeIndex_),
1553 distance(inputData) {
1554 BaseClassRef::root_node = NULL;
1555 BaseClassRef::m_size = 0;
1556 BaseClassRef::m_size_at_index_build = 0;
1557 BaseClassRef::dim = dimensionality;
1558 if (DIM > 0)
1559 BaseClassRef::dim = DIM;
1560 BaseClassRef::m_leaf_max_size = params.leaf_max_size;
1561 }
1562
1563 /** Assignment operator definiton */
1564 KDTreeSingleIndexDynamicAdaptor_
operator =(const KDTreeSingleIndexDynamicAdaptor_ & rhs)1565 operator=(const KDTreeSingleIndexDynamicAdaptor_ &rhs) {
1566 KDTreeSingleIndexDynamicAdaptor_ tmp(rhs);
1567 std::swap(BaseClassRef::vind, tmp.BaseClassRef::vind);
1568 std::swap(BaseClassRef::m_leaf_max_size, tmp.BaseClassRef::m_leaf_max_size);
1569 std::swap(index_params, tmp.index_params);
1570 std::swap(treeIndex, tmp.treeIndex);
1571 std::swap(BaseClassRef::m_size, tmp.BaseClassRef::m_size);
1572 std::swap(BaseClassRef::m_size_at_index_build,
1573 tmp.BaseClassRef::m_size_at_index_build);
1574 std::swap(BaseClassRef::root_node, tmp.BaseClassRef::root_node);
1575 std::swap(BaseClassRef::root_bbox, tmp.BaseClassRef::root_bbox);
1576 std::swap(BaseClassRef::pool, tmp.BaseClassRef::pool);
1577 return *this;
1578 }
1579
1580 /**
1581 * Builds the index
1582 */
buildIndex()1583 void buildIndex() {
1584 BaseClassRef::m_size = BaseClassRef::vind.size();
1585 this->freeIndex(*this);
1586 BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
1587 if (BaseClassRef::m_size == 0)
1588 return;
1589 computeBoundingBox(BaseClassRef::root_bbox);
1590 BaseClassRef::root_node =
1591 this->divideTree(*this, 0, BaseClassRef::m_size,
1592 BaseClassRef::root_bbox); // construct the tree
1593 }
1594
1595 /** \name Query methods
1596 * @{ */
1597
1598 /**
1599 * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored
1600 * inside the result object.
1601 *
1602 * Params:
1603 * result = the result object in which the indices of the
1604 * nearest-neighbors are stored vec = the vector for which to search the
1605 * nearest neighbors
1606 *
1607 * \tparam RESULTSET Should be any ResultSet<DistanceType>
1608 * \return True if the requested neighbors could be found.
1609 * \sa knnSearch, radiusSearch
1610 */
1611 template <typename RESULTSET>
findNeighbors(RESULTSET & result,const ElementType * vec,const SearchParams & searchParams) const1612 bool findNeighbors(RESULTSET &result, const ElementType *vec,
1613 const SearchParams &searchParams) const {
1614 assert(vec);
1615 if (this->size(*this) == 0)
1616 return false;
1617 if (!BaseClassRef::root_node)
1618 return false;
1619 float epsError = 1 + searchParams.eps;
1620
1621 // fixed or variable-sized container (depending on DIM)
1622 distance_vector_t dists;
1623 // Fill it with zeros.
1624 assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim),
1625 static_cast<typename distance_vector_t::value_type>(0));
1626 DistanceType distsq = this->computeInitialDistances(*this, vec, dists);
1627 searchLevel(result, vec, BaseClassRef::root_node, distsq, dists,
1628 epsError); // "count_leaf" parameter removed since was neither
1629 // used nor returned to the user.
1630 return result.full();
1631 }
1632
1633 /**
1634 * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1].
1635 * Their indices are stored inside the result object. \sa radiusSearch,
1636 * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility
1637 * with the original FLANN interface. \return Number `N` of valid points in
1638 * the result set. Only the first `N` entries in `out_indices` and
1639 * `out_distances_sq` will be valid. Return may be less than `num_closest`
1640 * only if the number of elements in the tree is less than `num_closest`.
1641 */
knnSearch(const ElementType * query_point,const size_t num_closest,IndexType * out_indices,DistanceType * out_distances_sq,const int=10) const1642 size_t knnSearch(const ElementType *query_point, const size_t num_closest,
1643 IndexType *out_indices, DistanceType *out_distances_sq,
1644 const int /* nChecks_IGNORED */ = 10) const {
1645 nanoflann::KNNResultSet<DistanceType, IndexType> resultSet(num_closest);
1646 resultSet.init(out_indices, out_distances_sq);
1647 this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
1648 return resultSet.size();
1649 }
1650
1651 /**
1652 * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius.
1653 * The output is given as a vector of pairs, of which the first element is a
1654 * point index and the second the corresponding distance. Previous contents of
1655 * \a IndicesDists are cleared.
1656 *
1657 * If searchParams.sorted==true, the output list is sorted by ascending
1658 * distances.
1659 *
1660 * For a better performance, it is advisable to do a .reserve() on the vector
1661 * if you have any wild guess about the number of expected matches.
1662 *
1663 * \sa knnSearch, findNeighbors, radiusSearchCustomCallback
1664 * \return The number of points within the given radius (i.e. indices.size()
1665 * or dists.size() )
1666 */
1667 size_t
radiusSearch(const ElementType * query_point,const DistanceType & radius,std::vector<std::pair<IndexType,DistanceType>> & IndicesDists,const SearchParams & searchParams) const1668 radiusSearch(const ElementType *query_point, const DistanceType &radius,
1669 std::vector<std::pair<IndexType, DistanceType>> &IndicesDists,
1670 const SearchParams &searchParams) const {
1671 RadiusResultSet<DistanceType, IndexType> resultSet(radius, IndicesDists);
1672 const size_t nFound =
1673 radiusSearchCustomCallback(query_point, resultSet, searchParams);
1674 if (searchParams.sorted)
1675 std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter());
1676 return nFound;
1677 }
1678
1679 /**
1680 * Just like radiusSearch() but with a custom callback class for each point
1681 * found in the radius of the query. See the source of RadiusResultSet<> as a
1682 * start point for your own classes. \sa radiusSearch
1683 */
1684 template <class SEARCH_CALLBACK>
radiusSearchCustomCallback(const ElementType * query_point,SEARCH_CALLBACK & resultSet,const SearchParams & searchParams=SearchParams ()) const1685 size_t radiusSearchCustomCallback(
1686 const ElementType *query_point, SEARCH_CALLBACK &resultSet,
1687 const SearchParams &searchParams = SearchParams()) const {
1688 this->findNeighbors(resultSet, query_point, searchParams);
1689 return resultSet.size();
1690 }
1691
1692 /** @} */
1693
1694 public:
computeBoundingBox(BoundingBox & bbox)1695 void computeBoundingBox(BoundingBox &bbox) {
1696 resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim));
1697
1698 if (dataset.kdtree_get_bbox(bbox)) {
1699 // Done! It was implemented in derived class
1700 } else {
1701 const size_t N = BaseClassRef::m_size;
1702 if (!N)
1703 throw std::runtime_error("[nanoflann] computeBoundingBox() called but "
1704 "no data points found.");
1705 for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {
1706 bbox[i].low = bbox[i].high =
1707 this->dataset_get(*this, BaseClassRef::vind[0], i);
1708 }
1709 for (size_t k = 1; k < N; ++k) {
1710 for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {
1711 if (this->dataset_get(*this, BaseClassRef::vind[k], i) < bbox[i].low)
1712 bbox[i].low = this->dataset_get(*this, BaseClassRef::vind[k], i);
1713 if (this->dataset_get(*this, BaseClassRef::vind[k], i) > bbox[i].high)
1714 bbox[i].high = this->dataset_get(*this, BaseClassRef::vind[k], i);
1715 }
1716 }
1717 }
1718 }
1719
1720 /**
1721 * Performs an exact search in the tree starting from a node.
1722 * \tparam RESULTSET Should be any ResultSet<DistanceType>
1723 */
1724 template <class RESULTSET>
searchLevel(RESULTSET & result_set,const ElementType * vec,const NodePtr node,DistanceType mindistsq,distance_vector_t & dists,const float epsError) const1725 void searchLevel(RESULTSET &result_set, const ElementType *vec,
1726 const NodePtr node, DistanceType mindistsq,
1727 distance_vector_t &dists, const float epsError) const {
1728 /* If this is a leaf node, then do check and return. */
1729 if ((node->child1 == NULL) && (node->child2 == NULL)) {
1730 // count_leaf += (node->lr.right-node->lr.left); // Removed since was
1731 // neither used nor returned to the user.
1732 DistanceType worst_dist = result_set.worstDist();
1733 for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right;
1734 ++i) {
1735 const IndexType index = BaseClassRef::vind[i]; // reorder... : i;
1736 if (treeIndex[index] == -1)
1737 continue;
1738 DistanceType dist = distance.evalMetric(
1739 vec, index, (DIM > 0 ? DIM : BaseClassRef::dim));
1740 if (dist < worst_dist) {
1741 if (!result_set.addPoint(
1742 static_cast<typename RESULTSET::DistanceType>(dist),
1743 static_cast<typename RESULTSET::IndexType>(
1744 BaseClassRef::vind[i]))) {
1745 // the resultset doesn't want to receive any more points, we're done
1746 // searching!
1747 return; // false;
1748 }
1749 }
1750 }
1751 return;
1752 }
1753
1754 /* Which child branch should be taken first? */
1755 int idx = node->node_type.sub.divfeat;
1756 ElementType val = vec[idx];
1757 DistanceType diff1 = val - node->node_type.sub.divlow;
1758 DistanceType diff2 = val - node->node_type.sub.divhigh;
1759
1760 NodePtr bestChild;
1761 NodePtr otherChild;
1762 DistanceType cut_dist;
1763 if ((diff1 + diff2) < 0) {
1764 bestChild = node->child1;
1765 otherChild = node->child2;
1766 cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx);
1767 } else {
1768 bestChild = node->child2;
1769 otherChild = node->child1;
1770 cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx);
1771 }
1772
1773 /* Call recursively to search next level down. */
1774 searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError);
1775
1776 DistanceType dst = dists[idx];
1777 mindistsq = mindistsq + cut_dist - dst;
1778 dists[idx] = cut_dist;
1779 if (mindistsq * epsError <= result_set.worstDist()) {
1780 searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError);
1781 }
1782 dists[idx] = dst;
1783 }
1784
1785 public:
1786 /** Stores the index in a binary file.
1787 * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when
1788 * loading the index object it must be constructed associated to the same
1789 * source of data points used while building it. See the example:
1790 * examples/saveload_example.cpp \sa loadIndex */
saveIndex(FILE * stream)1791 void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); }
1792
1793 /** Loads a previous index from a binary file.
1794 * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the
1795 * index object must be constructed associated to the same source of data
1796 * points used while building the index. See the example:
1797 * examples/saveload_example.cpp \sa loadIndex */
loadIndex(FILE * stream)1798 void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); }
1799 };
1800
1801 /** kd-tree dynaimic index
1802 *
1803 * class to create multiple static index and merge their results to behave as
1804 * single dynamic index as proposed in Logarithmic Approach.
1805 *
1806 * Example of usage:
1807 * examples/dynamic_pointcloud_example.cpp
1808 *
1809 * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
1810 * \tparam Distance The distance metric to use: nanoflann::metric_L1,
1811 * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM
1812 * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will
1813 * be typically size_t or int
1814 */
1815 template <typename Distance, class DatasetAdaptor, int DIM = -1,
1816 typename IndexType = size_t>
1817 class KDTreeSingleIndexDynamicAdaptor {
1818 public:
1819 typedef typename Distance::ElementType ElementType;
1820 typedef typename Distance::DistanceType DistanceType;
1821
1822 protected:
1823 size_t m_leaf_max_size;
1824 size_t treeCount;
1825 size_t pointCount;
1826
1827 /**
1828 * The dataset used by this index
1829 */
1830 const DatasetAdaptor &dataset; //!< The source of our data
1831
1832 std::vector<int> treeIndex; //!< treeIndex[idx] is the index of tree in which
1833 //!< point at idx is stored. treeIndex[idx]=-1
1834 //!< means that point has been removed.
1835
1836 KDTreeSingleIndexAdaptorParams index_params;
1837
1838 int dim; //!< Dimensionality of each data point
1839
1840 typedef KDTreeSingleIndexDynamicAdaptor_<Distance, DatasetAdaptor, DIM>
1841 index_container_t;
1842 std::vector<index_container_t> index;
1843
1844 public:
1845 /** Get a const ref to the internal list of indices; the number of indices is
1846 * adapted dynamically as the dataset grows in size. */
getAllIndices() const1847 const std::vector<index_container_t> &getAllIndices() const { return index; }
1848
1849 private:
1850 /** finds position of least significant unset bit */
First0Bit(IndexType num)1851 int First0Bit(IndexType num) {
1852 int pos = 0;
1853 while (num & 1) {
1854 num = num >> 1;
1855 pos++;
1856 }
1857 return pos;
1858 }
1859
1860 /** Creates multiple empty trees to handle dynamic support */
init()1861 void init() {
1862 typedef KDTreeSingleIndexDynamicAdaptor_<Distance, DatasetAdaptor, DIM>
1863 my_kd_tree_t;
1864 std::vector<my_kd_tree_t> index_(
1865 treeCount, my_kd_tree_t(dim /*dim*/, dataset, treeIndex, index_params));
1866 index = index_;
1867 }
1868
1869 public:
1870 Distance distance;
1871
1872 /**
1873 * KDTree constructor
1874 *
1875 * Refer to docs in README.md or online in
1876 * https://github.com/jlblancoc/nanoflann
1877 *
1878 * The KD-Tree point dimension (the length of each point in the datase, e.g. 3
1879 * for 3D points) is determined by means of:
1880 * - The \a DIM template parameter if >0 (highest priority)
1881 * - Otherwise, the \a dimensionality parameter of this constructor.
1882 *
1883 * @param inputData Dataset with the input features
1884 * @param params Basically, the maximum leaf node size
1885 */
KDTreeSingleIndexDynamicAdaptor(const int dimensionality,const DatasetAdaptor & inputData,const KDTreeSingleIndexAdaptorParams & params=KDTreeSingleIndexAdaptorParams (),const size_t maximumPointCount=1000000000U)1886 KDTreeSingleIndexDynamicAdaptor(const int dimensionality,
1887 const DatasetAdaptor &inputData,
1888 const KDTreeSingleIndexAdaptorParams ¶ms =
1889 KDTreeSingleIndexAdaptorParams(),
1890 const size_t maximumPointCount = 1000000000U)
1891 : dataset(inputData), index_params(params), distance(inputData) {
1892 treeCount = static_cast<size_t>(std::log2(maximumPointCount));
1893 pointCount = 0U;
1894 dim = dimensionality;
1895 treeIndex.clear();
1896 if (DIM > 0)
1897 dim = DIM;
1898 m_leaf_max_size = params.leaf_max_size;
1899 init();
1900 const size_t num_initial_points = dataset.kdtree_get_point_count();
1901 if (num_initial_points > 0) {
1902 addPoints(0, num_initial_points - 1);
1903 }
1904 }
1905
1906 /** Deleted copy constructor*/
1907 KDTreeSingleIndexDynamicAdaptor(
1908 const KDTreeSingleIndexDynamicAdaptor<Distance, DatasetAdaptor, DIM,
1909 IndexType> &) = delete;
1910
1911 /** Add points to the set, Inserts all points from [start, end] */
addPoints(IndexType start,IndexType end)1912 void addPoints(IndexType start, IndexType end) {
1913 size_t count = end - start + 1;
1914 treeIndex.resize(treeIndex.size() + count);
1915 for (IndexType idx = start; idx <= end; idx++) {
1916 int pos = First0Bit(pointCount);
1917 index[pos].vind.clear();
1918 treeIndex[pointCount] = pos;
1919 for (int i = 0; i < pos; i++) {
1920 for (int j = 0; j < static_cast<int>(index[i].vind.size()); j++) {
1921 index[pos].vind.push_back(index[i].vind[j]);
1922 if (treeIndex[index[i].vind[j]] != -1)
1923 treeIndex[index[i].vind[j]] = pos;
1924 }
1925 index[i].vind.clear();
1926 index[i].freeIndex(index[i]);
1927 }
1928 index[pos].vind.push_back(idx);
1929 index[pos].buildIndex();
1930 pointCount++;
1931 }
1932 }
1933
1934 /** Remove a point from the set (Lazy Deletion) */
removePoint(size_t idx)1935 void removePoint(size_t idx) {
1936 if (idx >= pointCount)
1937 return;
1938 treeIndex[idx] = -1;
1939 }
1940
1941 /**
1942 * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored
1943 * inside the result object.
1944 *
1945 * Params:
1946 * result = the result object in which the indices of the
1947 * nearest-neighbors are stored vec = the vector for which to search the
1948 * nearest neighbors
1949 *
1950 * \tparam RESULTSET Should be any ResultSet<DistanceType>
1951 * \return True if the requested neighbors could be found.
1952 * \sa knnSearch, radiusSearch
1953 */
1954 template <typename RESULTSET>
findNeighbors(RESULTSET & result,const ElementType * vec,const SearchParams & searchParams) const1955 bool findNeighbors(RESULTSET &result, const ElementType *vec,
1956 const SearchParams &searchParams) const {
1957 for (size_t i = 0; i < treeCount; i++) {
1958 index[i].findNeighbors(result, &vec[0], searchParams);
1959 }
1960 return result.full();
1961 }
1962 };
1963
1964 /** An L2-metric KD-tree adaptor for working with data directly stored in an
1965 * Eigen Matrix, without duplicating the data storage. You can select whether a
1966 * row or column in the matrix represents a point in the state space.
1967 *
1968 * Example of usage:
1969 * \code
1970 * Eigen::Matrix<num_t,Dynamic,Dynamic> mat;
1971 * // Fill out "mat"...
1972 *
1973 * typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix<num_t,Dynamic,Dynamic> >
1974 * my_kd_tree_t; const int max_leaf = 10; my_kd_tree_t mat_index(mat, max_leaf
1975 * ); mat_index.index->buildIndex(); mat_index.index->... \endcode
1976 *
1977 * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality
1978 * for the points in the data set, allowing more compiler optimizations. \tparam
1979 * Distance The distance metric to use: nanoflann::metric_L1,
1980 * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam row_major
1981 * If set to true the rows of the matrix are used as the points, if set to false
1982 * the columns of the matrix are used as the points.
1983 */
1984 template <class MatrixType, int DIM = -1, class Distance = nanoflann::metric_L2,
1985 bool row_major = true>
1986 struct KDTreeEigenMatrixAdaptor {
1987 typedef KDTreeEigenMatrixAdaptor<MatrixType, DIM, Distance, row_major> self_t;
1988 typedef typename MatrixType::Scalar num_t;
1989 typedef typename MatrixType::Index IndexType;
1990 typedef
1991 typename Distance::template traits<num_t, self_t>::distance_t metric_t;
1992 typedef KDTreeSingleIndexAdaptor<metric_t, self_t,
1993 MatrixType::ColsAtCompileTime, IndexType>
1994 index_t;
1995
1996 index_t *index; //! The kd-tree index for the user to call its methods as
1997 //! usual with any other FLANN index.
1998
1999 /// Constructor: takes a const ref to the matrix object with the data points
KDTreeEigenMatrixAdaptornanoflann::KDTreeEigenMatrixAdaptor2000 KDTreeEigenMatrixAdaptor(const size_t dimensionality,
2001 const std::reference_wrapper<const MatrixType> &mat,
2002 const int leaf_max_size = 10)
2003 : m_data_matrix(mat) {
2004 const auto dims = row_major ? mat.get().cols() : mat.get().rows();
2005 if (size_t(dims) != dimensionality)
2006 throw std::runtime_error(
2007 "Error: 'dimensionality' must match column count in data matrix");
2008 if (DIM > 0 && int(dims) != DIM)
2009 throw std::runtime_error(
2010 "Data set dimensionality does not match the 'DIM' template argument");
2011 index =
2012 new index_t(static_cast<int>(dims), *this /* adaptor */,
2013 nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size));
2014 index->buildIndex();
2015 }
2016
2017 public:
2018 /** Deleted copy constructor */
2019 KDTreeEigenMatrixAdaptor(const self_t &) = delete;
2020
~KDTreeEigenMatrixAdaptornanoflann::KDTreeEigenMatrixAdaptor2021 ~KDTreeEigenMatrixAdaptor() { delete index; }
2022
2023 const std::reference_wrapper<const MatrixType> m_data_matrix;
2024
2025 /** Query for the \a num_closest closest points to a given point (entered as
2026 * query_point[0:dim-1]). Note that this is a short-cut method for
2027 * index->findNeighbors(). The user can also call index->... methods as
2028 * desired. \note nChecks_IGNORED is ignored but kept for compatibility with
2029 * the original FLANN interface.
2030 */
querynanoflann::KDTreeEigenMatrixAdaptor2031 inline void query(const num_t *query_point, const size_t num_closest,
2032 IndexType *out_indices, num_t *out_distances_sq,
2033 const int /* nChecks_IGNORED */ = 10) const {
2034 nanoflann::KNNResultSet<num_t, IndexType> resultSet(num_closest);
2035 resultSet.init(out_indices, out_distances_sq);
2036 index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
2037 }
2038
2039 /** @name Interface expected by KDTreeSingleIndexAdaptor
2040 * @{ */
2041
derivednanoflann::KDTreeEigenMatrixAdaptor2042 const self_t &derived() const { return *this; }
derivednanoflann::KDTreeEigenMatrixAdaptor2043 self_t &derived() { return *this; }
2044
2045 // Must return the number of data points
kdtree_get_point_countnanoflann::KDTreeEigenMatrixAdaptor2046 inline size_t kdtree_get_point_count() const {
2047 if(row_major)
2048 return m_data_matrix.get().rows();
2049 else
2050 return m_data_matrix.get().cols();
2051 }
2052
2053 // Returns the dim'th component of the idx'th point in the class:
kdtree_get_ptnanoflann::KDTreeEigenMatrixAdaptor2054 inline num_t kdtree_get_pt(const IndexType idx, size_t dim) const {
2055 if(row_major)
2056 return m_data_matrix.get().coeff(idx, IndexType(dim));
2057 else
2058 return m_data_matrix.get().coeff(IndexType(dim), idx);
2059 }
2060
2061 // Optional bounding-box computation: return false to default to a standard
2062 // bbox computation loop.
2063 // Return true if the BBOX was already computed by the class and returned in
2064 // "bb" so it can be avoided to redo it again. Look at bb.size() to find out
2065 // the expected dimensionality (e.g. 2 or 3 for point clouds)
kdtree_get_bboxnanoflann::KDTreeEigenMatrixAdaptor2066 template <class BBOX> bool kdtree_get_bbox(BBOX & /*bb*/) const {
2067 return false;
2068 }
2069
2070 /** @} */
2071
2072 }; // end of KDTreeEigenMatrixAdaptor
2073 /** @} */
2074
2075 /** @} */ // end of grouping
2076 } // namespace nanoflann
2077
2078 #endif /* NANOFLANN_HPP_ */
2079