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