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