diff options
Diffstat (limited to 'intern/libmv/libmv/tracking/trklt_region_tracker.cc')
| -rw-r--r-- | intern/libmv/libmv/tracking/trklt_region_tracker.cc | 176 |
1 files changed, 176 insertions, 0 deletions
diff --git a/intern/libmv/libmv/tracking/trklt_region_tracker.cc b/intern/libmv/libmv/tracking/trklt_region_tracker.cc new file mode 100644 index 00000000000..05ef3d1d272 --- /dev/null +++ b/intern/libmv/libmv/tracking/trklt_region_tracker.cc @@ -0,0 +1,176 @@ +// Copyright (c) 2011 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include "libmv/tracking/trklt_region_tracker.h" + +#include "libmv/logging/logging.h" +#include "libmv/numeric/numeric.h" +#include "libmv/image/image.h" +#include "libmv/image/convolve.h" +#include "libmv/image/sample.h" + +namespace libmv { + +// TODO(keir): Switch this to use the smarter LM loop like in ESM. + +// Computes U and e from the Ud = e equation (number 14) from the paper. +static void ComputeTrackingEquation(const Array3Df &image_and_gradient1, + const Array3Df &image_and_gradient2, + double x1, double y1, + double x2, double y2, + int half_width, + double lambda, + Mat2f *U, + Vec2f *e) { + Mat2f A, B, C, D; + A = B = C = D = Mat2f::Zero(); + + Vec2f R, S, V, W; + R = S = V = W = Vec2f::Zero(); + + for (int r = -half_width; r <= half_width; ++r) { + for (int c = -half_width; c <= half_width; ++c) { + float xx1 = x1 + c; + float yy1 = y1 + r; + float xx2 = x2 + c; + float yy2 = y2 + r; + + float I = SampleLinear(image_and_gradient1, yy1, xx1, 0); + float J = SampleLinear(image_and_gradient2, yy2, xx2, 0); + + Vec2f gI, gJ; + gI << SampleLinear(image_and_gradient1, yy1, xx1, 1), + SampleLinear(image_and_gradient1, yy1, xx1, 2); + gJ << SampleLinear(image_and_gradient2, yy2, xx2, 1), + SampleLinear(image_and_gradient2, yy2, xx2, 2); + + // Equation 15 from the paper. + A += gI * gI.transpose(); + B += gI * gJ.transpose(); + C += gJ * gJ.transpose(); + R += I * gI; + S += J * gI; + V += I * gJ; + W += J * gJ; + } + } + + // In the paper they show a D matrix, but it is just B transpose, so use that + // instead of explicitly computing D. + Mat2f Di = B.transpose().inverse(); + + // Equation 14 from the paper. + *U = A*Di*C + lambda*Di*C - 0.5*B; + *e = (A + lambda*Mat2f::Identity())*Di*(V - W) + 0.5*(S - R); +} + +static bool RegionIsInBounds(const FloatImage &image1, + double x, double y, + int half_window_size) { + // Check the minimum coordinates. + int min_x = floor(x) - half_window_size - 1; + int min_y = floor(y) - half_window_size - 1; + if (min_x < 0.0 || + min_y < 0.0) { + return false; + } + + // Check the maximum coordinates. + int max_x = ceil(x) + half_window_size + 1; + int max_y = ceil(y) + half_window_size + 1; + if (max_x > image1.cols() || + max_y > image1.rows()) { + return false; + } + + // Ok, we're good. + return true; +} + +bool TrkltRegionTracker::Track(const FloatImage &image1, + const FloatImage &image2, + double x1, double y1, + double *x2, double *y2) const { + if (!RegionIsInBounds(image1, x1, y1, half_window_size)) { + LG << "Fell out of image1's window with x1=" << x1 << ", y1=" << y1 + << ", hw=" << half_window_size << "."; + return false; + } + + Array3Df image_and_gradient1; + Array3Df image_and_gradient2; + BlurredImageAndDerivativesChannels(image1, sigma, &image_and_gradient1); + BlurredImageAndDerivativesChannels(image2, sigma, &image_and_gradient2); + + int i; + Vec2f d = Vec2f::Zero(); + for (i = 0; i < max_iterations; ++i) { + // Check that the entire image patch is within the bounds of the images. + if (!RegionIsInBounds(image2, *x2, *y2, half_window_size)) { + LG << "Fell out of image2's window with x2=" << *x2 << ", y2=" << *y2 + << ", hw=" << half_window_size << "."; + return false; + } + + // Compute gradient matrix and error vector. + Mat2f U; + Vec2f e; + ComputeTrackingEquation(image_and_gradient1, + image_and_gradient2, + x1, y1, + *x2, *y2, + half_window_size, + lambda, + &U, &e); + + // Solve the linear system for the best update to x2 and y2. + d = U.lu().solve(e); + + // Update the position with the solved displacement. + *x2 += d[0]; + *y2 += d[1]; + + // Check for the quality of the solution, but not until having already + // updated the position with our best estimate. The reason to do the update + // anyway is that the user already knows the position is bad, so we may as + // well try our best. + float determinant = U.determinant(); + if (fabs(determinant) < min_determinant) { + // The determinant, which indicates the trackiness of the point, is too + // small, so fail out. + LG << "Determinant " << determinant << " is too small; failing tracking."; + return false; + } + LG << "x=" << *x2 << ", y=" << *y2 << ", dx=" << d[0] << ", dy=" << d[1] + << ", det=" << determinant; + + + // If the update is small, then we probably found the target. + if (d.squaredNorm() < min_update_squared_distance) { + LG << "Successful track in " << i << " iterations."; + return true; + } + } + // Getting here means we hit max iterations, so tracking failed. + LG << "Too many iterations; max is set to " << max_iterations << "."; + return false; +} + +} // namespace libmv |