Tag object with object solver constraint to recalc when changing value of
[blender-staging.git] / extern / libmv / libmv / tracking / brute_region_tracker.cc
1 // Copyright (c) 2011 libmv authors.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19 // IN THE SOFTWARE.
20
21 #include "libmv/tracking/brute_region_tracker.h"
22
23 #ifdef __SSE2__
24 #include <emmintrin.h>
25 #endif
26
27 #if !defined(__APPLE__) && !defined(__FreeBSD__)
28 // Needed for memalign on Linux and _aligned_alloc on Windows.
29 #ifdef FREE_WINDOWS
30 /* make sure _aligned_malloc is included */
31 #ifdef __MSVCRT_VERSION__
32 #undef __MSVCRT_VERSION__
33 #endif
34
35 #define __MSVCRT_VERSION__ 0x0700
36 #endif
37
38 #include <malloc.h>
39 #else
40 // Apple's malloc is 16-byte aligned, and does not have malloc.h, so include
41 // stdilb instead.
42 #include <cstdlib>
43 #endif
44
45 #include "libmv/image/image.h"
46 #include "libmv/image/convolve.h"
47 #include "libmv/image/sample.h"
48 #include "libmv/logging/logging.h"
49
50 namespace libmv {
51 namespace {
52
53 // TODO(keir): It's stupid that this is needed here. Push this somewhere else.
54 void *aligned_malloc(int size, int alignment) {
55 #ifdef _WIN32
56   return _aligned_malloc(size, alignment);
57 #elif __APPLE__
58   // On Mac OS X, both the heap and the stack are guaranteed 16-byte aligned so
59   // they work natively with SSE types with no further work.
60   CHECK_EQ(alignment, 16);
61   return malloc(size);
62 #elif __FreeBSD__
63   void *result;
64
65   if(posix_memalign(&result, alignment, size)) {
66     // non-zero means allocation error
67     // either no allocation or bad alignment value
68     return NULL;
69   }
70   return result;
71 #else // This is for Linux.
72   return memalign(alignment, size);
73 #endif
74 }
75
76 void aligned_free(void *ptr) {
77 #ifdef _WIN32
78   _aligned_free(ptr);
79 #else
80   free(ptr);
81 #endif
82 }
83
84 bool RegionIsInBounds(const FloatImage &image1,
85                       double x, double y,
86                       int half_window_size) {
87   // Check the minimum coordinates.
88   int min_x = floor(x) - half_window_size - 1;
89   int min_y = floor(y) - half_window_size - 1;
90   if (min_x < 0.0 ||
91       min_y < 0.0) {
92     return false;
93   }
94
95   // Check the maximum coordinates.
96   int max_x = ceil(x) + half_window_size + 1;
97   int max_y = ceil(y) + half_window_size + 1;
98   if (max_x > image1.cols() ||
99       max_y > image1.rows()) {
100     return false;
101   }
102
103   // Ok, we're good.
104   return true;
105 }
106
107 #ifdef __SSE2__
108
109 // Compute the sub of absolute differences between the arrays "a" and "b". 
110 // The array "a" is assumed to be 16-byte aligned, while "b" is not. The
111 // result is returned as the first and third elements of __m128i if
112 // interpreted as a 4-element 32-bit integer array. The SAD is the sum of the
113 // elements.
114 //
115 // The function requires size % 16 valid extra elements at the end of both "a"
116 // and "b", since the SSE load instructionst will pull in memory past the end
117 // of the arrays if their size is not a multiple of 16.
118 inline static __m128i SumOfAbsoluteDifferencesContiguousSSE(
119     const unsigned char *a,  // aligned
120     const unsigned char *b,  // not aligned
121     unsigned int size,
122     __m128i sad) {
123   // Do the bulk of the work as 16-way integer operations.
124   for(unsigned int j = 0; j < size / 16; j++) {
125     sad = _mm_add_epi32(sad, _mm_sad_epu8( _mm_load_si128 ((__m128i*)(a + 16 * j)),
126                                            _mm_loadu_si128((__m128i*)(b + 16 * j))));
127   }
128   // Handle the trailing end.
129   // TODO(keir): Benchmark to verify that the below SSE is a win compared to a
130   // hand-rolled loop. It's not clear that the hand rolled loop would be slower
131   // than the potential cache miss when loading the immediate table below.
132   //
133   // An alternative to this version is to take a packet of all 1's then do a
134   // 128-bit shift. The issue is that the shift instruction needs an immediate
135   // amount rather than a variable amount, so the branch instruction here must
136   // remain. See _mm_srli_si128 and  _mm_slli_si128.
137   unsigned int remainder = size % 16u;
138   if (remainder) {
139     unsigned int j = size / 16;
140     __m128i a_trail = _mm_load_si128 ((__m128i*)(a + 16 * j));
141     __m128i b_trail = _mm_loadu_si128((__m128i*)(b + 16 * j));
142     __m128i mask;
143     switch (remainder) {
144 #define X 0xff
145       case  1: mask = _mm_setr_epi8(X, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); break;
146       case  2: mask = _mm_setr_epi8(X, X, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); break;
147       case  3: mask = _mm_setr_epi8(X, X, X, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); break;
148       case  4: mask = _mm_setr_epi8(X, X, X, X, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); break;
149       case  5: mask = _mm_setr_epi8(X, X, X, X, X, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); break;
150       case  6: mask = _mm_setr_epi8(X, X, X, X, X, X, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); break;
151       case  7: mask = _mm_setr_epi8(X, X, X, X, X, X, X, 0, 0, 0, 0, 0, 0, 0, 0, 0); break;
152       case  8: mask = _mm_setr_epi8(X, X, X, X, X, X, X, X, 0, 0, 0, 0, 0, 0, 0, 0); break;
153       case  9: mask = _mm_setr_epi8(X, X, X, X, X, X, X, X, X, 0, 0, 0, 0, 0, 0, 0); break;
154       case 10: mask = _mm_setr_epi8(X, X, X, X, X, X, X, X, X, X, 0, 0, 0, 0, 0, 0); break;
155       case 11: mask = _mm_setr_epi8(X, X, X, X, X, X, X, X, X, X, X, 0, 0, 0, 0, 0); break;
156       case 12: mask = _mm_setr_epi8(X, X, X, X, X, X, X, X, X, X, X, X, 0, 0, 0, 0); break;
157       case 13: mask = _mm_setr_epi8(X, X, X, X, X, X, X, X, X, X, X, X, X, 0, 0, 0); break;
158       case 14: mask = _mm_setr_epi8(X, X, X, X, X, X, X, X, X, X, X, X, X, X, 0, 0); break;
159       case 15: mask = _mm_setr_epi8(X, X, X, X, X, X, X, X, X, X, X, X, X, X, X, 0); break;
160 #undef X
161     }
162     sad = _mm_add_epi32(sad, _mm_sad_epu8(_mm_and_si128(mask, a_trail),
163                                           _mm_and_si128(mask, b_trail)));
164   }
165   return sad;
166 }
167 #endif
168
169 // Computes the sum of absolute differences between pattern and image. Pattern
170 // must be 16-byte aligned, and the stride must be a multiple of 16. The image
171 // does pointer does not have to be aligned.
172 int SumOfAbsoluteDifferencesContiguousImage(
173     const unsigned char *pattern,
174     unsigned int pattern_width,
175     unsigned int pattern_height,
176     unsigned int pattern_stride,
177     const unsigned char *image,
178     unsigned int image_stride) {
179 #ifdef __SSE2__
180   // TODO(keir): Add interleaved accumulation, where accumulation is done into
181   // two or more SSE registers that then get combined at the end. This reduces
182   // instruction dependency; in Eigen's squared norm code, splitting the
183   // accumulation produces a ~2x speedup. It's not clear it will help here,
184   // where the number of SSE instructions in the inner loop is smaller.
185   __m128i sad = _mm_setzero_si128();
186   for (int r = 0; r < pattern_height; ++r) {
187     sad = SumOfAbsoluteDifferencesContiguousSSE(&pattern[pattern_stride * r],
188                                                 &image[image_stride * r],
189                                                 pattern_width,
190                                                 sad);
191   }
192   return _mm_cvtsi128_si32(
193              _mm_add_epi32(sad,
194                  _mm_shuffle_epi32(sad, _MM_SHUFFLE(3, 0, 1, 2))));
195 #else
196   int sad = 0;
197   for (int r = 0; r < pattern_height; ++r) {
198     for (int c = 0; c < pattern_width; ++c) {
199       sad += abs(pattern[pattern_stride * r + c] - image[image_stride * r + c]);
200     }
201   }
202   return sad;
203 #endif
204 }
205
206 // Sample a region of size width, height centered at x,y in image, converting
207 // from float to byte in the process. Samples from the first channel. Puts
208 // result into *pattern.
209 void SampleRectangularPattern(const FloatImage &image,
210                               double x, double y,
211                               int width,
212                               int height,
213                               int pattern_stride,
214                               unsigned char *pattern) {
215   // There are two cases for width and height: even or odd. If it's odd, then
216   // the bounds [-width / 2, width / 2] works as expected. However, for even,
217   // this results in one extra access past the end. So use < instead of <= in
218   // the loops below, but increase the end limit by one in the odd case.
219   int end_width = (width / 2) + (width % 2);
220   int end_height = (height / 2) + (height % 2);
221   for (int r = -height / 2; r < end_height; ++r) {
222     for (int c = -width / 2; c < end_width; ++c) {
223       pattern[pattern_stride * (r + height / 2) +  c + width / 2] =
224           SampleLinear(image, y + r, x + c, 0) * 255.0;
225     }
226   }
227 }
228
229 // Returns x rounded up to the nearest multiple of alignment.
230 inline int PadToAlignment(int x, int alignment) {
231   if (x % alignment != 0) {
232     x += alignment - (x % alignment);
233   }
234   return x;
235 }
236
237 // Sample a region centered at x,y in image with size extending by half_width
238 // from x. Samples from the first channel. The resulting array is placed in
239 // *pattern, and the stride, which will be a multiple of 16 if SSE is enabled,
240 // is returned in *pattern_stride.
241 //
242 // NOTE: Caller must free *pattern with aligned_malloc() from above.
243 void SampleSquarePattern(const FloatImage &image,
244                          double x, double y,
245                          int half_width,
246                          unsigned char **pattern,
247                          int *pattern_stride) {
248   int width = 2 * half_width + 1;
249   // Allocate an aligned block with padding on the end so each row of the
250   // pattern starts on a 16-byte boundary.
251   *pattern_stride = PadToAlignment(width, 16);
252   int pattern_size_bytes = *pattern_stride * width;
253   *pattern = static_cast<unsigned char *>(
254       aligned_malloc(pattern_size_bytes, 16));
255   SampleRectangularPattern(image, x, y, width, width,
256                            *pattern_stride,
257                            *pattern);
258 }
259
260 // NOTE: Caller must free *image with aligned_malloc() from above.
261 void FloatArrayToByteArrayWithPadding(const FloatImage &float_image,
262                                       unsigned char **image,
263                                       int *image_stride) {
264   // Allocate enough so that accessing 16 elements past the end is fine.
265   *image_stride = float_image.Width() + 16;
266   *image = static_cast<unsigned char *>(
267       aligned_malloc(*image_stride * float_image.Height(), 16));
268   for (int i = 0; i < float_image.Height(); ++i) {
269     for (int j = 0; j < float_image.Width(); ++j) {
270       (*image)[*image_stride * i + j] =
271           static_cast<unsigned char>(255.0 * float_image(i, j, 0));
272     }
273   }
274 }
275
276 }  // namespace
277
278 // TODO(keir): Compare the "sharpness" of the peak around the best pixel. It's
279 // probably worth plotting a few examples to see what the histogram of SAD
280 // values for every hypothesis looks like.
281 //
282 // TODO(keir): Priority queue for multiple hypothesis.
283 bool BruteRegionTracker::Track(const FloatImage &image1,
284                                const FloatImage &image2,
285                                double  x1, double  y1,
286                                double *x2, double *y2) const {
287   if (!RegionIsInBounds(image1, x1, y1, half_window_size)) {
288     LG << "Fell out of image1's window with x1=" << x1 << ", y1=" << y1
289        << ", hw=" << half_window_size << ".";
290     return false;
291   }
292   int pattern_width = 2 * half_window_size + 1;
293
294   Array3Df image_and_gradient1;
295   Array3Df image_and_gradient2;
296   BlurredImageAndDerivativesChannels(image1, 0.9, &image_and_gradient1);
297   BlurredImageAndDerivativesChannels(image2, 0.9, &image_and_gradient2);
298
299   // Sample the pattern to get it aligned to an image grid.
300   unsigned char *pattern;
301   int pattern_stride;
302   SampleSquarePattern(image_and_gradient1, x1, y1, half_window_size,
303                       &pattern,
304                       &pattern_stride);
305
306   // Convert the search area directly to bytes without sampling.
307   unsigned char *search_area;
308   int search_area_stride;
309   FloatArrayToByteArrayWithPadding(image_and_gradient2, &search_area, &search_area_stride);
310
311   // Try all possible locations inside the search area. Yes, everywhere.
312   int best_i, best_j, best_sad = INT_MAX;
313   for (int i = 0; i < image2.Height() - pattern_width; ++i) {
314     for (int j = 0; j < image2.Width() - pattern_width; ++j) {
315       int sad = SumOfAbsoluteDifferencesContiguousImage(pattern,
316                                                         pattern_width,
317                                                         pattern_width,
318                                                         pattern_stride,
319                                                         search_area + search_area_stride * i + j,
320                                                         search_area_stride);
321       if (sad < best_sad) {
322         best_i = i;
323         best_j = j;
324         best_sad = sad;
325       }
326     }
327   }
328
329   aligned_free(pattern);
330   aligned_free(search_area);
331
332   if (best_sad != INT_MAX) {
333     *x2 = best_j + half_window_size;
334     *y2 = best_i + half_window_size;
335     return true;
336   }
337   return false;
338 }
339
340 }  // namespace libmv