Assorted camera tracker improvements
[blender.git] / extern / libmv / libmv / tracking / trklt_region_tracker.cc
1 // Copyright (c) 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/trklt_region_tracker.h"
22
23 #include "libmv/logging/logging.h"
24 #include "libmv/numeric/numeric.h"
25 #include "libmv/image/image.h"
26 #include "libmv/image/convolve.h"
27 #include "libmv/image/sample.h"
28
29 namespace libmv {
30
31 // TODO(keir): Switch this to use the smarter LM loop like in ESM.
32
33 // Computes U and e from the Ud = e equation (number 14) from the paper.
34 static void ComputeTrackingEquation(const Array3Df &image_and_gradient1,
35                                     const Array3Df &image_and_gradient2,
36                                     double x1, double y1,
37                                     double x2, double y2,
38                                     int half_width,
39                                     double lambda,
40                                     Mat2f *U,
41                                     Vec2f *e) {
42   Mat2f A, B, C, D;
43   A = B = C = D  = Mat2f::Zero();
44
45   Vec2f R, S, V, W;
46   R = S = V = W = Vec2f::Zero();
47
48   for (int r = -half_width; r <= half_width; ++r) {
49     for (int c = -half_width; c <= half_width; ++c) {
50       float xx1 = x1 + c;
51       float yy1 = y1 + r;
52       float xx2 = x2 + c;
53       float yy2 = y2 + r;
54
55       float I = SampleLinear(image_and_gradient1, yy1, xx1, 0);
56       float J = SampleLinear(image_and_gradient2, yy2, xx2, 0);
57
58       Vec2f gI, gJ;
59       gI << SampleLinear(image_and_gradient1, yy1, xx1, 1),
60             SampleLinear(image_and_gradient1, yy1, xx1, 2);
61       gJ << SampleLinear(image_and_gradient2, yy2, xx2, 1),
62             SampleLinear(image_and_gradient2, yy2, xx2, 2);
63
64       // Equation 15 from the paper.
65       A += gI * gI.transpose();
66       B += gI * gJ.transpose();
67       C += gJ * gJ.transpose();
68       R += I * gI;
69       S += J * gI;
70       V += I * gJ;
71       W += J * gJ;
72     }
73   }
74
75   // In the paper they show a D matrix, but it is just B transpose, so use that
76   // instead of explicitly computing D.
77   Mat2f Di = B.transpose().inverse();
78
79   // Equation 14 from the paper.
80   *U = A*Di*C + lambda*Di*C - 0.5*B;
81   *e = (A + lambda*Mat2f::Identity())*Di*(V - W) + 0.5*(S - R);
82 }
83
84 bool RegionIsInBounds(const FloatImage &image1,
85                       double x, double y,
86                       int half_window_size) {
87   // Check the minimum coordinates.
88   int min_x = floor(x) - half_window_size - 1;
89   int min_y = floor(y) - half_window_size - 1;
90   if (min_x < 0.0 ||
91       min_y < 0.0) {
92     return false;
93   }
94
95   // Check the maximum coordinates.
96   int max_x = ceil(x) + half_window_size + 1;
97   int max_y = ceil(y) + half_window_size + 1;
98   if (max_x > image1.cols() ||
99       max_y > image1.rows()) {
100     return false;
101   }
102
103   // Ok, we're good.
104   return true;
105 }
106
107 bool TrkltRegionTracker::Track(const FloatImage &image1,
108                                const FloatImage &image2,
109                                double  x1, double  y1,
110                                double *x2, double *y2) const {
111   if (!RegionIsInBounds(image1, x1, y1, half_window_size)) {
112     LG << "Fell out of image1's window with x1=" << x1 << ", y1=" << y1
113        << ", hw=" << half_window_size << ".";
114     return false;
115   }
116
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   int i;
123   Vec2f d = Vec2f::Zero();
124   for (i = 0; i < max_iterations; ++i) {
125     // Check that the entire image patch is within the bounds of the images.
126     if (!RegionIsInBounds(image2, *x2, *y2, half_window_size)) {
127       LG << "Fell out of image2's window with x2=" << *x2 << ", y2=" << *y2
128          << ", hw=" << half_window_size << ".";
129       return false;
130     }
131
132     // Compute gradient matrix and error vector.
133     Mat2f U;
134     Vec2f e;
135     ComputeTrackingEquation(image_and_gradient1,
136                             image_and_gradient2,
137                             x1, y1,
138                             *x2, *y2,
139                             half_window_size,
140                             lambda,
141                             &U, &e);
142
143     // Solve the linear system for the best update to x2 and y2.
144     d = U.lu().solve(e);
145
146     // Update the position with the solved displacement.
147     *x2 += d[0];
148     *y2 += d[1];
149
150     // Check for the quality of the solution, but not until having already
151     // updated the position with our best estimate. The reason to do the update
152     // anyway is that the user already knows the position is bad, so we may as
153     // well try our best.
154     float determinant = U.determinant();
155     if (fabs(determinant) < min_determinant) {
156       // The determinant, which indicates the trackiness of the point, is too
157       // small, so fail out.
158       LG << "Determinant " << determinant << " is too small; failing tracking.";
159       return false;
160     }
161     LG << "x=" << *x2 << ", y=" << *y2 << ", dx=" << d[0] << ", dy=" << d[1] << ", det=" << determinant;
162
163
164     // If the update is small, then we probably found the target.
165     if (d.squaredNorm() < min_update_squared_distance) {
166       LG << "Successful track in " << i << " iterations.";
167       return true;
168     }
169   }
170   // Getting here means we hit max iterations, so tracking failed.
171   LG << "Too many iterations; max is set to " << max_iterations << ".";
172   return false;
173 }
174
175 }  // namespace libmv