dist.h

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 OPENCV_FLANN_DIST_H_
00032 #define OPENCV_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 "defines.h"
00045 
00046 #if (defined WIN32 || defined _WIN32) && defined(_M_ARM)
00047 # include <Intrin.h>
00048 #endif
00049 
00050 #ifdef __ARM_NEON__
00051 # include "arm_neon.h"
00052 #endif
00053 
00054 namespace cvflann
00055 {
00056 
00057 template<typename T>
00058 inline T abs(T x) { return (x<0) ? -x : x; }
00059 
00060 template<>
00061 inline int abs<int>(int x) { return ::abs(x); }
00062 
00063 template<>
00064 inline float abs<float>(float x) { return fabsf(x); }
00065 
00066 template<>
00067 inline double abs<double>(double x) { return fabs(x); }
00068 
00069 template<typename T>
00070 struct Accumulator { typedef T Type; };
00071 template<>
00072 struct Accumulator<unsigned char>  { typedef float Type; };
00073 template<>
00074 struct Accumulator<unsigned short> { typedef float Type; };
00075 template<>
00076 struct Accumulator<unsigned int> { typedef float Type; };
00077 template<>
00078 struct Accumulator<char>   { typedef float Type; };
00079 template<>
00080 struct Accumulator<short>  { typedef float Type; };
00081 template<>
00082 struct Accumulator<int> { typedef float Type; };
00083 
00084 #undef True
00085 #undef False
00086 
00087 class True
00088 {
00089 };
00090 
00091 class False
00092 {
00093 };
00094 
00095 
00096 /**
00097  * Squared Euclidean distance functor.
00098  *
00099  * This is the simpler, unrolled version. This is preferable for
00100  * very low dimensionality data (eg 3D points)
00101  */
00102 template<class T>
00103 struct L2_Simple
00104 {
00105     typedef True is_kdtree_distance;
00106     typedef True is_vector_space_distance;
00107 
00108     typedef T ElementType;
00109     typedef typename Accumulator<T>::Type ResultType;
00110 
00111     template <typename Iterator1, typename Iterator2>
00112     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
00113     {
00114         ResultType result = ResultType();
00115         ResultType diff;
00116         for(size_t i = 0; i < size; ++i ) {
00117             diff = *a++ - *b++;
00118             result += diff*diff;
00119         }
00120         return result;
00121     }
00122 
00123     template <typename U, typename V>
00124     inline ResultType accum_dist(const U& a, const V& b, int) const
00125     {
00126         return (a-b)*(a-b);
00127     }
00128 };
00129 
00130 
00131 
00132 /**
00133  * Squared Euclidean distance functor, optimized version
00134  */
00135 template<class T>
00136 struct L2
00137 {
00138     typedef True is_kdtree_distance;
00139     typedef True is_vector_space_distance;
00140 
00141     typedef T ElementType;
00142     typedef typename Accumulator<T>::Type ResultType;
00143 
00144     /**
00145      *  Compute the squared Euclidean distance between two vectors.
00146      *
00147      *  This is highly optimised, with loop unrolling, as it is one
00148      *  of the most expensive inner loops.
00149      *
00150      *  The computation of squared root at the end is omitted for
00151      *  efficiency.
00152      */
00153     template <typename Iterator1, typename Iterator2>
00154     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
00155     {
00156         ResultType result = ResultType();
00157         ResultType diff0, diff1, diff2, diff3;
00158         Iterator1 last = a + size;
00159         Iterator1 lastgroup = last - 3;
00160 
00161         /* Process 4 items with each loop for efficiency. */
00162         while (a < lastgroup) {
00163             diff0 = (ResultType)(a[0] - b[0]);
00164             diff1 = (ResultType)(a[1] - b[1]);
00165             diff2 = (ResultType)(a[2] - b[2]);
00166             diff3 = (ResultType)(a[3] - b[3]);
00167             result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
00168             a += 4;
00169             b += 4;
00170 
00171             if ((worst_dist>0)&&(result>worst_dist)) {
00172                 return result;
00173             }
00174         }
00175         /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
00176         while (a < last) {
00177             diff0 = (ResultType)(*a++ - *b++);
00178             result += diff0 * diff0;
00179         }
00180         return result;
00181     }
00182 
00183     /**
00184      *  Partial euclidean distance, using just one dimension. This is used by the
00185      *  kd-tree when computing partial distances while traversing the tree.
00186      *
00187      *  Squared root is omitted for efficiency.
00188      */
00189     template <typename U, typename V>
00190     inline ResultType accum_dist(const U& a, const V& b, int) const
00191     {
00192         return (a-b)*(a-b);
00193     }
00194 };
00195 
00196 
00197 /*
00198  * Manhattan distance functor, optimized version
00199  */
00200 template<class T>
00201 struct L1
00202 {
00203     typedef True is_kdtree_distance;
00204     typedef True is_vector_space_distance;
00205 
00206     typedef T ElementType;
00207     typedef typename Accumulator<T>::Type ResultType;
00208 
00209     /**
00210      *  Compute the Manhattan (L_1) distance between two vectors.
00211      *
00212      *  This is highly optimised, with loop unrolling, as it is one
00213      *  of the most expensive inner loops.
00214      */
00215     template <typename Iterator1, typename Iterator2>
00216     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
00217     {
00218         ResultType result = ResultType();
00219         ResultType diff0, diff1, diff2, diff3;
00220         Iterator1 last = a + size;
00221         Iterator1 lastgroup = last - 3;
00222 
00223         /* Process 4 items with each loop for efficiency. */
00224         while (a < lastgroup) {
00225             diff0 = (ResultType)abs(a[0] - b[0]);
00226             diff1 = (ResultType)abs(a[1] - b[1]);
00227             diff2 = (ResultType)abs(a[2] - b[2]);
00228             diff3 = (ResultType)abs(a[3] - b[3]);
00229             result += diff0 + diff1 + diff2 + diff3;
00230             a += 4;
00231             b += 4;
00232 
00233             if ((worst_dist>0)&&(result>worst_dist)) {
00234                 return result;
00235             }
00236         }
00237         /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
00238         while (a < last) {
00239             diff0 = (ResultType)abs(*a++ - *b++);
00240             result += diff0;
00241         }
00242         return result;
00243     }
00244 
00245     /**
00246      * Partial distance, used by the kd-tree.
00247      */
00248     template <typename U, typename V>
00249     inline ResultType accum_dist(const U& a, const V& b, int) const
00250     {
00251         return abs(a-b);
00252     }
00253 };
00254 
00255 
00256 
00257 template<class T>
00258 struct MinkowskiDistance
00259 {
00260     typedef True is_kdtree_distance;
00261     typedef True is_vector_space_distance;
00262 
00263     typedef T ElementType;
00264     typedef typename Accumulator<T>::Type ResultType;
00265 
00266     int order;
00267 
00268     MinkowskiDistance(int order_) : order(order_) {}
00269 
00270     /**
00271      *  Compute the Minkowsky (L_p) distance between two vectors.
00272      *
00273      *  This is highly optimised, with loop unrolling, as it is one
00274      *  of the most expensive inner loops.
00275      *
00276      *  The computation of squared root at the end is omitted for
00277      *  efficiency.
00278      */
00279     template <typename Iterator1, typename Iterator2>
00280     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
00281     {
00282         ResultType result = ResultType();
00283         ResultType diff0, diff1, diff2, diff3;
00284         Iterator1 last = a + size;
00285         Iterator1 lastgroup = last - 3;
00286 
00287         /* Process 4 items with each loop for efficiency. */
00288         while (a < lastgroup) {
00289             diff0 = (ResultType)abs(a[0] - b[0]);
00290             diff1 = (ResultType)abs(a[1] - b[1]);
00291             diff2 = (ResultType)abs(a[2] - b[2]);
00292             diff3 = (ResultType)abs(a[3] - b[3]);
00293             result += pow(diff0,order) + pow(diff1,order) + pow(diff2,order) + pow(diff3,order);
00294             a += 4;
00295             b += 4;
00296 
00297             if ((worst_dist>0)&&(result>worst_dist)) {
00298                 return result;
00299             }
00300         }
00301         /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
00302         while (a < last) {
00303             diff0 = (ResultType)abs(*a++ - *b++);
00304             result += pow(diff0,order);
00305         }
00306         return result;
00307     }
00308 
00309     /**
00310      * Partial distance, used by the kd-tree.
00311      */
00312     template <typename U, typename V>
00313     inline ResultType accum_dist(const U& a, const V& b, int) const
00314     {
00315         return pow(static_cast<ResultType>(abs(a-b)),order);
00316     }
00317 };
00318 
00319 
00320 
00321 template<class T>
00322 struct MaxDistance
00323 {
00324     typedef False is_kdtree_distance;
00325     typedef True is_vector_space_distance;
00326 
00327     typedef T ElementType;
00328     typedef typename Accumulator<T>::Type ResultType;
00329 
00330     /**
00331      *  Compute the max distance (L_infinity) between two vectors.
00332      *
00333      *  This distance is not a valid kdtree distance, it's not dimensionwise additive.
00334      */
00335     template <typename Iterator1, typename Iterator2>
00336     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
00337     {
00338         ResultType result = ResultType();
00339         ResultType diff0, diff1, diff2, diff3;
00340         Iterator1 last = a + size;
00341         Iterator1 lastgroup = last - 3;
00342 
00343         /* Process 4 items with each loop for efficiency. */
00344         while (a < lastgroup) {
00345             diff0 = abs(a[0] - b[0]);
00346             diff1 = abs(a[1] - b[1]);
00347             diff2 = abs(a[2] - b[2]);
00348             diff3 = abs(a[3] - b[3]);
00349             if (diff0>result) {result = diff0; }
00350             if (diff1>result) {result = diff1; }
00351             if (diff2>result) {result = diff2; }
00352             if (diff3>result) {result = diff3; }
00353             a += 4;
00354             b += 4;
00355 
00356             if ((worst_dist>0)&&(result>worst_dist)) {
00357                 return result;
00358             }
00359         }
00360         /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
00361         while (a < last) {
00362             diff0 = abs(*a++ - *b++);
00363             result = (diff0>result) ? diff0 : result;
00364         }
00365         return result;
00366     }
00367 
00368     /* This distance functor is not dimension-wise additive, which
00369      * makes it an invalid kd-tree distance, not implementing the accum_dist method */
00370 
00371 };
00372 
00373 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00374 
00375 /**
00376  * Hamming distance functor - counts the bit differences between two strings - useful for the Brief descriptor
00377  * bit count of A exclusive XOR'ed with B
00378  */
00379 struct HammingLUT
00380 {
00381     typedef False is_kdtree_distance;
00382     typedef False is_vector_space_distance;
00383 
00384     typedef unsigned char ElementType;
00385     typedef int ResultType;
00386 
00387     /** this will count the bits in a ^ b
00388      */
00389     ResultType operator()(const unsigned char* a, const unsigned char* b, size_t size) const
00390     {
00391         static const uchar popCountTable[] =
00392         {
00393             0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4, 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5,
00394             1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
00395             1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
00396             2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
00397             1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
00398             2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
00399             2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
00400             3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 4, 5, 5, 6, 5, 6, 6, 7, 5, 6, 6, 7, 6, 7, 7, 8
00401         };
00402         ResultType result = 0;
00403         for (size_t i = 0; i < size; i++) {
00404             result += popCountTable[a[i] ^ b[i]];
00405         }
00406         return result;
00407     }
00408 };
00409 
00410 /**
00411  * Hamming distance functor (pop count between two binary vectors, i.e. xor them and count the number of bits set)
00412  * That code was taken from brief.cpp in OpenCV
00413  */
00414 template<class T>
00415 struct Hamming
00416 {
00417     typedef False is_kdtree_distance;
00418     typedef False is_vector_space_distance;
00419 
00420 
00421     typedef T ElementType;
00422     typedef int ResultType;
00423 
00424     template<typename Iterator1, typename Iterator2>
00425     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
00426     {
00427         ResultType result = 0;
00428 #ifdef __ARM_NEON__
00429         {
00430             uint32x4_t bits = vmovq_n_u32(0);
00431             for (size_t i = 0; i < size; i += 16) {
00432                 uint8x16_t A_vec = vld1q_u8 (a + i);
00433                 uint8x16_t B_vec = vld1q_u8 (b + i);
00434                 uint8x16_t AxorB = veorq_u8 (A_vec, B_vec);
00435                 uint8x16_t bitsSet = vcntq_u8 (AxorB);
00436                 uint16x8_t bitSet8 = vpaddlq_u8 (bitsSet);
00437                 uint32x4_t bitSet4 = vpaddlq_u16 (bitSet8);
00438                 bits = vaddq_u32(bits, bitSet4);
00439             }
00440             uint64x2_t bitSet2 = vpaddlq_u32 (bits);
00441             result = vgetq_lane_s32 (vreinterpretq_s32_u64(bitSet2),0);
00442             result += vgetq_lane_s32 (vreinterpretq_s32_u64(bitSet2),2);
00443         }
00444 #elif __GNUC__
00445         {
00446             //for portability just use unsigned long -- and use the __builtin_popcountll (see docs for __builtin_popcountll)
00447             typedef unsigned long long pop_t;
00448             const size_t modulo = size % sizeof(pop_t);
00449             const pop_t* a2 = reinterpret_cast<const pop_t*> (a);
00450             const pop_t* b2 = reinterpret_cast<const pop_t*> (b);
00451             const pop_t* a2_end = a2 + (size / sizeof(pop_t));
00452 
00453             for (; a2 != a2_end; ++a2, ++b2) result += __builtin_popcountll((*a2) ^ (*b2));
00454 
00455             if (modulo) {
00456                 //in the case where size is not dividable by sizeof(size_t)
00457                 //need to mask off the bits at the end
00458                 pop_t a_final = 0, b_final = 0;
00459                 memcpy(&a_final, a2, modulo);
00460                 memcpy(&b_final, b2, modulo);
00461                 result += __builtin_popcountll(a_final ^ b_final);
00462             }
00463         }
00464 #else // NO NEON and NOT GNUC
00465         typedef unsigned long long pop_t;
00466         HammingLUT lut;
00467         result = lut(reinterpret_cast<const unsigned char*> (a),
00468                      reinterpret_cast<const unsigned char*> (b), size * sizeof(pop_t));
00469 #endif
00470         return result;
00471     }
00472 };
00473 
00474 template<typename T>
00475 struct Hamming2
00476 {
00477     typedef False is_kdtree_distance;
00478     typedef False is_vector_space_distance;
00479 
00480     typedef T ElementType;
00481     typedef int ResultType;
00482 
00483     /** This is popcount_3() from:
00484      * http://en.wikipedia.org/wiki/Hamming_weight */
00485     unsigned int popcnt32(uint32_t n) const
00486     {
00487         n -= ((n >> 1) & 0x55555555);
00488         n = (n & 0x33333333) + ((n >> 2) & 0x33333333);
00489         return (((n + (n >> 4))& 0xF0F0F0F)* 0x1010101) >> 24;
00490     }
00491 
00492 #ifdef FLANN_PLATFORM_64_BIT
00493     unsigned int popcnt64(uint64_t n) const
00494     {
00495         n -= ((n >> 1) & 0x5555555555555555);
00496         n = (n & 0x3333333333333333) + ((n >> 2) & 0x3333333333333333);
00497         return (((n + (n >> 4))& 0x0f0f0f0f0f0f0f0f)* 0x0101010101010101) >> 56;
00498     }
00499 #endif
00500 
00501     template <typename Iterator1, typename Iterator2>
00502     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
00503     {
00504 #ifdef FLANN_PLATFORM_64_BIT
00505         const uint64_t* pa = reinterpret_cast<const uint64_t*>(a);
00506         const uint64_t* pb = reinterpret_cast<const uint64_t*>(b);
00507         ResultType result = 0;
00508         size /= (sizeof(uint64_t)/sizeof(unsigned char));
00509         for(size_t i = 0; i < size; ++i ) {
00510             result += popcnt64(*pa ^ *pb);
00511             ++pa;
00512             ++pb;
00513         }
00514 #else
00515         const uint32_t* pa = reinterpret_cast<const uint32_t*>(a);
00516         const uint32_t* pb = reinterpret_cast<const uint32_t*>(b);
00517         ResultType result = 0;
00518         size /= (sizeof(uint32_t)/sizeof(unsigned char));
00519         for(size_t i = 0; i < size; ++i ) {
00520             result += popcnt32(*pa ^ *pb);
00521             ++pa;
00522             ++pb;
00523         }
00524 #endif
00525         return result;
00526     }
00527 };
00528 
00529 
00530 
00531 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00532 
00533 template<class T>
00534 struct HistIntersectionDistance
00535 {
00536     typedef True is_kdtree_distance;
00537     typedef True is_vector_space_distance;
00538 
00539     typedef T ElementType;
00540     typedef typename Accumulator<T>::Type ResultType;
00541 
00542     /**
00543      *  Compute the histogram intersection distance
00544      */
00545     template <typename Iterator1, typename Iterator2>
00546     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
00547     {
00548         ResultType result = ResultType();
00549         ResultType min0, min1, min2, min3;
00550         Iterator1 last = a + size;
00551         Iterator1 lastgroup = last - 3;
00552 
00553         /* Process 4 items with each loop for efficiency. */
00554         while (a < lastgroup) {
00555             min0 = (ResultType)(a[0] < b[0] ? a[0] : b[0]);
00556             min1 = (ResultType)(a[1] < b[1] ? a[1] : b[1]);
00557             min2 = (ResultType)(a[2] < b[2] ? a[2] : b[2]);
00558             min3 = (ResultType)(a[3] < b[3] ? a[3] : b[3]);
00559             result += min0 + min1 + min2 + min3;
00560             a += 4;
00561             b += 4;
00562             if ((worst_dist>0)&&(result>worst_dist)) {
00563                 return result;
00564             }
00565         }
00566         /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
00567         while (a < last) {
00568             min0 = (ResultType)(*a < *b ? *a : *b);
00569             result += min0;
00570             ++a;
00571             ++b;
00572         }
00573         return result;
00574     }
00575 
00576     /**
00577      * Partial distance, used by the kd-tree.
00578      */
00579     template <typename U, typename V>
00580     inline ResultType accum_dist(const U& a, const V& b, int) const
00581     {
00582         return a<b ? a : b;
00583     }
00584 };
00585 
00586 
00587 
00588 template<class T>
00589 struct HellingerDistance
00590 {
00591     typedef True is_kdtree_distance;
00592     typedef True is_vector_space_distance;
00593 
00594     typedef T ElementType;
00595     typedef typename Accumulator<T>::Type ResultType;
00596 
00597     /**
00598      *  Compute the Hellinger distance
00599      */
00600     template <typename Iterator1, typename Iterator2>
00601     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
00602     {
00603         ResultType result = ResultType();
00604         ResultType diff0, diff1, diff2, diff3;
00605         Iterator1 last = a + size;
00606         Iterator1 lastgroup = last - 3;
00607 
00608         /* Process 4 items with each loop for efficiency. */
00609         while (a < lastgroup) {
00610             diff0 = sqrt(static_cast<ResultType>(a[0])) - sqrt(static_cast<ResultType>(b[0]));
00611             diff1 = sqrt(static_cast<ResultType>(a[1])) - sqrt(static_cast<ResultType>(b[1]));
00612             diff2 = sqrt(static_cast<ResultType>(a[2])) - sqrt(static_cast<ResultType>(b[2]));
00613             diff3 = sqrt(static_cast<ResultType>(a[3])) - sqrt(static_cast<ResultType>(b[3]));
00614             result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
00615             a += 4;
00616             b += 4;
00617         }
00618         while (a < last) {
00619             diff0 = sqrt(static_cast<ResultType>(*a++)) - sqrt(static_cast<ResultType>(*b++));
00620             result += diff0 * diff0;
00621         }
00622         return result;
00623     }
00624 
00625     /**
00626      * Partial distance, used by the kd-tree.
00627      */
00628     template <typename U, typename V>
00629     inline ResultType accum_dist(const U& a, const V& b, int) const
00630     {
00631         ResultType diff = sqrt(static_cast<ResultType>(a)) - sqrt(static_cast<ResultType>(b));
00632         return diff * diff;
00633     }
00634 };
00635 
00636 
00637 template<class T>
00638 struct ChiSquareDistance
00639 {
00640     typedef True is_kdtree_distance;
00641     typedef True is_vector_space_distance;
00642 
00643     typedef T ElementType;
00644     typedef typename Accumulator<T>::Type ResultType;
00645 
00646     /**
00647      *  Compute the chi-square distance
00648      */
00649     template <typename Iterator1, typename Iterator2>
00650     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
00651     {
00652         ResultType result = ResultType();
00653         ResultType sum, diff;
00654         Iterator1 last = a + size;
00655 
00656         while (a < last) {
00657             sum = (ResultType)(*a + *b);
00658             if (sum>0) {
00659                 diff = (ResultType)(*a - *b);
00660                 result += diff*diff/sum;
00661             }
00662             ++a;
00663             ++b;
00664 
00665             if ((worst_dist>0)&&(result>worst_dist)) {
00666                 return result;
00667             }
00668         }
00669         return result;
00670     }
00671 
00672     /**
00673      * Partial distance, used by the kd-tree.
00674      */
00675     template <typename U, typename V>
00676     inline ResultType accum_dist(const U& a, const V& b, int) const
00677     {
00678         ResultType result = ResultType();
00679         ResultType sum, diff;
00680 
00681         sum = (ResultType)(a+b);
00682         if (sum>0) {
00683             diff = (ResultType)(a-b);
00684             result = diff*diff/sum;
00685         }
00686         return result;
00687     }
00688 };
00689 
00690 
00691 template<class T>
00692 struct KL_Divergence
00693 {
00694     typedef True is_kdtree_distance;
00695     typedef True is_vector_space_distance;
00696 
00697     typedef T ElementType;
00698     typedef typename Accumulator<T>::Type ResultType;
00699 
00700     /**
00701      *  Compute the Kullback–Leibler divergence
00702      */
00703     template <typename Iterator1, typename Iterator2>
00704     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
00705     {
00706         ResultType result = ResultType();
00707         Iterator1 last = a + size;
00708 
00709         while (a < last) {
00710             if (* b != 0) {
00711                 ResultType ratio = (ResultType)(*a / *b);
00712                 if (ratio>0) {
00713                     result += *a * log(ratio);
00714                 }
00715             }
00716             ++a;
00717             ++b;
00718 
00719             if ((worst_dist>0)&&(result>worst_dist)) {
00720                 return result;
00721             }
00722         }
00723         return result;
00724     }
00725 
00726     /**
00727      * Partial distance, used by the kd-tree.
00728      */
00729     template <typename U, typename V>
00730     inline ResultType accum_dist(const U& a, const V& b, int) const
00731     {
00732         ResultType result = ResultType();
00733         if( *b != 0 ) {
00734             ResultType ratio = (ResultType)(a / b);
00735             if (ratio>0) {
00736                 result = a * log(ratio);
00737             }
00738         }
00739         return result;
00740     }
00741 };
00742 
00743 
00744 
00745 /*
00746  * This is a "zero iterator". It basically behaves like a zero filled
00747  * array to all algorithms that use arrays as iterators (STL style).
00748  * It's useful when there's a need to compute the distance between feature
00749  * and origin it and allows for better compiler optimisation than using a
00750  * zero-filled array.
00751  */
00752 template <typename T>
00753 struct ZeroIterator
00754 {
00755 
00756     T operator*()
00757     {
00758         return 0;
00759     }
00760 
00761     T operator[](int)
00762     {
00763         return 0;
00764     }
00765 
00766     const ZeroIterator<T>& operator ++()
00767     {
00768         return *this;
00769     }
00770 
00771     ZeroIterator<T> operator ++(int)
00772     {
00773         return *this;
00774     }
00775 
00776     ZeroIterator<T>& operator+=(int)
00777     {
00778         return *this;
00779     }
00780 
00781 };
00782 
00783 
00784 /*
00785  * Depending on processed distances, some of them are already squared (e.g. L2)
00786  * and some are not (e.g.Hamming). In KMeans++ for instance we want to be sure
00787  * we are working on ^2 distances, thus following templates to ensure that.
00788  */
00789 template <typename Distance, typename ElementType>
00790 struct squareDistance
00791 {
00792     typedef typename Distance::ResultType ResultType;
00793     ResultType operator()( ResultType dist ) { return dist*dist; }
00794 };
00795 
00796 
00797 template <typename ElementType>
00798 struct squareDistance<L2_Simple<ElementType>, ElementType>
00799 {
00800     typedef typename L2_Simple<ElementType>::ResultType ResultType;
00801     ResultType operator()( ResultType dist ) { return dist; }
00802 };
00803 
00804 template <typename ElementType>
00805 struct squareDistance<L2<ElementType>, ElementType>
00806 {
00807     typedef typename L2<ElementType>::ResultType ResultType;
00808     ResultType operator()( ResultType dist ) { return dist; }
00809 };
00810 
00811 
00812 template <typename ElementType>
00813 struct squareDistance<MinkowskiDistance<ElementType>, ElementType>
00814 {
00815     typedef typename MinkowskiDistance<ElementType>::ResultType ResultType;
00816     ResultType operator()( ResultType dist ) { return dist; }
00817 };
00818 
00819 template <typename ElementType>
00820 struct squareDistance<HellingerDistance<ElementType>, ElementType>
00821 {
00822     typedef typename HellingerDistance<ElementType>::ResultType ResultType;
00823     ResultType operator()( ResultType dist ) { return dist; }
00824 };
00825 
00826 template <typename ElementType>
00827 struct squareDistance<ChiSquareDistance<ElementType>, ElementType>
00828 {
00829     typedef typename ChiSquareDistance<ElementType>::ResultType ResultType;
00830     ResultType operator()( ResultType dist ) { return dist; }
00831 };
00832 
00833 
00834 template <typename Distance>
00835 typename Distance::ResultType ensureSquareDistance( typename Distance::ResultType dist )
00836 {
00837     typedef typename Distance::ElementType ElementType;
00838 
00839     squareDistance<Distance, ElementType> dummy;
00840     return dummy( dist );
00841 }
00842 
00843 
00844 /*
00845  * ...and a template to ensure the user that he will process the normal distance,
00846  * and not squared distance, without loosing processing time calling sqrt(ensureSquareDistance)
00847  * that will result in doing actually sqrt(dist*dist) for L1 distance for instance.
00848  */
00849 template <typename Distance, typename ElementType>
00850 struct simpleDistance
00851 {
00852     typedef typename Distance::ResultType ResultType;
00853     ResultType operator()( ResultType dist ) { return dist; }
00854 };
00855 
00856 
00857 template <typename ElementType>
00858 struct simpleDistance<L2_Simple<ElementType>, ElementType>
00859 {
00860     typedef typename L2_Simple<ElementType>::ResultType ResultType;
00861     ResultType operator()( ResultType dist ) { return sqrt(dist); }
00862 };
00863 
00864 template <typename ElementType>
00865 struct simpleDistance<L2<ElementType>, ElementType>
00866 {
00867     typedef typename L2<ElementType>::ResultType ResultType;
00868     ResultType operator()( ResultType dist ) { return sqrt(dist); }
00869 };
00870 
00871 
00872 template <typename ElementType>
00873 struct simpleDistance<MinkowskiDistance<ElementType>, ElementType>
00874 {
00875     typedef typename MinkowskiDistance<ElementType>::ResultType ResultType;
00876     ResultType operator()( ResultType dist ) { return sqrt(dist); }
00877 };
00878 
00879 template <typename ElementType>
00880 struct simpleDistance<HellingerDistance<ElementType>, ElementType>
00881 {
00882     typedef typename HellingerDistance<ElementType>::ResultType ResultType;
00883     ResultType operator()( ResultType dist ) { return sqrt(dist); }
00884 };
00885 
00886 template <typename ElementType>
00887 struct simpleDistance<ChiSquareDistance<ElementType>, ElementType>
00888 {
00889     typedef typename ChiSquareDistance<ElementType>::ResultType ResultType;
00890     ResultType operator()( ResultType dist ) { return sqrt(dist); }
00891 };
00892 
00893 
00894 template <typename Distance>
00895 typename Distance::ResultType ensureSimpleDistance( typename Distance::ResultType dist )
00896 {
00897     typedef typename Distance::ElementType ElementType;
00898 
00899     simpleDistance<Distance, ElementType> dummy;
00900     return dummy( dist );
00901 }
00902 
00903 }
00904 
00905 #endif //OPENCV_FLANN_DIST_H_