94319fcb7d3392460be4e4343c4c037d85e50d52
[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 // Computes U and e from the Ud = e equation (number 14) from the paper.
32 static void ComputeTrackingEquation(const Array3Df &image_and_gradient1,
33                                     const Array3Df &image_and_gradient2,
34                                     double x1, double y1,
35                                     double x2, double y2,
36                                     int half_width,
37                                     double lambda,
38                                     Mat2f *U,
39                                     Vec2f *e) {
40   Mat2f A, B, C, D;
41   A = B = C = D  = Mat2f::Zero();
42
43   Vec2f R, S, V, W;
44   R = S = V = W = Vec2f::Zero();
45
46   for (int r = -half_width; r <= half_width; ++r) {
47     for (int c = -half_width; c <= half_width; ++c) {
48       float xx1 = x1 + c;
49       float yy1 = y1 + r;
50       float xx2 = x2 + c;
51       float yy2 = y2 + r;
52
53       float I = SampleLinear(image_and_gradient1, yy1, xx1, 0);
54       float J = SampleLinear(image_and_gradient2, yy2, xx2, 0);
55
56       Vec2f gI, gJ;
57       gI << SampleLinear(image_and_gradient1, yy1, xx1, 1),
58             SampleLinear(image_and_gradient1, yy1, xx1, 2);
59       gJ << SampleLinear(image_and_gradient2, yy2, xx2, 1),
60             SampleLinear(image_and_gradient2, yy2, xx2, 2);
61
62       // Equation 15 from the paper.
63       A += gI * gI.transpose();
64       B += gI * gJ.transpose();
65       C += gJ * gJ.transpose();
66       R += I * gI;
67       S += J * gI;
68       V += I * gJ;
69       W += J * gJ;
70     }
71   }
72
73   // In the paper they show a D matrix, but it is just B transpose, so use that
74   // instead of explicitly computing D.
75   Mat2f Di = B.transpose().inverse();
76
77   // Equation 14 from the paper.
78   *U = A*Di*C + lambda*Di*C - 0.5*B;
79   *e = (A + lambda*Mat2f::Identity())*Di*(V - W) + 0.5*(S - R);
80 }
81
82 bool TrkltRegionTracker::Track(const FloatImage &image1,
83                                const FloatImage &image2,
84                                double  x1, double  y1,
85                                double *x2, double *y2) const {
86   Array3Df image_and_gradient1;
87   Array3Df image_and_gradient2;
88   BlurredImageAndDerivativesChannels(image1, sigma, &image_and_gradient1);
89   BlurredImageAndDerivativesChannels(image2, sigma, &image_and_gradient2);
90
91   int i;
92   Vec2f d = Vec2f::Zero();
93   for (i = 0; i < max_iterations; ++i) {
94     // Compute gradient matrix and error vector.
95     Mat2f U;
96     Vec2f e;
97     ComputeTrackingEquation(image_and_gradient1,
98                             image_and_gradient2,
99                             x1, y1,
100                             *x2, *y2,
101                             half_window_size,
102                             lambda,
103                             &U, &e);
104
105     // Solve the linear system for the best update to x2 and y2.
106     d = U.lu().solve(e);
107
108     // Update the position with the solved displacement.
109     *x2 += d[0];
110     *y2 += d[1];
111
112     // Check for the quality of the solution, but not until having already
113     // updated the position with our best estimate. The reason to do the update
114     // anyway is that the user already knows the position is bad, so we may as
115     // well try our best.
116     float determinant = U.determinant();
117     if (fabs(determinant) < min_determinant) {
118       // The determinant, which indicates the trackiness of the point, is too
119       // small, so fail out.
120       LG << "Determinant " << determinant << " is too small; failing tracking.";
121       return false;
122     }
123
124     // If the update is small, then we probably found the target.
125     if (d.squaredNorm() < min_update_squared_distance) {
126       LG << "Successful track in " << i << " iterations.";
127       return true;
128     }
129   }
130   // Getting here means we hit max iterations, so tracking failed.
131   LG << "Too many iterations; max is set to " << max_iterations << ".";
132   return false;
133 }
134
135 }  // namespace libmv