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