dist.h
Go to the documentation of this file.
00001 /***********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
00005  * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
00006  *
00007  * THE BSD LICENSE
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions
00011  * are met:
00012  *
00013  * 1. Redistributions of source code must retain the above copyright
00014  *    notice, this list of conditions and the following disclaimer.
00015  * 2. Redistributions in binary form must reproduce the above copyright
00016  *    notice, this list of conditions and the following disclaimer in the
00017  *    documentation and/or other materials provided with the distribution.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
00020  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
00021  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
00022  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
00023  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
00024  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00025  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
00026  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
00028  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029  *************************************************************************/
00030 
00031 #ifndef RTABMAP_FLANN_DIST_H_
00032 #define RTABMAP_FLANN_DIST_H_
00033 
00034 #include <cmath>
00035 #include <cstdlib>
00036 #include <string.h>
00037 #ifdef _MSC_VER
00038 typedef unsigned __int32 uint32_t;
00039 typedef unsigned __int64 uint64_t;
00040 #else
00041 #include <stdint.h>
00042 #endif
00043 
00044 #include "rtflann/defines.h"
00045 
00046 
00047 namespace rtflann
00048 {
00049 
00050 template<typename T>
00051 struct Accumulator { typedef T Type; };
00052 template<>
00053 struct Accumulator<unsigned char>  { typedef float Type; };
00054 template<>
00055 struct Accumulator<unsigned short> { typedef float Type; };
00056 template<>
00057 struct Accumulator<unsigned int> { typedef float Type; };
00058 template<>
00059 struct Accumulator<char>   { typedef float Type; };
00060 template<>
00061 struct Accumulator<short>  { typedef float Type; };
00062 template<>
00063 struct Accumulator<int> { typedef float Type; };
00064 
00065 
00066 
00073 template<class T>
00074 struct L2_Simple
00075 {
00076     typedef bool is_kdtree_distance;
00077 
00078     typedef T ElementType;
00079     typedef typename Accumulator<T>::Type ResultType;
00080 
00081     template <typename Iterator1, typename Iterator2>
00082     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
00083     {
00084         ResultType result = ResultType();
00085         ResultType diff;
00086         for(size_t i = 0; i < size; ++i ) {
00087             diff = *a++ - *b++;
00088             result += diff*diff;
00089         }
00090         return result;
00091     }
00092 
00093     template <typename U, typename V>
00094     inline ResultType accum_dist(const U& a, const V& b, int) const
00095     {
00096         return (a-b)*(a-b);
00097     }
00098 };
00099 
00100 template<class T>
00101 struct L2_3D
00102 {
00103     typedef bool is_kdtree_distance;
00104 
00105     typedef T ElementType;
00106     typedef typename Accumulator<T>::Type ResultType;
00107 
00108     template <typename Iterator1, typename Iterator2>
00109     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
00110     {
00111         ResultType result = ResultType();        
00112         ResultType diff;
00113         diff = *a++ - *b++;
00114         result += diff*diff;
00115         diff = *a++ - *b++;
00116         result += diff*diff;
00117         diff = *a++ - *b++;
00118         result += diff*diff;        
00119         return result;
00120     }
00121 
00122     template <typename U, typename V>
00123     inline ResultType accum_dist(const U& a, const V& b, int) const
00124     {
00125         return (a-b)*(a-b);
00126     }
00127 };
00128 
00132 template<class T>
00133 struct L2
00134 {
00135     typedef bool is_kdtree_distance;
00136 
00137     typedef T ElementType;
00138     typedef typename Accumulator<T>::Type ResultType;
00139 
00149     template <typename Iterator1, typename Iterator2>
00150     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
00151     {
00152         ResultType result = ResultType();
00153         ResultType diff0, diff1, diff2, diff3;
00154         Iterator1 last = a + size;
00155         Iterator1 lastgroup = last - 3;
00156 
00157         /* Process 4 items with each loop for efficiency. */
00158         while (a < lastgroup) {
00159             diff0 = (ResultType)(a[0] - b[0]);
00160             diff1 = (ResultType)(a[1] - b[1]);
00161             diff2 = (ResultType)(a[2] - b[2]);
00162             diff3 = (ResultType)(a[3] - b[3]);
00163             result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
00164             a += 4;
00165             b += 4;
00166 
00167             if ((worst_dist>0)&&(result>worst_dist)) {
00168                 return result;
00169             }
00170         }
00171         /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
00172         while (a < last) {
00173             diff0 = (ResultType)(*a++ - *b++);
00174             result += diff0 * diff0;
00175         }
00176         return result;
00177     }
00178 
00185     template <typename U, typename V>
00186     inline ResultType accum_dist(const U& a, const V& b, int) const
00187     {
00188         return (a-b)*(a-b);
00189     }
00190 };
00191 
00192 
00193 /*
00194  * Manhattan distance functor, optimized version
00195  */
00196 template<class T>
00197 struct L1
00198 {
00199     typedef bool is_kdtree_distance;
00200 
00201     typedef T ElementType;
00202     typedef typename Accumulator<T>::Type ResultType;
00203 
00210     template <typename Iterator1, typename Iterator2>
00211     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
00212     {
00213         ResultType result = ResultType();
00214         ResultType diff0, diff1, diff2, diff3;
00215         Iterator1 last = a + size;
00216         Iterator1 lastgroup = last - 3;
00217 
00218         /* Process 4 items with each loop for efficiency. */
00219         while (a < lastgroup) {
00220             diff0 = (ResultType)std::abs(a[0] - b[0]);
00221             diff1 = (ResultType)std::abs(a[1] - b[1]);
00222             diff2 = (ResultType)std::abs(a[2] - b[2]);
00223             diff3 = (ResultType)std::abs(a[3] - b[3]);
00224             result += diff0 + diff1 + diff2 + diff3;
00225             a += 4;
00226             b += 4;
00227 
00228             if ((worst_dist>0)&&(result>worst_dist)) {
00229                 return result;
00230             }
00231         }
00232         /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
00233         while (a < last) {
00234             diff0 = (ResultType)std::abs(*a++ - *b++);
00235             result += diff0;
00236         }
00237         return result;
00238     }
00239 
00243     template <typename U, typename V>
00244     inline ResultType accum_dist(const U& a, const V& b, int) const
00245     {
00246         return std::abs(a-b);
00247     }
00248 };
00249 
00250 
00251 
00252 template<class T>
00253 struct MinkowskiDistance
00254 {
00255     typedef bool is_kdtree_distance;
00256 
00257     typedef T ElementType;
00258     typedef typename Accumulator<T>::Type ResultType;
00259 
00260     int order;
00261 
00262     MinkowskiDistance(int order_) : order(order_) {}
00263 
00273     template <typename Iterator1, typename Iterator2>
00274     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
00275     {
00276         ResultType result = ResultType();
00277         ResultType diff0, diff1, diff2, diff3;
00278         Iterator1 last = a + size;
00279         Iterator1 lastgroup = last - 3;
00280 
00281         /* Process 4 items with each loop for efficiency. */
00282         while (a < lastgroup) {
00283             diff0 = (ResultType)std::abs(a[0] - b[0]);
00284             diff1 = (ResultType)std::abs(a[1] - b[1]);
00285             diff2 = (ResultType)std::abs(a[2] - b[2]);
00286             diff3 = (ResultType)std::abs(a[3] - b[3]);
00287             result += pow(diff0,order) + pow(diff1,order) + pow(diff2,order) + pow(diff3,order);
00288             a += 4;
00289             b += 4;
00290 
00291             if ((worst_dist>0)&&(result>worst_dist)) {
00292                 return result;
00293             }
00294         }
00295         /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
00296         while (a < last) {
00297             diff0 = (ResultType)std::abs(*a++ - *b++);
00298             result += pow(diff0,order);
00299         }
00300         return result;
00301     }
00302 
00306     template <typename U, typename V>
00307     inline ResultType accum_dist(const U& a, const V& b, int) const
00308     {
00309         return pow(static_cast<ResultType>(std::abs(a-b)),order);
00310     }
00311 };
00312 
00313 
00314 
00315 template<class T>
00316 struct MaxDistance
00317 {
00318     typedef bool is_vector_space_distance;
00319 
00320     typedef T ElementType;
00321     typedef typename Accumulator<T>::Type ResultType;
00322 
00328     template <typename Iterator1, typename Iterator2>
00329     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
00330     {
00331         ResultType result = ResultType();
00332         ResultType diff0, diff1, diff2, diff3;
00333         Iterator1 last = a + size;
00334         Iterator1 lastgroup = last - 3;
00335 
00336         /* Process 4 items with each loop for efficiency. */
00337         while (a < lastgroup) {
00338             diff0 = std::abs(a[0] - b[0]);
00339             diff1 = std::abs(a[1] - b[1]);
00340             diff2 = std::abs(a[2] - b[2]);
00341             diff3 = std::abs(a[3] - b[3]);
00342             if (diff0>result) {result = diff0; }
00343             if (diff1>result) {result = diff1; }
00344             if (diff2>result) {result = diff2; }
00345             if (diff3>result) {result = diff3; }
00346             a += 4;
00347             b += 4;
00348 
00349             if ((worst_dist>0)&&(result>worst_dist)) {
00350                 return result;
00351             }
00352         }
00353         /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
00354         while (a < last) {
00355             diff0 = std::abs(*a++ - *b++);
00356             result = (diff0>result) ? diff0 : result;
00357         }
00358         return result;
00359     }
00360 
00361     /* This distance functor is not dimension-wise additive, which
00362      * makes it an invalid kd-tree distance, not implementing the accum_dist method */
00363 
00364 };
00365 
00367 
00372 struct HammingLUT
00373 {
00374     typedef unsigned char ElementType;
00375     typedef int ResultType;
00376 
00379     ResultType operator()(const unsigned char* a, const unsigned char* b, int size) const
00380     {
00381         ResultType result = 0;
00382         for (int i = 0; i < size; i++) {
00383             result += byteBitsLookUp(a[i] ^ b[i]);
00384         }
00385         return result;
00386     }
00387 
00388 
00394     static unsigned char byteBitsLookUp(unsigned char b)
00395     {
00396         static const unsigned char table[256]  = {
00397             /* 0 */ 0, /* 1 */ 1, /* 2 */ 1, /* 3 */ 2,
00398             /* 4 */ 1, /* 5 */ 2, /* 6 */ 2, /* 7 */ 3,
00399             /* 8 */ 1, /* 9 */ 2, /* a */ 2, /* b */ 3,
00400             /* c */ 2, /* d */ 3, /* e */ 3, /* f */ 4,
00401             /* 10 */ 1, /* 11 */ 2, /* 12 */ 2, /* 13 */ 3,
00402             /* 14 */ 2, /* 15 */ 3, /* 16 */ 3, /* 17 */ 4,
00403             /* 18 */ 2, /* 19 */ 3, /* 1a */ 3, /* 1b */ 4,
00404             /* 1c */ 3, /* 1d */ 4, /* 1e */ 4, /* 1f */ 5,
00405             /* 20 */ 1, /* 21 */ 2, /* 22 */ 2, /* 23 */ 3,
00406             /* 24 */ 2, /* 25 */ 3, /* 26 */ 3, /* 27 */ 4,
00407             /* 28 */ 2, /* 29 */ 3, /* 2a */ 3, /* 2b */ 4,
00408             /* 2c */ 3, /* 2d */ 4, /* 2e */ 4, /* 2f */ 5,
00409             /* 30 */ 2, /* 31 */ 3, /* 32 */ 3, /* 33 */ 4,
00410             /* 34 */ 3, /* 35 */ 4, /* 36 */ 4, /* 37 */ 5,
00411             /* 38 */ 3, /* 39 */ 4, /* 3a */ 4, /* 3b */ 5,
00412             /* 3c */ 4, /* 3d */ 5, /* 3e */ 5, /* 3f */ 6,
00413             /* 40 */ 1, /* 41 */ 2, /* 42 */ 2, /* 43 */ 3,
00414             /* 44 */ 2, /* 45 */ 3, /* 46 */ 3, /* 47 */ 4,
00415             /* 48 */ 2, /* 49 */ 3, /* 4a */ 3, /* 4b */ 4,
00416             /* 4c */ 3, /* 4d */ 4, /* 4e */ 4, /* 4f */ 5,
00417             /* 50 */ 2, /* 51 */ 3, /* 52 */ 3, /* 53 */ 4,
00418             /* 54 */ 3, /* 55 */ 4, /* 56 */ 4, /* 57 */ 5,
00419             /* 58 */ 3, /* 59 */ 4, /* 5a */ 4, /* 5b */ 5,
00420             /* 5c */ 4, /* 5d */ 5, /* 5e */ 5, /* 5f */ 6,
00421             /* 60 */ 2, /* 61 */ 3, /* 62 */ 3, /* 63 */ 4,
00422             /* 64 */ 3, /* 65 */ 4, /* 66 */ 4, /* 67 */ 5,
00423             /* 68 */ 3, /* 69 */ 4, /* 6a */ 4, /* 6b */ 5,
00424             /* 6c */ 4, /* 6d */ 5, /* 6e */ 5, /* 6f */ 6,
00425             /* 70 */ 3, /* 71 */ 4, /* 72 */ 4, /* 73 */ 5,
00426             /* 74 */ 4, /* 75 */ 5, /* 76 */ 5, /* 77 */ 6,
00427             /* 78 */ 4, /* 79 */ 5, /* 7a */ 5, /* 7b */ 6,
00428             /* 7c */ 5, /* 7d */ 6, /* 7e */ 6, /* 7f */ 7,
00429             /* 80 */ 1, /* 81 */ 2, /* 82 */ 2, /* 83 */ 3,
00430             /* 84 */ 2, /* 85 */ 3, /* 86 */ 3, /* 87 */ 4,
00431             /* 88 */ 2, /* 89 */ 3, /* 8a */ 3, /* 8b */ 4,
00432             /* 8c */ 3, /* 8d */ 4, /* 8e */ 4, /* 8f */ 5,
00433             /* 90 */ 2, /* 91 */ 3, /* 92 */ 3, /* 93 */ 4,
00434             /* 94 */ 3, /* 95 */ 4, /* 96 */ 4, /* 97 */ 5,
00435             /* 98 */ 3, /* 99 */ 4, /* 9a */ 4, /* 9b */ 5,
00436             /* 9c */ 4, /* 9d */ 5, /* 9e */ 5, /* 9f */ 6,
00437             /* a0 */ 2, /* a1 */ 3, /* a2 */ 3, /* a3 */ 4,
00438             /* a4 */ 3, /* a5 */ 4, /* a6 */ 4, /* a7 */ 5,
00439             /* a8 */ 3, /* a9 */ 4, /* aa */ 4, /* ab */ 5,
00440             /* ac */ 4, /* ad */ 5, /* ae */ 5, /* af */ 6,
00441             /* b0 */ 3, /* b1 */ 4, /* b2 */ 4, /* b3 */ 5,
00442             /* b4 */ 4, /* b5 */ 5, /* b6 */ 5, /* b7 */ 6,
00443             /* b8 */ 4, /* b9 */ 5, /* ba */ 5, /* bb */ 6,
00444             /* bc */ 5, /* bd */ 6, /* be */ 6, /* bf */ 7,
00445             /* c0 */ 2, /* c1 */ 3, /* c2 */ 3, /* c3 */ 4,
00446             /* c4 */ 3, /* c5 */ 4, /* c6 */ 4, /* c7 */ 5,
00447             /* c8 */ 3, /* c9 */ 4, /* ca */ 4, /* cb */ 5,
00448             /* cc */ 4, /* cd */ 5, /* ce */ 5, /* cf */ 6,
00449             /* d0 */ 3, /* d1 */ 4, /* d2 */ 4, /* d3 */ 5,
00450             /* d4 */ 4, /* d5 */ 5, /* d6 */ 5, /* d7 */ 6,
00451             /* d8 */ 4, /* d9 */ 5, /* da */ 5, /* db */ 6,
00452             /* dc */ 5, /* dd */ 6, /* de */ 6, /* df */ 7,
00453             /* e0 */ 3, /* e1 */ 4, /* e2 */ 4, /* e3 */ 5,
00454             /* e4 */ 4, /* e5 */ 5, /* e6 */ 5, /* e7 */ 6,
00455             /* e8 */ 4, /* e9 */ 5, /* ea */ 5, /* eb */ 6,
00456             /* ec */ 5, /* ed */ 6, /* ee */ 6, /* ef */ 7,
00457             /* f0 */ 4, /* f1 */ 5, /* f2 */ 5, /* f3 */ 6,
00458             /* f4 */ 5, /* f5 */ 6, /* f6 */ 6, /* f7 */ 7,
00459             /* f8 */ 5, /* f9 */ 6, /* fa */ 6, /* fb */ 7,
00460             /* fc */ 6, /* fd */ 7, /* fe */ 7, /* ff */ 8
00461         };
00462         return table[b];
00463     }
00464 };
00465 
00470 template<class T>
00471 struct HammingPopcnt
00472 {
00473     typedef T ElementType;
00474     typedef int ResultType;
00475 
00476     template<typename Iterator1, typename Iterator2>
00477     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
00478     {
00479         ResultType result = 0;
00480 #if __GNUC__
00481 #if ANDROID && HAVE_NEON
00482         static uint64_t features = android_getCpuFeatures();
00483         if ((features& ANDROID_CPU_ARM_FEATURE_NEON)) {
00484             for (size_t i = 0; i < size; i += 16) {
00485                 uint8x16_t A_vec = vld1q_u8 (a + i);
00486                 uint8x16_t B_vec = vld1q_u8 (b + i);
00487                 //uint8x16_t veorq_u8 (uint8x16_t, uint8x16_t)
00488                 uint8x16_t AxorB = veorq_u8 (A_vec, B_vec);
00489 
00490                 uint8x16_t bitsSet += vcntq_u8 (AxorB);
00491                 //uint16x8_t vpadalq_u8 (uint16x8_t, uint8x16_t)
00492                 uint16x8_t bitSet8 = vpaddlq_u8 (bitsSet);
00493                 uint32x4_t bitSet4 = vpaddlq_u16 (bitSet8);
00494 
00495                 uint64x2_t bitSet2 = vpaddlq_u32 (bitSet4);
00496                 result += vgetq_lane_u64 (bitSet2,0);
00497                 result += vgetq_lane_u64 (bitSet2,1);
00498             }
00499         }
00500         else
00501 #endif
00502         //for portability just use unsigned long -- and use the __builtin_popcountll (see docs for __builtin_popcountll)
00503         typedef unsigned long long pop_t;
00504         const size_t modulo = size % sizeof(pop_t);
00505         const pop_t* a2 = reinterpret_cast<const pop_t*> (a);
00506         const pop_t* b2 = reinterpret_cast<const pop_t*> (b);
00507         const pop_t* a2_end = a2 + (size / sizeof(pop_t));
00508 
00509         for (; a2 != a2_end; ++a2, ++b2) result += __builtin_popcountll((*a2) ^ (*b2));
00510 
00511         if (modulo) {
00512             //in the case where size is not dividable by sizeof(size_t)
00513             //need to mask off the bits at the end
00514             pop_t a_final = 0, b_final = 0;
00515             memcpy(&a_final, a2, modulo);
00516             memcpy(&b_final, b2, modulo);
00517             result += __builtin_popcountll(a_final ^ b_final);
00518         }
00519 #else
00520         HammingLUT lut;
00521         result = lut(reinterpret_cast<const unsigned char*> (a),
00522                      reinterpret_cast<const unsigned char*> (b), size * sizeof(pop_t));
00523 #endif
00524         return result;
00525     }
00526 };
00527 
00528 template<typename T>
00529 struct Hamming
00530 {
00531     typedef T ElementType;
00532     typedef unsigned int ResultType;
00533 
00536     unsigned int popcnt32(uint32_t n) const
00537     {
00538         n -= ((n >> 1) & 0x55555555);
00539         n = (n & 0x33333333) + ((n >> 2) & 0x33333333);
00540         return (((n + (n >> 4))& 0xF0F0F0F)* 0x1010101) >> 24;
00541     }
00542 
00543     unsigned int popcnt64(uint64_t n) const
00544     {
00545         n -= ((n >> 1) & 0x5555555555555555LL);
00546         n = (n & 0x3333333333333333LL) + ((n >> 2) & 0x3333333333333333LL);
00547         return (((n + (n >> 4))& 0x0f0f0f0f0f0f0f0fLL)* 0x0101010101010101LL) >> 56;
00548     }
00549 
00550     template <typename Iterator1, typename Iterator2>
00551     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = 0) const
00552     {
00553 #ifdef FLANN_PLATFORM_64_BIT
00554         const uint64_t* pa = reinterpret_cast<const uint64_t*>(a);
00555         const uint64_t* pb = reinterpret_cast<const uint64_t*>(b);
00556         ResultType result = 0;
00557         size /= (sizeof(uint64_t)/sizeof(unsigned char));
00558         for(size_t i = 0; i < size; ++i ) {
00559             result += popcnt64(*pa ^ *pb);
00560             ++pa;
00561             ++pb;
00562         }
00563 #else
00564         const uint32_t* pa = reinterpret_cast<const uint32_t*>(a);
00565         const uint32_t* pb = reinterpret_cast<const uint32_t*>(b);
00566         ResultType result = 0;
00567         size /= (sizeof(uint32_t)/sizeof(unsigned char));
00568         for(size_t i = 0; i < size; ++i ) {
00569                 result += popcnt32(*pa ^ *pb);
00570                 ++pa;
00571                 ++pb;
00572         }
00573 #endif
00574         return result;
00575     }
00576 };
00577 
00578 
00579 
00581 
00582 template<class T>
00583 struct HistIntersectionDistance
00584 {
00585     typedef bool is_kdtree_distance;
00586 
00587     typedef T ElementType;
00588     typedef typename Accumulator<T>::Type ResultType;
00589 
00593     template <typename Iterator1, typename Iterator2>
00594     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
00595     {
00596         ResultType result = ResultType();
00597         ResultType min0, min1, min2, min3;
00598         Iterator1 last = a + size;
00599         Iterator1 lastgroup = last - 3;
00600 
00601         /* Process 4 items with each loop for efficiency. */
00602         while (a < lastgroup) {
00603             min0 = (ResultType)(a[0] < b[0] ? a[0] : b[0]);
00604             min1 = (ResultType)(a[1] < b[1] ? a[1] : b[1]);
00605             min2 = (ResultType)(a[2] < b[2] ? a[2] : b[2]);
00606             min3 = (ResultType)(a[3] < b[3] ? a[3] : b[3]);
00607             result += min0 + min1 + min2 + min3;
00608             a += 4;
00609             b += 4;
00610             if ((worst_dist>0)&&(result>worst_dist)) {
00611                 return result;
00612             }
00613         }
00614         /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
00615         while (a < last) {
00616             min0 = (ResultType)(*a < *b ? *a : *b);
00617             result += min0;
00618             ++a;
00619             ++b;
00620         }
00621         return result;
00622     }
00623 
00627     template <typename U, typename V>
00628     inline ResultType accum_dist(const U& a, const V& b, int) const
00629     {
00630         return a<b ? a : b;
00631     }
00632 };
00633 
00634 
00635 
00636 template<class T>
00637 struct HellingerDistance
00638 {
00639     typedef bool is_kdtree_distance;
00640 
00641     typedef T ElementType;
00642     typedef typename Accumulator<T>::Type ResultType;
00643 
00647     template <typename Iterator1, typename Iterator2>
00648     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
00649     {
00650         ResultType result = ResultType();
00651         ResultType diff0, diff1, diff2, diff3;
00652         Iterator1 last = a + size;
00653         Iterator1 lastgroup = last - 3;
00654 
00655         /* Process 4 items with each loop for efficiency. */
00656         while (a < lastgroup) {
00657             diff0 = sqrt(static_cast<ResultType>(a[0])) - sqrt(static_cast<ResultType>(b[0]));
00658             diff1 = sqrt(static_cast<ResultType>(a[1])) - sqrt(static_cast<ResultType>(b[1]));
00659             diff2 = sqrt(static_cast<ResultType>(a[2])) - sqrt(static_cast<ResultType>(b[2]));
00660             diff3 = sqrt(static_cast<ResultType>(a[3])) - sqrt(static_cast<ResultType>(b[3]));
00661             result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
00662             a += 4;
00663             b += 4;
00664         }
00665         while (a < last) {
00666             diff0 = sqrt(static_cast<ResultType>(*a++)) - sqrt(static_cast<ResultType>(*b++));
00667             result += diff0 * diff0;
00668         }
00669         return result;
00670     }
00671 
00675     template <typename U, typename V>
00676     inline ResultType accum_dist(const U& a, const V& b, int) const
00677     {
00678         ResultType dist = sqrt(static_cast<ResultType>(a)) - sqrt(static_cast<ResultType>(b));
00679         return dist * dist;
00680     }
00681 };
00682 
00683 
00684 template<class T>
00685 struct ChiSquareDistance
00686 {
00687     typedef bool is_kdtree_distance;
00688 
00689     typedef T ElementType;
00690     typedef typename Accumulator<T>::Type ResultType;
00691 
00695     template <typename Iterator1, typename Iterator2>
00696     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
00697     {
00698         ResultType result = ResultType();
00699         ResultType sum, diff;
00700         Iterator1 last = a + size;
00701 
00702         while (a < last) {
00703             sum = (ResultType)(*a + *b);
00704             if (sum>0) {
00705                 diff = (ResultType)(*a - *b);
00706                 result += diff*diff/sum;
00707             }
00708             ++a;
00709             ++b;
00710 
00711             if ((worst_dist>0)&&(result>worst_dist)) {
00712                 return result;
00713             }
00714         }
00715         return result;
00716     }
00717 
00721     template <typename U, typename V>
00722     inline ResultType accum_dist(const U& a, const V& b, int) const
00723     {
00724         ResultType result = ResultType();
00725         ResultType sum, diff;
00726 
00727         sum = (ResultType)(a+b);
00728         if (sum>0) {
00729             diff = (ResultType)(a-b);
00730             result = diff*diff/sum;
00731         }
00732         return result;
00733     }
00734 };
00735 
00736 
00737 template<class T>
00738 struct KL_Divergence
00739 {
00740     typedef bool is_kdtree_distance;
00741 
00742     typedef T ElementType;
00743     typedef typename Accumulator<T>::Type ResultType;
00744 
00748     template <typename Iterator1, typename Iterator2>
00749     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
00750     {
00751         ResultType result = ResultType();
00752         Iterator1 last = a + size;
00753 
00754         while (a < last) {
00755             if ( *a != 0 && *b != 0 ) {
00756                 ResultType ratio = (ResultType)(*a / *b);
00757                 if (ratio>0) {
00758                     result += *a * log(ratio);
00759                 }
00760             }
00761             ++a;
00762             ++b;
00763 
00764             if ((worst_dist>0)&&(result>worst_dist)) {
00765                 return result;
00766             }
00767         }
00768         return result;
00769     }
00770 
00774     template <typename U, typename V>
00775     inline ResultType accum_dist(const U& a, const V& b, int) const
00776     {
00777         ResultType result = ResultType();
00778         if( a != 0 && b != 0 ) {
00779             ResultType ratio = (ResultType)(a / b);
00780             if (ratio>0) {
00781                 result = a * log(ratio);
00782             }
00783                 }
00784         return result;
00785     }
00786 };
00787 
00788 }
00789 
00790 #endif //FLANN_DIST_H_


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:19