Fix link errors for MinGW debug for blenderplayer. This bizarre error, not present...
[blender.git] / extern / libmv / libmv / tracking / klt_region_tracker.cc
1 // Copyright (c) 2007, 2008, 2009, 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/logging/logging.h"
22 #include "libmv/tracking/klt_region_tracker.h"
23 #include "libmv/image/image.h"
24 #include "libmv/image/convolve.h"
25 #include "libmv/image/sample.h"
26
27 namespace libmv {
28
29 // Compute the gradient matrix noted by Z and the error vector e. See Good
30 // Features to Track.
31 //
32 // TODO(keir): The calls to SampleLinear() do boundary checking that should
33 // instead happen outside the loop. Since this is the innermost loop, the extra
34 // bounds checking hurts performance.
35 static void ComputeTrackingEquation(const Array3Df &image_and_gradient1,
36                                     const Array3Df &image_and_gradient2,
37                                     double x1, double y1,
38                                     double x2, double y2,
39                                     int half_width,
40                                     float *gxx,
41                                     float *gxy,
42                                     float *gyy,
43                                     float *ex,
44                                     float *ey) {
45   *gxx = *gxy = *gyy = 0;
46   *ex = *ey = 0;
47   for (int r = -half_width; r <= half_width; ++r) {
48     for (int c = -half_width; c <= half_width; ++c) {
49       float xx1 = x1 + c;
50       float yy1 = y1 + r;
51       float xx2 = x2 + c;
52       float yy2 = y2 + r;
53       float I =  SampleLinear(image_and_gradient1, yy1, xx1, 0);
54       float J =  SampleLinear(image_and_gradient2, yy2, xx2, 0);
55       float gx = SampleLinear(image_and_gradient2, yy2, xx2, 1);
56       float gy = SampleLinear(image_and_gradient2, yy2, xx2, 2);
57       *gxx += gx * gx;
58       *gxy += gx * gy;
59       *gyy += gy * gy;
60       *ex += (I - J) * gx;
61       *ey += (I - J) * gy;
62     }
63   }
64 }
65
66 static bool RegionIsInBounds(const FloatImage &image1,
67                       double x, double y,
68                       int half_window_size) {
69   // Check the minimum coordinates.
70   int min_x = floor(x) - half_window_size - 1;
71   int min_y = floor(y) - half_window_size - 1;
72   if (min_x < 0.0 ||
73       min_y < 0.0) {
74     return false;
75   }
76
77   // Check the maximum coordinates.
78   int max_x = ceil(x) + half_window_size + 1;
79   int max_y = ceil(y) + half_window_size + 1;
80   if (max_x > image1.cols() ||
81       max_y > image1.rows()) {
82     return false;
83   }
84
85   // Ok, we're good.
86   return true;
87 }
88
89 bool KltRegionTracker::Track(const FloatImage &image1,
90                              const FloatImage &image2,
91                              double  x1, double  y1,
92                              double *x2, double *y2) const {
93   if (!RegionIsInBounds(image1, x1, y1, half_window_size)) {
94     LG << "Fell out of image1's window with x1=" << x1 << ", y1=" << y1
95        << ", hw=" << half_window_size << ".";
96     return false;
97   }
98
99   Array3Df image_and_gradient1;
100   Array3Df image_and_gradient2;
101   BlurredImageAndDerivativesChannels(image1, sigma, &image_and_gradient1);
102   BlurredImageAndDerivativesChannels(image2, sigma, &image_and_gradient2);
103
104   int i;
105   float dx = 0, dy = 0;
106   for (i = 0; i < max_iterations; ++i) {
107     // Check that the entire image patch is within the bounds of the images.
108     if (!RegionIsInBounds(image2, *x2, *y2, half_window_size)) {
109       LG << "Fell out of image2's window with x2=" << *x2 << ", y2=" << *y2
110          << ", hw=" << half_window_size << ".";
111       return false;
112     }
113
114     // Compute gradient matrix and error vector.
115     float gxx, gxy, gyy, ex, ey;
116     ComputeTrackingEquation(image_and_gradient1,
117                             image_and_gradient2,
118                             x1, y1,
119                             *x2, *y2,
120                             half_window_size,
121                             &gxx, &gxy, &gyy, &ex, &ey);
122
123     // Solve the tracking equation
124     //
125     //   [gxx gxy] [dx] = [ex]
126     //   [gxy gyy] [dy] = [ey]
127     //
128     // for dx and dy.  Borrowed from Stan Birchfield's KLT implementation.
129     float determinant = gxx * gyy - gxy * gxy;
130     dx = (gyy * ex - gxy * ey) / determinant;
131     dy = (gxx * ey - gxy * ex) / determinant;
132
133     // Update the position with the solved displacement.
134     *x2 += dx;
135     *y2 += dy;
136
137     // Check for the quality of the solution, but not until having already
138     // updated the position with our best estimate. The reason to do the update
139     // anyway is that the user already knows the position is bad, so we may as
140     // well try our best.
141     if (determinant < min_determinant) {
142       // The determinant, which indicates the trackiness of the point, is too
143       // small, so fail out.
144       LG << "Determinant " << determinant << " is too small; failing tracking.";
145       return false;
146     }
147     LG << "x=" << *x2 << ", y=" << *y2 << ", dx=" << dx << ", dy=" << dy << ", det=" << determinant;
148
149     // If the update is small, then we probably found the target.
150     if (dx * dx + dy * dy < min_update_squared_distance) {
151       LG << "Successful track in " << i << " iterations.";
152       return true;
153     }
154   }
155   // Getting here means we hit max iterations, so tracking failed.
156   LG << "Too many iterations; max is set to " << max_iterations << ".";
157   return false;
158 }
159
160 }  // namespace libmv