modules/flann/include/opencv2/flann/dist.h

   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  *
   7  * THE BSD LICENSE
   8  *
   9  * Redistribution and use in source and binary forms, with or without
  10  * modification, are permitted provided that the following conditions
  11  * are met:
  12  *
  13  * 1. Redistributions of source code must retain the above copyright
  14  *    notice, this list of conditions and the following disclaimer.
  15  * 2. Redistributions in binary form must reproduce the above copyright
  16  *    notice, this list of conditions and the following disclaimer in the
  17  *    documentation and/or other materials provided with the distribution.
  18  *
  19  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
  20  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
  21  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
  22  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
  23  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
  24  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  25  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  26  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  27  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  28  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  29  *************************************************************************/
  30
  31 #ifndef OPENCV_FLANN_DIST_H_
  32 #define OPENCV_FLANN_DIST_H_
  33
  34 //! @cond IGNORED
  35
  36 #include <cmath>
  37 #include <cstdlib>
  38 #include <string.h>
  39 #ifdef _MSC_VER
  40 typedef unsigned __int32 uint32_t;
  41 typedef unsigned __int64 uint64_t;
  42 #else
  43 #include <stdint.h>
  44 #endif
  45
  46 #include "defines.h"
  47
  48 #if defined _WIN32 && (defined(_M_ARM) || defined(_M_ARM64))
  49 # include <Intrin.h>
  50 #endif
  51
  52 #if defined(__ARM_NEON__) && !defined(__CUDACC__)
  53 # include "arm_neon.h"
  54 #endif
  55
  56 namespace cvflann
  57 {
  58
  59 template<typename T>
  60 inline T abs(T x) { return (x<0) ? -x : x; }
  61
  62 template<>
  63 inline int abs<int>(int x) { return ::abs(x); }
  64
  65 template<>
  66 inline float abs<float>(float x) { return fabsf(x); }
  67
  68 template<>
  69 inline double abs<double>(double x) { return fabs(x); }
  70
  71 template<typename T>
  72 struct Accumulator { typedef T Type; };
  73 template<>
  74 struct Accumulator<unsigned char>  { typedef float Type; };
  75 template<>
  76 struct Accumulator<unsigned short> { typedef float Type; };
  77 template<>
  78 struct Accumulator<unsigned int> { typedef float Type; };
  79 template<>
  80 struct Accumulator<char>   { typedef float Type; };
  81 template<>
  82 struct Accumulator<short>  { typedef float Type; };
  83 template<>
  84 struct Accumulator<int> { typedef float Type; };
  85
  86 #undef True
  87 #undef False
  88
  89 class True
  90 {
  91 };
  92
  93 class False
  94 {
  95 };
  96
  97
  98 /**
  99  * Squared Euclidean distance functor.
 100  *
 101  * This is the simpler, unrolled version. This is preferable for
 102  * very low dimensionality data (eg 3D points)
 103  */
 104 template<class T>
 105 struct L2_Simple
 106 {
 107     typedef True is_kdtree_distance;
 108     typedef True is_vector_space_distance;
 109
 110     typedef T ElementType;
 111     typedef typename Accumulator<T>::Type ResultType;
 112
 113     template <typename Iterator1, typename Iterator2>
 114     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
 115     {
 116         ResultType result = ResultType();
 117         ResultType diff;
 118         for(size_t i = 0; i < size; ++i ) {
 119             diff = (ResultType)(*a++ - *b++);
 120             result += diff*diff;
 121         }
 122         return result;
 123     }
 124
 125     template <typename U, typename V>
 126     inline ResultType accum_dist(const U& a, const V& b, int) const
 127     {
 128         return (a-b)*(a-b);
 129     }
 130 };
 131
 132
 133
 134 /**
 135  * Squared Euclidean distance functor, optimized version
 136  */
 137 template<class T>
 138 struct L2
 139 {
 140     typedef True is_kdtree_distance;
 141     typedef True is_vector_space_distance;
 142
 143     typedef T ElementType;
 144     typedef typename Accumulator<T>::Type ResultType;
 145
 146     /**
 147      *  Compute the squared Euclidean distance between two vectors.
 148      *
 149      *  This is highly optimised, with loop unrolling, as it is one
 150      *  of the most expensive inner loops.
 151      *
 152      *  The computation of squared root at the end is omitted for
 153      *  efficiency.
 154      */
 155     template <typename Iterator1, typename Iterator2>
 156     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
 157     {
 158         ResultType result = ResultType();
 159         ResultType diff0, diff1, diff2, diff3;
 160         Iterator1 last = a + size;
 161         Iterator1 lastgroup = last - 3;
 162
 163         /* Process 4 items with each loop for efficiency. */
 164         while (a < lastgroup) {
 165             diff0 = (ResultType)(a[0] - b[0]);
 166             diff1 = (ResultType)(a[1] - b[1]);
 167             diff2 = (ResultType)(a[2] - b[2]);
 168             diff3 = (ResultType)(a[3] - b[3]);
 169             result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
 170             a += 4;
 171             b += 4;
 172
 173             if ((worst_dist>0)&&(result>worst_dist)) {
 174                 return result;
 175             }
 176         }
 177         /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
 178         while (a < last) {
 179             diff0 = (ResultType)(*a++ - *b++);
 180             result += diff0 * diff0;
 181         }
 182         return result;
 183     }
 184
 185     /**
 186      *  Partial euclidean distance, using just one dimension. This is used by the
 187      *  kd-tree when computing partial distances while traversing the tree.
 188      *
 189      *  Squared root is omitted for efficiency.
 190      */
 191     template <typename U, typename V>
 192     inline ResultType accum_dist(const U& a, const V& b, int) const
 193     {
 194         return (a-b)*(a-b);
 195     }
 196 };
 197
 198
 199 /*
 200  * Manhattan distance functor, optimized version
 201  */
 202 template<class T>
 203 struct L1
 204 {
 205     typedef True is_kdtree_distance;
 206     typedef True is_vector_space_distance;
 207
 208     typedef T ElementType;
 209     typedef typename Accumulator<T>::Type ResultType;
 210
 211     /**
 212      *  Compute the Manhattan (L_1) distance between two vectors.
 213      *
 214      *  This is highly optimised, with loop unrolling, as it is one
 215      *  of the most expensive inner loops.
 216      */
 217     template <typename Iterator1, typename Iterator2>
 218     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
 219     {
 220         ResultType result = ResultType();
 221         ResultType diff0, diff1, diff2, diff3;
 222         Iterator1 last = a + size;
 223         Iterator1 lastgroup = last - 3;
 224
 225         /* Process 4 items with each loop for efficiency. */
 226         while (a < lastgroup) {
 227             diff0 = (ResultType)abs(a[0] - b[0]);
 228             diff1 = (ResultType)abs(a[1] - b[1]);
 229             diff2 = (ResultType)abs(a[2] - b[2]);
 230             diff3 = (ResultType)abs(a[3] - b[3]);
 231             result += diff0 + diff1 + diff2 + diff3;
 232             a += 4;
 233             b += 4;
 234
 235             if ((worst_dist>0)&&(result>worst_dist)) {
 236                 return result;
 237             }
 238         }
 239         /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
 240         while (a < last) {
 241             diff0 = (ResultType)abs(*a++ - *b++);
 242             result += diff0;
 243         }
 244         return result;
 245     }
 246
 247     /**
 248      * Partial distance, used by the kd-tree.
 249      */
 250     template <typename U, typename V>
 251     inline ResultType accum_dist(const U& a, const V& b, int) const
 252     {
 253         return abs(a-b);
 254     }
 255 };
 256
 257
 258
 259 template<class T>
 260 struct MinkowskiDistance
 261 {
 262     typedef True is_kdtree_distance;
 263     typedef True is_vector_space_distance;
 264
 265     typedef T ElementType;
 266     typedef typename Accumulator<T>::Type ResultType;
 267
 268     int order;
 269
 270     MinkowskiDistance(int order_) : order(order_) {}
 271
 272     /**
 273      *  Compute the Minkowsky (L_p) distance between two vectors.
 274      *
 275      *  This is highly optimised, with loop unrolling, as it is one
 276      *  of the most expensive inner loops.
 277      *
 278      *  The computation of squared root at the end is omitted for
 279      *  efficiency.
 280      */
 281     template <typename Iterator1, typename Iterator2>
 282     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
 283     {
 284         ResultType result = ResultType();
 285         ResultType diff0, diff1, diff2, diff3;
 286         Iterator1 last = a + size;
 287         Iterator1 lastgroup = last - 3;
 288
 289         /* Process 4 items with each loop for efficiency. */
 290         while (a < lastgroup) {
 291             diff0 = (ResultType)abs(a[0] - b[0]);
 292             diff1 = (ResultType)abs(a[1] - b[1]);
 293             diff2 = (ResultType)abs(a[2] - b[2]);
 294             diff3 = (ResultType)abs(a[3] - b[3]);
 295             result += pow(diff0,order) + pow(diff1,order) + pow(diff2,order) + pow(diff3,order);
 296             a += 4;
 297             b += 4;
 298
 299             if ((worst_dist>0)&&(result>worst_dist)) {
 300                 return result;
 301             }
 302         }
 303         /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
 304         while (a < last) {
 305             diff0 = (ResultType)abs(*a++ - *b++);
 306             result += pow(diff0,order);
 307         }
 308         return result;
 309     }
 310
 311     /**
 312      * Partial distance, used by the kd-tree.
 313      */
 314     template <typename U, typename V>
 315     inline ResultType accum_dist(const U& a, const V& b, int) const
 316     {
 317         return pow(static_cast<ResultType>(abs(a-b)),order);
 318     }
 319 };
 320
 321
 322
 323 template<class T>
 324 struct MaxDistance
 325 {
 326     typedef False is_kdtree_distance;
 327     typedef True is_vector_space_distance;
 328
 329     typedef T ElementType;
 330     typedef typename Accumulator<T>::Type ResultType;
 331
 332     /**
 333      *  Compute the max distance (L_infinity) between two vectors.
 334      *
 335      *  This distance is not a valid kdtree distance, it's not dimensionwise additive.
 336      */
 337     template <typename Iterator1, typename Iterator2>
 338     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
 339     {
 340         ResultType result = ResultType();
 341         ResultType diff0, diff1, diff2, diff3;
 342         Iterator1 last = a + size;
 343         Iterator1 lastgroup = last - 3;
 344
 345         /* Process 4 items with each loop for efficiency. */
 346         while (a < lastgroup) {
 347             diff0 = abs(a[0] - b[0]);
 348             diff1 = abs(a[1] - b[1]);
 349             diff2 = abs(a[2] - b[2]);
 350             diff3 = abs(a[3] - b[3]);
 351             if (diff0>result) {result = diff0; }
 352             if (diff1>result) {result = diff1; }
 353             if (diff2>result) {result = diff2; }
 354             if (diff3>result) {result = diff3; }
 355             a += 4;
 356             b += 4;
 357
 358             if ((worst_dist>0)&&(result>worst_dist)) {
 359                 return result;
 360             }
 361         }
 362         /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
 363         while (a < last) {
 364             diff0 = abs(*a++ - *b++);
 365             result = (diff0>result) ? diff0 : result;
 366         }
 367         return result;
 368     }
 369
 370     /* This distance functor is not dimension-wise additive, which
 371      * makes it an invalid kd-tree distance, not implementing the accum_dist method */
 372
 373 };
 374
 375 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 376
 377 /**
 378  * Hamming distance functor - counts the bit differences between two strings - useful for the Brief descriptor
 379  * bit count of A exclusive XOR'ed with B
 380  */
 381 struct HammingLUT
 382 {
 383     typedef False is_kdtree_distance;
 384     typedef False is_vector_space_distance;
 385
 386     typedef unsigned char ElementType;
 387     typedef int ResultType;
 388
 389     /** this will count the bits in a ^ b
 390      */
 391     ResultType operator()(const unsigned char* a, const unsigned char* b, size_t size) const
 392     {
 393         static const uchar popCountTable[] =
 394         {
 395             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,
 396             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,
 397             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,
 398             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,
 399             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,
 400             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,
 401             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,
 402             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
 403         };
 404         ResultType result = 0;
 405         for (size_t i = 0; i < size; i++) {
 406             result += popCountTable[a[i] ^ b[i]];
 407         }
 408         return result;
 409     }
 410 };
 411
 412 /**
 413  * Hamming distance functor (pop count between two binary vectors, i.e. xor them and count the number of bits set)
 414  * That code was taken from brief.cpp in OpenCV
 415  */
 416 template<class T>
 417 struct Hamming
 418 {
 419     typedef False is_kdtree_distance;
 420     typedef False is_vector_space_distance;
 421
 422
 423     typedef T ElementType;
 424     typedef int ResultType;
 425
 426     template<typename Iterator1, typename Iterator2>
 427     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
 428     {
 429         ResultType result = 0;
 430 #if defined(__ARM_NEON__) && !defined(__CUDACC__)
 431         {
 432             uint32x4_t bits = vmovq_n_u32(0);
 433             for (size_t i = 0; i < size; i += 16) {
 434                 uint8x16_t A_vec = vld1q_u8 (a + i);
 435                 uint8x16_t B_vec = vld1q_u8 (b + i);
 436                 uint8x16_t AxorB = veorq_u8 (A_vec, B_vec);
 437                 uint8x16_t bitsSet = vcntq_u8 (AxorB);
 438                 uint16x8_t bitSet8 = vpaddlq_u8 (bitsSet);
 439                 uint32x4_t bitSet4 = vpaddlq_u16 (bitSet8);
 440                 bits = vaddq_u32(bits, bitSet4);
 441             }
 442             uint64x2_t bitSet2 = vpaddlq_u32 (bits);
 443             result = vgetq_lane_s32 (vreinterpretq_s32_u64(bitSet2),0);
 444             result += vgetq_lane_s32 (vreinterpretq_s32_u64(bitSet2),2);
 445         }
 446 #elif defined(__GNUC__)
 447         {
 448             //for portability just use unsigned long -- and use the __builtin_popcountll (see docs for __builtin_popcountll)
 449             typedef unsigned long long pop_t;
 450             const size_t modulo = size % sizeof(pop_t);
 451             const pop_t* a2 = reinterpret_cast<const pop_t*> (a);
 452             const pop_t* b2 = reinterpret_cast<const pop_t*> (b);
 453             const pop_t* a2_end = a2 + (size / sizeof(pop_t));
 454
 455             for (; a2 != a2_end; ++a2, ++b2) result += __builtin_popcountll((*a2) ^ (*b2));
 456
 457             if (modulo) {
 458                 //in the case where size is not dividable by sizeof(size_t)
 459                 //need to mask off the bits at the end
 460                 pop_t a_final = 0, b_final = 0;
 461                 memcpy(&a_final, a2, modulo);
 462                 memcpy(&b_final, b2, modulo);
 463                 result += __builtin_popcountll(a_final ^ b_final);
 464             }
 465         }
 466 #else // NO NEON and NOT GNUC
 467         HammingLUT lut;
 468         result = lut(reinterpret_cast<const unsigned char*> (a),
 469                      reinterpret_cast<const unsigned char*> (b), size);
 470 #endif
 471         return result;
 472     }
 473 };
 474
 475 template<typename T>
 476 struct Hamming2
 477 {
 478     typedef False is_kdtree_distance;
 479     typedef False is_vector_space_distance;
 480
 481     typedef T ElementType;
 482     typedef int ResultType;
 483
 484     /** This is popcount_3() from:
 485      * http://en.wikipedia.org/wiki/Hamming_weight */
 486     unsigned int popcnt32(uint32_t n) const
 487     {
 488         n -= ((n >> 1) & 0x55555555);
 489         n = (n & 0x33333333) + ((n >> 2) & 0x33333333);
 490         return (((n + (n >> 4))& 0xF0F0F0F)* 0x1010101) >> 24;
 491     }
 492
 493 #ifdef FLANN_PLATFORM_64_BIT
 494     unsigned int popcnt64(uint64_t n) const
 495     {
 496         n -= ((n >> 1) & 0x5555555555555555);
 497         n = (n & 0x3333333333333333) + ((n >> 2) & 0x3333333333333333);
 498         return (((n + (n >> 4))& 0x0f0f0f0f0f0f0f0f)* 0x0101010101010101) >> 56;
 499     }
 500 #endif
 501
 502     template <typename Iterator1, typename Iterator2>
 503     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
 504     {
 505 #ifdef FLANN_PLATFORM_64_BIT
 506         const uint64_t* pa = reinterpret_cast<const uint64_t*>(a);
 507         const uint64_t* pb = reinterpret_cast<const uint64_t*>(b);
 508         ResultType result = 0;
 509         size /= (sizeof(uint64_t)/sizeof(unsigned char));
 510         for(size_t i = 0; i < size; ++i ) {
 511             result += popcnt64(*pa ^ *pb);
 512             ++pa;
 513             ++pb;
 514         }
 515 #else
 516         const uint32_t* pa = reinterpret_cast<const uint32_t*>(a);
 517         const uint32_t* pb = reinterpret_cast<const uint32_t*>(b);
 518         ResultType result = 0;
 519         size /= (sizeof(uint32_t)/sizeof(unsigned char));
 520         for(size_t i = 0; i < size; ++i ) {
 521             result += popcnt32(*pa ^ *pb);
 522             ++pa;
 523             ++pb;
 524         }
 525 #endif
 526         return result;
 527     }
 528 };
 529
 530
 531
 532 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 533
 534 template<class T>
 535 struct HistIntersectionDistance
 536 {
 537     typedef True is_kdtree_distance;
 538     typedef True is_vector_space_distance;
 539
 540     typedef T ElementType;
 541     typedef typename Accumulator<T>::Type ResultType;
 542
 543     /**
 544      *  Compute the histogram intersection distance
 545      */
 546     template <typename Iterator1, typename Iterator2>
 547     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
 548     {
 549         ResultType result = ResultType();
 550         ResultType min0, min1, min2, min3;
 551         Iterator1 last = a + size;
 552         Iterator1 lastgroup = last - 3;
 553
 554         /* Process 4 items with each loop for efficiency. */
 555         while (a < lastgroup) {
 556             min0 = (ResultType)(a[0] < b[0] ? a[0] : b[0]);
 557             min1 = (ResultType)(a[1] < b[1] ? a[1] : b[1]);
 558             min2 = (ResultType)(a[2] < b[2] ? a[2] : b[2]);
 559             min3 = (ResultType)(a[3] < b[3] ? a[3] : b[3]);
 560             result += min0 + min1 + min2 + min3;
 561             a += 4;
 562             b += 4;
 563             if ((worst_dist>0)&&(result>worst_dist)) {
 564                 return result;
 565             }
 566         }
 567         /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
 568         while (a < last) {
 569             min0 = (ResultType)(*a < *b ? *a : *b);
 570             result += min0;
 571             ++a;
 572             ++b;
 573         }
 574         return result;
 575     }
 576
 577     /**
 578      * Partial distance, used by the kd-tree.
 579      */
 580     template <typename U, typename V>
 581     inline ResultType accum_dist(const U& a, const V& b, int) const
 582     {
 583         return a<b ? a : b;
 584     }
 585 };
 586
 587
 588
 589 template<class T>
 590 struct HellingerDistance
 591 {
 592     typedef True is_kdtree_distance;
 593     typedef True is_vector_space_distance;
 594
 595     typedef T ElementType;
 596     typedef typename Accumulator<T>::Type ResultType;
 597
 598     /**
 599      *  Compute the Hellinger distance
 600      */
 601     template <typename Iterator1, typename Iterator2>
 602     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
 603     {
 604         ResultType result = ResultType();
 605         ResultType diff0, diff1, diff2, diff3;
 606         Iterator1 last = a + size;
 607         Iterator1 lastgroup = last - 3;
 608
 609         /* Process 4 items with each loop for efficiency. */
 610         while (a < lastgroup) {
 611             diff0 = sqrt(static_cast<ResultType>(a[0])) - sqrt(static_cast<ResultType>(b[0]));
 612             diff1 = sqrt(static_cast<ResultType>(a[1])) - sqrt(static_cast<ResultType>(b[1]));
 613             diff2 = sqrt(static_cast<ResultType>(a[2])) - sqrt(static_cast<ResultType>(b[2]));
 614             diff3 = sqrt(static_cast<ResultType>(a[3])) - sqrt(static_cast<ResultType>(b[3]));
 615             result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
 616             a += 4;
 617             b += 4;
 618         }
 619         while (a < last) {
 620             diff0 = sqrt(static_cast<ResultType>(*a++)) - sqrt(static_cast<ResultType>(*b++));
 621             result += diff0 * diff0;
 622         }
 623         return result;
 624     }
 625
 626     /**
 627      * Partial distance, used by the kd-tree.
 628      */
 629     template <typename U, typename V>
 630     inline ResultType accum_dist(const U& a, const V& b, int) const
 631     {
 632         ResultType diff = sqrt(static_cast<ResultType>(a)) - sqrt(static_cast<ResultType>(b));
 633         return diff * diff;
 634     }
 635 };
 636
 637
 638 template<class T>
 639 struct ChiSquareDistance
 640 {
 641     typedef True is_kdtree_distance;
 642     typedef True is_vector_space_distance;
 643
 644     typedef T ElementType;
 645     typedef typename Accumulator<T>::Type ResultType;
 646
 647     /**
 648      *  Compute the chi-square distance
 649      */
 650     template <typename Iterator1, typename Iterator2>
 651     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
 652     {
 653         ResultType result = ResultType();
 654         ResultType sum, diff;
 655         Iterator1 last = a + size;
 656
 657         while (a < last) {
 658             sum = (ResultType)(*a + *b);
 659             if (sum>0) {
 660                 diff = (ResultType)(*a - *b);
 661                 result += diff*diff/sum;
 662             }
 663             ++a;
 664             ++b;
 665
 666             if ((worst_dist>0)&&(result>worst_dist)) {
 667                 return result;
 668             }
 669         }
 670         return result;
 671     }
 672
 673     /**
 674      * Partial distance, used by the kd-tree.
 675      */
 676     template <typename U, typename V>
 677     inline ResultType accum_dist(const U& a, const V& b, int) const
 678     {
 679         ResultType result = ResultType();
 680         ResultType sum, diff;
 681
 682         sum = (ResultType)(a+b);
 683         if (sum>0) {
 684             diff = (ResultType)(a-b);
 685             result = diff*diff/sum;
 686         }
 687         return result;
 688     }
 689 };
 690
 691
 692 template<class T>
 693 struct KL_Divergence
 694 {
 695     typedef True is_kdtree_distance;
 696     typedef True is_vector_space_distance;
 697
 698     typedef T ElementType;
 699     typedef typename Accumulator<T>::Type ResultType;
 700
 701     /**
 702      *  Compute the Kullback-Leibler divergence
 703      */
 704     template <typename Iterator1, typename Iterator2>
 705     ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
 706     {
 707         ResultType result = ResultType();
 708         Iterator1 last = a + size;
 709
 710         while (a < last) {
 711             if (* b != 0) {
 712                 ResultType ratio = (ResultType)(*a / *b);
 713                 if (ratio>0) {
 714                     result += *a * log(ratio);
 715                 }
 716             }
 717             ++a;
 718             ++b;
 719
 720             if ((worst_dist>0)&&(result>worst_dist)) {
 721                 return result;
 722             }
 723         }
 724         return result;
 725     }
 726
 727     /**
 728      * Partial distance, used by the kd-tree.
 729      */
 730     template <typename U, typename V>
 731     inline ResultType accum_dist(const U& a, const V& b, int) const
 732     {
 733         ResultType result = ResultType();
 734         if( *b != 0 ) {
 735             ResultType ratio = (ResultType)(a / b);
 736             if (ratio>0) {
 737                 result = a * log(ratio);
 738             }
 739         }
 740         return result;
 741     }
 742 };
 743
 744
 745
 746 /*
 747  * This is a "zero iterator". It basically behaves like a zero filled
 748  * array to all algorithms that use arrays as iterators (STL style).
 749  * It's useful when there's a need to compute the distance between feature
 750  * and origin it and allows for better compiler optimisation than using a
 751  * zero-filled array.
 752  */
 753 template <typename T>
 754 struct ZeroIterator
 755 {
 756
 757     T operator*()
 758     {
 759         return 0;
 760     }
 761
 762     T operator[](int)
 763     {
 764         return 0;
 765     }
 766
 767     const ZeroIterator<T>& operator ++()
 768     {
 769         return *this;
 770     }
 771
 772     ZeroIterator<T> operator ++(int)
 773     {
 774         return *this;
 775     }
 776
 777     ZeroIterator<T>& operator+=(int)
 778     {
 779         return *this;
 780     }
 781
 782 };
 783
 784
 785 /*
 786  * Depending on processed distances, some of them are already squared (e.g. L2)
 787  * and some are not (e.g.Hamming). In KMeans++ for instance we want to be sure
 788  * we are working on ^2 distances, thus following templates to ensure that.
 789  */
 790 template <typename Distance, typename ElementType>
 791 struct squareDistance
 792 {
 793     typedef typename Distance::ResultType ResultType;
 794     ResultType operator()( ResultType dist ) { return dist*dist; }
 795 };
 796
 797
 798 template <typename ElementType>
 799 struct squareDistance<L2_Simple<ElementType>, ElementType>
 800 {
 801     typedef typename L2_Simple<ElementType>::ResultType ResultType;
 802     ResultType operator()( ResultType dist ) { return dist; }
 803 };
 804
 805 template <typename ElementType>
 806 struct squareDistance<L2<ElementType>, ElementType>
 807 {
 808     typedef typename L2<ElementType>::ResultType ResultType;
 809     ResultType operator()( ResultType dist ) { return dist; }
 810 };
 811
 812
 813 template <typename ElementType>
 814 struct squareDistance<MinkowskiDistance<ElementType>, ElementType>
 815 {
 816     typedef typename MinkowskiDistance<ElementType>::ResultType ResultType;
 817     ResultType operator()( ResultType dist ) { return dist; }
 818 };
 819
 820 template <typename ElementType>
 821 struct squareDistance<HellingerDistance<ElementType>, ElementType>
 822 {
 823     typedef typename HellingerDistance<ElementType>::ResultType ResultType;
 824     ResultType operator()( ResultType dist ) { return dist; }
 825 };
 826
 827 template <typename ElementType>
 828 struct squareDistance<ChiSquareDistance<ElementType>, ElementType>
 829 {
 830     typedef typename ChiSquareDistance<ElementType>::ResultType ResultType;
 831     ResultType operator()( ResultType dist ) { return dist; }
 832 };
 833
 834
 835 template <typename Distance>
 836 typename Distance::ResultType ensureSquareDistance( typename Distance::ResultType dist )
 837 {
 838     typedef typename Distance::ElementType ElementType;
 839
 840     squareDistance<Distance, ElementType> dummy;
 841     return dummy( dist );
 842 }
 843
 844
 845 /*
 846  * ...and a template to ensure the user that he will process the normal distance,
 847  * and not squared distance, without losing processing time calling sqrt(ensureSquareDistance)
 848  * that will result in doing actually sqrt(dist*dist) for L1 distance for instance.
 849  */
 850 template <typename Distance, typename ElementType>
 851 struct simpleDistance
 852 {
 853     typedef typename Distance::ResultType ResultType;
 854     ResultType operator()( ResultType dist ) { return dist; }
 855 };
 856
 857
 858 template <typename ElementType>
 859 struct simpleDistance<L2_Simple<ElementType>, ElementType>
 860 {
 861     typedef typename L2_Simple<ElementType>::ResultType ResultType;
 862     ResultType operator()( ResultType dist ) { return sqrt(dist); }
 863 };
 864
 865 template <typename ElementType>
 866 struct simpleDistance<L2<ElementType>, ElementType>
 867 {
 868     typedef typename L2<ElementType>::ResultType ResultType;
 869     ResultType operator()( ResultType dist ) { return sqrt(dist); }
 870 };
 871
 872
 873 template <typename ElementType>
 874 struct simpleDistance<MinkowskiDistance<ElementType>, ElementType>
 875 {
 876     typedef typename MinkowskiDistance<ElementType>::ResultType ResultType;
 877     ResultType operator()( ResultType dist ) { return sqrt(dist); }
 878 };
 879
 880 template <typename ElementType>
 881 struct simpleDistance<HellingerDistance<ElementType>, ElementType>
 882 {
 883     typedef typename HellingerDistance<ElementType>::ResultType ResultType;
 884     ResultType operator()( ResultType dist ) { return sqrt(dist); }
 885 };
 886
 887 template <typename ElementType>
 888 struct simpleDistance<ChiSquareDistance<ElementType>, ElementType>
 889 {
 890     typedef typename ChiSquareDistance<ElementType>::ResultType ResultType;
 891     ResultType operator()( ResultType dist ) { return sqrt(dist); }
 892 };
 893
 894
 895 template <typename Distance>
 896 typename Distance::ResultType ensureSimpleDistance( typename Distance::ResultType dist )
 897 {
 898     typedef typename Distance::ElementType ElementType;
 899
 900     simpleDistance<Distance, ElementType> dummy;
 901     return dummy( dist );
 902 }
 903
 904 }
 905
 906 //! @endcond
 907
 908 #endif //OPENCV_FLANN_DIST_H_