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