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