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