Fix link errors for MinGW debug for blenderplayer. This bizarre error, not present...
[blender.git] / extern / libmv / libmv / tracking / lmicklt_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/tracking/lmicklt_region_tracker.h"
22
23 #include "libmv/logging/logging.h"
24 #include "libmv/image/image.h"
25 #include "libmv/image/convolve.h"
26 #include "libmv/image/sample.h"
27 #include "libmv/numeric/numeric.h"
28
29 namespace libmv {
30
31 // TODO(keir): Reduce duplication between here and the other region trackers.
32 static bool RegionIsInBounds(const FloatImage &image1,
33                       double x, double y,
34                       int half_window_size) {
35   // Check the minimum coordinates.
36   int min_x = floor(x) - half_window_size - 1;
37   int min_y = floor(y) - half_window_size - 1;
38   if (min_x < 0.0 ||
39       min_y < 0.0) {
40     return false;
41   }
42
43   // Check the maximum coordinates.
44   int max_x = ceil(x) + half_window_size + 1;
45   int max_y = ceil(y) + half_window_size + 1;
46   if (max_x > image1.cols() ||
47       max_y > image1.rows()) {
48     return false;
49   }
50
51   // Ok, we're good.
52   return true;
53 }
54
55 // Sample a region centered at x,y in image with size extending by half_width
56 // from x,y. Channels specifies the number of channels to sample from.
57 static void SamplePattern(const FloatImage &image,
58                    double x, double y,
59                    int half_width,
60                    int channels,
61                    FloatImage *sampled) {
62   sampled->Resize(2 * half_width + 1, 2 * half_width + 1, channels);
63   for (int r = -half_width; r <= half_width; ++r) {
64     for (int c = -half_width; c <= half_width; ++c) {
65       for (int i = 0; i < channels; ++i) {
66         (*sampled)(r + half_width, c + half_width, i) =
67             SampleLinear(image, y + r, x + c, i);
68       }
69     }
70   }
71 }
72
73 // Estimate "reasonable" error by computing autocorrelation for a small shift.
74 static double EstimateReasonableError(const FloatImage &image,
75                                double x, double y,
76                                int half_width) {
77   double error = 0.0;
78   for (int r = -half_width; r <= half_width; ++r) {
79     for (int c = -half_width; c <= half_width; ++c) {
80       double s = SampleLinear(image, y + r, x + c, 0);
81       double e1 = SampleLinear(image, y + r + 0.5, x + c, 0) - s;
82       double e2 = SampleLinear(image, y + r, x + c + 0.5, 0) - s;
83       error += e1*e1 + e2*e2;
84     }
85   }
86   return error / 2.0 * 8.0;
87 }
88
89 // This is implemented from "Lukas and Kanade 20 years on: Part 1. Page 42,
90 // figure 14: the Levenberg-Marquardt-Inverse Compositional Algorithm".
91 bool LmickltRegionTracker::Track(const FloatImage &image1,
92                              const FloatImage &image2,
93                              double  x1, double  y1,
94                              double *x2, double *y2) const {
95   if (!RegionIsInBounds(image1, x1, y1, half_window_size)) {
96     LG << "Fell out of image1's window with x1=" << x1 << ", y1=" << y1
97        << ", hw=" << half_window_size << ".";
98     return false;
99   }
100   
101   int width = 2 * half_window_size + 1;
102
103   // TODO(keir): Avoid recomputing gradients for e.g. the pyramid tracker.
104   Array3Df image_and_gradient1;
105   Array3Df image_and_gradient2;
106   BlurredImageAndDerivativesChannels(image1, sigma, &image_and_gradient1);
107
108   // TODO(keir): Avoid computing the derivative of image2.
109   BlurredImageAndDerivativesChannels(image2, sigma, &image_and_gradient2);
110
111   // Step -1: Resample the template (image1) since it is not pixel aligned.
112   //
113   // Take a sample of the gradient of the pattern area of image1 at the
114   // subpixel position x1, x2. This is reused for each iteration, so
115   // precomputing it saves time.
116   Array3Df image_and_gradient1_sampled;
117   SamplePattern(image_and_gradient1, x1, y1, half_window_size, 3,
118                 &image_and_gradient1_sampled);
119
120   // Step 0: Initialize delta = 0.01.
121   // 
122   // Ignored for my "normal" LM loop.
123
124   double reasonable_error =
125       EstimateReasonableError(image1, x1, y1, half_window_size);
126
127   // Step 1: Warp I with W(x, p) to compute I(W(x; p).
128   //
129   // Use two images for accepting / rejecting updates.
130   int current_image = 0, new_image = 1;
131   Array3Df image2_sampled[2];
132   SamplePattern(image_and_gradient2, *x2, *y2, half_window_size, 1,
133                 &image2_sampled[current_image]);
134
135   // Step 2: Compute the squared error I - J.
136   double error = 0;
137   for (int r = 0; r < width; ++r) {
138     for (int c = 0; c < width; ++c) {
139       double e = image_and_gradient1_sampled(r, c, 0) -
140                  image2_sampled[current_image](r, c, 0);
141       error += e*e;
142     }
143   }
144
145   // Step 3: Evaluate the gradient of the template.
146   //
147   // This is done above when sampling the template (step -1).
148
149   // Step 4: Evaluate the jacobian dw/dp at (x; 0).
150   //
151   // The jacobian between dx,dy and the warp is constant 2x2 identity, so it
152   // doesn't have to get computed. The Gauss-Newton Hessian matrix computation
153   // below would normally have to include a multiply by the jacobian.
154
155   // Step 5: Compute the steepest descent images of the template.
156   //
157   // Since the jacobian of the position w.r.t. the sampled template position is
158   // the identity, the steepest descent images are the same as the gradient.
159
160   // Step 6: Compute the Gauss-Newton Hessian for the template (image1).
161   //
162   // This could get rolled into the previous loop, but split it for now for
163   // clarity.
164   Mat2 H = Mat2::Zero();
165   for (int r = 0; r < width; ++r) {
166     for (int c = 0; c < width; ++c) {
167       Vec2 g(image_and_gradient1_sampled(r, c, 1),
168              image_and_gradient1_sampled(r, c, 2));
169       H += g * g.transpose();
170     }
171   }
172
173   double tau = 1e-3, eps1, eps2, eps3;
174   eps1 = eps2 = eps3 = 1e-15;
175
176   double mu = tau * std::max(H(0, 0), H(1, 1));
177   double nu = 2.0;
178
179   int i;
180   for (i = 0; i < max_iterations; ++i) {
181     // Check that the entire image patch is within the bounds of the images.
182     if (!RegionIsInBounds(image2, *x2, *y2, half_window_size)) {
183       LG << "Fell out of image2's window with x2=" << *x2 << ", y2=" << *y2
184          << ", hw=" << half_window_size << ".";
185       return false;
186     }
187
188     // Step 7: Compute z
189     Vec2 z = Vec2::Zero();
190     for (int r = 0; r < width; ++r) {
191       for (int c = 0; c < width; ++c) {
192         double e = image2_sampled[current_image](r, c, 0) -
193                    image_and_gradient1_sampled(r, c, 0);
194         z(0) += image_and_gradient1_sampled(r, c, 1) * e;
195         z(1) += image_and_gradient1_sampled(r, c, 2) * e;
196       }
197     }
198
199     // Step 8: Compute Hlm and (dx,dy)
200     Mat2 diag  = H.diagonal().asDiagonal();
201     diag *= mu;
202     Mat2 Hlm = H + diag;
203     Vec2 d = Hlm.lu().solve(z);
204
205     // TODO(keir): Use the usual LM termination and update criterion instead of
206     // this hacky version from the LK 20 years on paper.
207     LG << "x=" << *x2 << ", y=" << *y2 << ", dx=" << d[0] << ", dy=" << d[1]
208        << ", mu=" << mu << ", nu=" << nu;
209
210     // Step 9: Update the warp; W(x; p) <-- W(x;p) compose W(x, dp)^-1
211     double new_x2 = *x2 - d[0];
212     double new_y2 = *y2 - d[1];
213
214     // Step 9.1: Sample the image at the new position.
215     SamplePattern(image_and_gradient2, new_x2, new_y2, half_window_size, 1,
216                   &image2_sampled[new_image]);
217
218     // Step 9.2: Compute the new error.
219     // TODO(keir): Eliminate duplication with above code.
220     double new_error = 0;
221     for (int r = 0; r < width; ++r) {
222       for (int c = 0; c < width; ++c) {
223         double e = image_and_gradient1_sampled(r, c, 0) -
224                    image2_sampled[new_image](r, c, 0);
225         new_error += e*e;
226       }
227     }
228     LG << "Old error: " << error << ", new error: " << new_error;
229
230     // If the step was accepted, then check for termination.
231     if (d.squaredNorm() < min_update_squared_distance) {
232       if (new_error > reasonable_error) {
233         LG << "Update size shrank but reasonable error ("
234            << reasonable_error << ") not achieved; failing.";
235         return false;
236       }
237       LG << "Successful track in " << i << " iterations.";
238       return true;
239     }
240
241     double rho = (error - new_error) / (d.transpose() * (mu * d + z));
242
243     // Step 10: Accept or reject step.
244     if (rho <= 0) {
245       // This was a bad step, so don't update.
246       mu *= nu;
247       nu *= 2;
248       LG << "Error increased, so reject update.";
249     } else {
250       // The step was good, so update.
251       *x2 = new_x2;
252       *y2 = new_y2;
253       std::swap(new_image, current_image);
254       error = new_error;
255
256       mu *= std::max(1/3., 1 - pow(2*rho - 1, 3));
257       nu = 2;
258     }
259   }
260   // Getting here means we hit max iterations, so tracking failed.
261   LG << "Too many iterations; max is set to " << max_iterations << ".";
262   return false;
263 }
264
265 }  // namespace libmv