libmv/tracking/track_region.cc

Go to the documentation of this file.

// Copyright (c) 2012 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.
//
// Author: mierle@gmail.com (Keir Mierle)
//
// TODO(keir): While this tracking code works rather well, it has some
// outragous inefficiencies. There is probably a 5-10x speedup to be had if a
// smart coder went through the TODO's and made the suggested performance
// enhancements.
 
#include "libmv/tracking/track_region.h"
 
#include <Eigen/QR>
#include <Eigen/SVD>
#include <iostream>
#include "ceres/ceres.h"
#include "libmv/image/convolve.h"
#include "libmv/image/image.h"
#include "libmv/image/sample.h"
#include "libmv/logging/logging.h"
#include "libmv/multiview/homography.h"
#include "libmv/numeric/numeric.h"
 
// Expand the Jet functionality of Ceres to allow mixed numeric/autodiff.
//
// TODO(keir): Push this (or something similar) into upstream Ceres.
namespace ceres {
 
// A jet traits class to make it easier to work with mixed auto / numeric diff.
template <typename T>
struct JetOps {
  static bool IsScalar() { return true; }
  static T GetScalar(const T& t) { return t; }
  static void SetScalar(const T& scalar, T* t) { *t = scalar; }
  static void ScaleDerivative(double scale_by, T* value) {
    // For double, there is no derivative to scale.
    (void)scale_by;  // Ignored.
    (void)value;     // Ignored.
  }
};
 
template <typename T, int N>
struct JetOps<Jet<T, N>> {
  static bool IsScalar() { return false; }
  static T GetScalar(const Jet<T, N>& t) { return t.a; }
  static void SetScalar(const T& scalar, Jet<T, N>* t) { t->a = scalar; }
  static void ScaleDerivative(double scale_by, Jet<T, N>* value) {
    value->v *= scale_by;
  }
};
 
template <typename FunctionType, int kNumArgs, typename ArgumentType>
struct Chain {
  static ArgumentType Rule(const FunctionType& f,
                           const FunctionType dfdx[kNumArgs],
                           const ArgumentType x[kNumArgs]) {
    // In the default case of scalars, there's nothing to do since there are no
    // derivatives to propagate.
    (void)dfdx;  // Ignored.
    (void)x;     // Ignored.
    return f;
  }
};
 
// XXX Add documentation here!
template <typename FunctionType, int kNumArgs, typename T, int N>
struct Chain<FunctionType, kNumArgs, Jet<T, N>> {
  static Jet<T, N> Rule(const FunctionType& f,
                        const FunctionType dfdx[kNumArgs],
                        const Jet<T, N> x[kNumArgs]) {
    // x is itself a function of another variable ("z"); what this function
    // needs to return is "f", but with the derivative with respect to z
    // attached to the jet. So combine the derivative part of x's jets to form
    // a Jacobian matrix between x and z (i.e. dx/dz).
    Eigen::Matrix<T, kNumArgs, N> dxdz;
    for (int i = 0; i < kNumArgs; ++i) {
      dxdz.row(i) = x[i].v.transpose();
    }
 
    // Map the input gradient dfdx into an Eigen row vector.
    Eigen::Map<const Eigen::Matrix<FunctionType, 1, kNumArgs>> vector_dfdx(
        dfdx, 1, kNumArgs);
 
    // Now apply the chain rule to obtain df/dz. Combine the derivative with
    // the scalar part to obtain f with full derivative information.
    Jet<T, N> jet_f;
    jet_f.a = f;
    jet_f.v = vector_dfdx.template cast<T>() * dxdz;  // Also known as dfdz.
    return jet_f;
  }
};
 
}  // namespace ceres
 
namespace libmv {
 
using ceres::Chain;
using ceres::Jet;
using ceres::JetOps;
 
TrackRegionOptions::TrackRegionOptions()
    : direction(FORWARD),
      mode(TRANSLATION),
      minimum_correlation(0),
      max_iterations(20),
      use_esm(true),
      use_brute_initialization(true),
      use_normalized_intensities(false),
      sigma(0.9),
      num_extra_points(0),
      regularization_coefficient(0.0),
      minimum_corner_shift_tolerance_pixels(0.005),
      image1_mask(NULL) {
}
 
namespace {
 
// TODO(keir): Consider adding padding.
template <typename T>
bool InBounds(const FloatImage& image, const T& x, const T& y) {
  return 0.0 <= x && x < image.Width() && 0.0 <= y && y < image.Height();
}
 
bool AllInBounds(const FloatImage& image, const double* x, const double* y) {
  for (int i = 0; i < 4; ++i) {
    if (!InBounds(image, x[i], y[i])) {
      return false;
    }
  }
  return true;
}
 
// Sample the image at position (x, y) but use the gradient, if present, to
// propagate derivatives from x and y. This is needed to integrate the numeric
// image gradients with Ceres's autodiff framework.
template <typename T>
static T SampleWithDerivative(const FloatImage& image_and_gradient,
                              const T& x,
                              const T& y) {
  float scalar_x = JetOps<T>::GetScalar(x);
  float scalar_y = JetOps<T>::GetScalar(y);
 
  // Note that sample[1] and sample[2] will be uninitialized in the scalar
  // case, but that is not an issue because the Chain::Rule below will not read
  // the uninitialized values.
  float sample[3];
  if (JetOps<T>::IsScalar()) {
    // For the scalar case, only sample the image.
    sample[0] = SampleLinear(image_and_gradient, scalar_y, scalar_x, 0);
  } else {
    // For the derivative case, sample the gradient as well.
    SampleLinear(image_and_gradient, scalar_y, scalar_x, sample);
  }
  T xy[2] = {x, y};
  return Chain<float, 2, T>::Rule(sample[0], sample + 1, xy);
}
 
template <typename Warp>
class TerminationCheckingCallback : public ceres::IterationCallback {
 public:
  TerminationCheckingCallback(const TrackRegionOptions& options,
                              const FloatImage& image2,
                              const Warp& warp,
                              const double* x1,
                              const double* y1)
      : options_(options),
        image2_(image2),
        warp_(warp),
        x1_(x1),
        y1_(y1),
        have_last_successful_step_(false) {}
 
  virtual ceres::CallbackReturnType operator()(
      const ceres::IterationSummary& summary) {
    // If the step wasn't successful, there's nothing to do.
    if (!summary.step_is_successful) {
      return ceres::SOLVER_CONTINUE;
    }
    // Warp the original 4 points with the current warp into image2.
    double x2[4];
    double y2[4];
    for (int i = 0; i < 4; ++i) {
      warp_.Forward(warp_.parameters, x1_[i], y1_[i], x2 + i, y2 + i);
    }
    // Ensure the corners are all in bounds.
    if (!AllInBounds(image2_, x2, y2)) {
      LG << "Successful step fell outside of the pattern bounds; aborting.";
      return ceres::SOLVER_ABORT;
    }
 
    // Ensure the minimizer is making large enough shifts to bother continuing.
    // Ideally, this check would happen on the parameters themselves which
    // Ceres supports directly; however, the mapping from parameter change
    // magnitude to corner movement in pixels is not a simple norm. Hence, the
    // need for a stateful callback which tracks the last successful set of
    // parameters (and the position of the projected patch corners).
    if (have_last_successful_step_) {
      // Compute the maximum shift of any corner in pixels since the last
      // successful iteration.
      double max_change_pixels = 0;
      for (int i = 0; i < 4; ++i) {
        double dx = x2[i] - x2_last_successful_[i];
        double dy = y2[i] - y2_last_successful_[i];
        double change_pixels = dx * dx + dy * dy;
        if (change_pixels > max_change_pixels) {
          max_change_pixels = change_pixels;
        }
      }
      max_change_pixels = sqrt(max_change_pixels);
      LG << "Max patch corner shift is " << max_change_pixels;
 
      // Bail if the shift is too small.
      if (max_change_pixels < options_.minimum_corner_shift_tolerance_pixels) {
        LG << "Max patch corner shift is " << max_change_pixels
           << " from the last iteration; returning success.";
        return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
      }
    }
 
    // Save the projected corners for checking on the next successful iteration.
    for (int i = 0; i < 4; ++i) {
      x2_last_successful_[i] = x2[i];
      y2_last_successful_[i] = y2[i];
    }
    have_last_successful_step_ = true;
    return ceres::SOLVER_CONTINUE;
  }
 
 private:
  const TrackRegionOptions& options_;
  const FloatImage& image2_;
  const Warp& warp_;
  const double* x1_;
  const double* y1_;
 
  bool have_last_successful_step_;
  double x2_last_successful_[4];
  double y2_last_successful_[4];
};
 
template <typename Warp>
class PixelDifferenceCostFunctor {
 public:
  PixelDifferenceCostFunctor(const TrackRegionOptions& options,
                             const FloatImage& image_and_gradient1,
                             const FloatImage& image_and_gradient2,
                             const Mat3& canonical_to_image1,
                             int num_samples_x,
                             int num_samples_y,
                             const Warp& warp)
      : options_(options),
        image_and_gradient1_(image_and_gradient1),
        image_and_gradient2_(image_and_gradient2),
        canonical_to_image1_(canonical_to_image1),
        num_samples_x_(num_samples_x),
        num_samples_y_(num_samples_y),
        warp_(warp),
        pattern_and_gradient_(num_samples_y_, num_samples_x_, 3),
        pattern_positions_(num_samples_y_, num_samples_x_, 2),
        pattern_mask_(num_samples_y_, num_samples_x_, 1) {
    ComputeCanonicalPatchAndNormalizer();
  }
 
  void ComputeCanonicalPatchAndNormalizer() {
    src_mean_ = 0.0;
    double num_samples = 0.0;
    for (int r = 0; r < num_samples_y_; ++r) {
      for (int c = 0; c < num_samples_x_; ++c) {
        // Compute the position; cache it.
        Vec3 image_position = canonical_to_image1_ * Vec3(c, r, 1);
        image_position /= image_position(2);
        pattern_positions_(r, c, 0) = image_position(0);
        pattern_positions_(r, c, 1) = image_position(1);
 
        // Sample the pattern and gradients.
        SampleLinear(image_and_gradient1_,
                     image_position(1),  // SampleLinear is r, c.
                     image_position(0),
                     &pattern_and_gradient_(r, c, 0));
 
        // Sample sample the mask.
        double mask_value = 1.0;
        if (options_.image1_mask != NULL) {
          SampleLinear(*options_.image1_mask,
                       image_position(1),  // SampleLinear is r, c.
                       image_position(0),
                       &pattern_mask_(r, c, 0));
          mask_value = pattern_mask_(r, c);
        }
        src_mean_ += pattern_and_gradient_(r, c, 0) * mask_value;
        num_samples += mask_value;
      }
    }
    src_mean_ /= num_samples;
  }
 
  template <typename T>
  bool operator()(const T* warp_parameters, T* residuals) const {
    if (options_.image1_mask != NULL) {
      VLOG(2) << "Using a mask.";
    }
    for (int i = 0; i < Warp::NUM_PARAMETERS; ++i) {
      VLOG(2) << "warp_parameters[" << i << "]: " << warp_parameters[i];
    }
 
    T dst_mean = T(1.0);
    if (options_.use_normalized_intensities) {
      ComputeNormalizingCoefficient(warp_parameters, &dst_mean);
    }
 
    int cursor = 0;
    for (int r = 0; r < num_samples_y_; ++r) {
      for (int c = 0; c < num_samples_x_; ++c) {
        // Use the pre-computed image1 position.
        Vec2 image1_position(pattern_positions_(r, c, 0),
                             pattern_positions_(r, c, 1));
 
        // Sample the mask early; if it's zero, this pixel has no effect. This
        // allows early bailout from the expensive sampling that happens below.
        //
        // Note that partial masks are not short circuited. To see why short
        // circuiting produces bitwise-exact same results, consider that the
        // residual for each pixel is
        //
        //    residual = mask * (src - dst)  ,
        //
        // and for jets, multiplying by a scalar multiplies the derivative
        // components by the scalar as well. Therefore, if the mask is exactly
        // zero, then so too will the final residual and derivatives.
        double mask_value = 1.0;
        if (options_.image1_mask != NULL) {
          mask_value = pattern_mask_(r, c);
          if (mask_value == 0.0) {
            residuals[cursor++] = T(0.0);
            continue;
          }
        }
 
        // Compute the location of the destination pixel.
        T image2_position[2];
        warp_.Forward(warp_parameters,
                      T(image1_position[0]),
                      T(image1_position[1]),
                      &image2_position[0],
                      &image2_position[1]);
 
        // Sample the destination, propagating derivatives.
        T dst_sample = SampleWithDerivative(
            image_and_gradient2_, image2_position[0], image2_position[1]);
 
        // Sample the source. This is made complicated by ESM mode.
        T src_sample;
        if (options_.use_esm && !JetOps<T>::IsScalar()) {
          // In ESM mode, the derivative of the source is also taken into
          // account. This changes the linearization in a way that causes
          // better convergence. Copy the derivative of the warp parameters
          // onto the jets for the image1 position. This is the ESM hack.
          T image1_position_jet[2] = {
              image2_position[0],  // Order is x, y. This matches the
              image2_position[1]   // derivative order in the patch.
          };
          JetOps<T>::SetScalar(image1_position[0], image1_position_jet + 0);
          JetOps<T>::SetScalar(image1_position[1], image1_position_jet + 1);
 
          // Now that the image1 positions have the jets applied from the
          // image2 position (the ESM hack), chain the image gradients to
          // obtain a sample with the derivative with respect to the warp
          // parameters attached.
          src_sample = Chain<float, 2, T>::Rule(pattern_and_gradient_(r, c),
                                                &pattern_and_gradient_(r, c, 1),
                                                image1_position_jet);
 
          // The jacobians for these should be averaged. Due to the subtraction
          // below, flip the sign of the src derivative so that the effect
          // after subtraction of the jets is that they are averaged.
          JetOps<T>::ScaleDerivative(-0.5, &src_sample);
          JetOps<T>::ScaleDerivative(0.5, &dst_sample);
        } else {
          // This is the traditional, forward-mode KLT solution.
          src_sample = T(pattern_and_gradient_(r, c));
        }
 
        // Normalize the samples by the mean values of each signal. The typical
        // light model assumes multiplicative intensity changes with changing
        // light, so this is a reasonable choice. Note that dst_mean has
        // derivative information attached thanks to autodiff.
        if (options_.use_normalized_intensities) {
          src_sample /= T(src_mean_);
          dst_sample /= dst_mean;
        }
 
        // The difference is the error.
        T error = src_sample - dst_sample;
 
        // Weight the error by the mask, if one is present.
        if (options_.image1_mask != NULL) {
          error *= T(mask_value);
        }
        residuals[cursor++] = error;
      }
    }
    return true;
  }
 
  // For normalized matching, the average and
  template <typename T>
  void ComputeNormalizingCoefficient(const T* warp_parameters,
                                     T* dst_mean) const {
    *dst_mean = T(0.0);
    double num_samples = 0.0;
    for (int r = 0; r < num_samples_y_; ++r) {
      for (int c = 0; c < num_samples_x_; ++c) {
        // Use the pre-computed image1 position.
        Vec2 image1_position(pattern_positions_(r, c, 0),
                             pattern_positions_(r, c, 1));
 
        // Sample the mask early; if it's zero, this pixel has no effect. This
        // allows early bailout from the expensive sampling that happens below.
        double mask_value = 1.0;
        if (options_.image1_mask != NULL) {
          mask_value = pattern_mask_(r, c);
          if (mask_value == 0.0) {
            continue;
          }
        }
 
        // Compute the location of the destination pixel.
        T image2_position[2];
        warp_.Forward(warp_parameters,
                      T(image1_position[0]),
                      T(image1_position[1]),
                      &image2_position[0],
                      &image2_position[1]);
 
        // Sample the destination, propagating derivatives.
        // TODO(keir): This accumulation can, surprisingly, be done as a
        // pre-pass by using integral images. This is complicated by the need
        // to store the jets in the integral image, but it is possible.
        T dst_sample = SampleWithDerivative(
            image_and_gradient2_, image2_position[0], image2_position[1]);
 
        // Weight the sample by the mask, if one is present.
        if (options_.image1_mask != NULL) {
          dst_sample *= T(mask_value);
        }
 
        *dst_mean += dst_sample;
        num_samples += mask_value;
      }
    }
    *dst_mean /= T(num_samples);
    LG << "Normalization for dst:" << *dst_mean;
  }
 
  // TODO(keir): Consider also computing the cost here.
  double PearsonProductMomentCorrelationCoefficient(
      const double* warp_parameters) const {
    for (int i = 0; i < Warp::NUM_PARAMETERS; ++i) {
      VLOG(2) << "Correlation warp_parameters[" << i
              << "]: " << warp_parameters[i];
    }
 
    // The single-pass PMCC computation is somewhat numerically unstable, but
    // it's sufficient for the tracker.
    double sX = 0, sY = 0, sXX = 0, sYY = 0, sXY = 0;
 
    // Due to masking, it's important to account for fractional samples.
    // For example, samples with a 50% mask are counted as a half sample.
    double num_samples = 0;
 
    for (int r = 0; r < num_samples_y_; ++r) {
      for (int c = 0; c < num_samples_x_; ++c) {
        // Use the pre-computed image1 position.
        Vec2 image1_position(pattern_positions_(r, c, 0),
                             pattern_positions_(r, c, 1));
 
        double mask_value = 1.0;
        if (options_.image1_mask != NULL) {
          mask_value = pattern_mask_(r, c);
          if (mask_value == 0.0) {
            continue;
          }
        }
 
        // Compute the location of the destination pixel.
        double image2_position[2];
        warp_.Forward(warp_parameters,
                      image1_position[0],
                      image1_position[1],
                      &image2_position[0],
                      &image2_position[1]);
 
        double x = pattern_and_gradient_(r, c);
        double y = SampleLinear(image_and_gradient2_,
                                image2_position[1],  // SampleLinear is r, c.
                                image2_position[0]);
 
        // Weight the signals by the mask, if one is present.
        if (options_.image1_mask != NULL) {
          x *= mask_value;
          y *= mask_value;
          num_samples += mask_value;
        } else {
          num_samples++;
        }
        sX += x;
        sY += y;
        sXX += x * x;
        sYY += y * y;
        sXY += x * y;
      }
    }
    // Normalize.
    sX /= num_samples;
    sY /= num_samples;
    sXX /= num_samples;
    sYY /= num_samples;
    sXY /= num_samples;
 
    double var_x = sXX - sX * sX;
    double var_y = sYY - sY * sY;
    double covariance_xy = sXY - sX * sY;
 
    double correlation = covariance_xy / sqrt(var_x * var_y);
    LG << "Covariance xy: " << covariance_xy << ", var 1: " << var_x
       << ", var 2: " << var_y << ", correlation: " << correlation;
    return correlation;
  }
 
 private:
  const TrackRegionOptions& options_;
  const FloatImage& image_and_gradient1_;
  const FloatImage& image_and_gradient2_;
  const Mat3& canonical_to_image1_;
  int num_samples_x_;
  int num_samples_y_;
  const Warp& warp_;
  double src_mean_;
  FloatImage pattern_and_gradient_;
 
  // This contains the position from where the cached pattern samples were
  // taken from. This is also used to warp from src to dest without going from
  // canonical pixels to src first.
  FloatImage pattern_positions_;
 
  FloatImage pattern_mask_;
};
 
template <typename Warp>
class WarpRegularizingCostFunctor {
 public:
  WarpRegularizingCostFunctor(const TrackRegionOptions& options,
                              const double* x1,
                              const double* y1,
                              const double* x2_original,
                              const double* y2_original,
                              const Warp& warp)
      : options_(options),
        x1_(x1),
        y1_(y1),
        x2_original_(x2_original),
        y2_original_(y2_original),
        warp_(warp) {
    // Compute the centroid of the first guess quad.
    // TODO(keir): Use Quad class here.
    original_centroid_[0] = 0.0;
    original_centroid_[1] = 0.0;
    for (int i = 0; i < 4; ++i) {
      original_centroid_[0] += x2_original[i];
      original_centroid_[1] += y2_original[i];
    }
    original_centroid_[0] /= 4;
    original_centroid_[1] /= 4;
  }
 
  template <typename T>
  bool operator()(const T* warp_parameters, T* residuals) const {
    T dst_centroid[2] = {T(0.0), T(0.0)};
    for (int i = 0; i < 4; ++i) {
      T image1_position[2] = {T(x1_[i]), T(y1_[i])};
      T image2_position[2];
      warp_.Forward(warp_parameters,
                    T(x1_[i]),
                    T(y1_[i]),
                    &image2_position[0],
                    &image2_position[1]);
 
      // Subtract the positions. Note that this ignores the centroids.
      residuals[2 * i + 0] = image2_position[0] - image1_position[0];
      residuals[2 * i + 1] = image2_position[1] - image1_position[1];
 
      // Accumulate the dst centroid.
      dst_centroid[0] += image2_position[0];
      dst_centroid[1] += image2_position[1];
    }
    dst_centroid[0] /= T(4.0);
    dst_centroid[1] /= T(4.0);
 
    // Adjust for the centroids.
    for (int i = 0; i < 4; ++i) {
      residuals[2 * i + 0] += T(original_centroid_[0]) - dst_centroid[0];
      residuals[2 * i + 1] += T(original_centroid_[1]) - dst_centroid[1];
    }
 
    // Reweight the residuals.
    for (int i = 0; i < 8; ++i) {
      residuals[i] *= T(options_.regularization_coefficient);
    }
 
    return true;
  }
 
  const TrackRegionOptions& options_;
  const double* x1_;
  const double* y1_;
  const double* x2_original_;
  const double* y2_original_;
  double original_centroid_[2];
  const Warp& warp_;
};
 
// Compute the warp from rectangular coordinates, where one corner is the
// origin, and the opposite corner is at (num_samples_x, num_samples_y).
Mat3 ComputeCanonicalHomography(const double* x1,
                                const double* y1,
                                int num_samples_x,
                                int num_samples_y) {
  Mat canonical(2, 4);
  canonical << 0, num_samples_x, num_samples_x, 0, 0, 0, num_samples_y,
      num_samples_y;
 
  Mat xy1(2, 4);
  // clang-format off
  xy1 << x1[0], x1[1], x1[2], x1[3],
         y1[0], y1[1], y1[2], y1[3];
  // clang-format om
 
  Mat3 H;
  if (!Homography2DFromCorrespondencesLinear(canonical, xy1, &H, 1e-12)) {
    LG << "Couldn't construct homography.";
  }
  return H;
}
 
class Quad {
 public:
  Quad(const double* x, const double* y) : x_(x), y_(y) {
    // Compute the centroid and store it.
    centroid_ = Vec2(0.0, 0.0);
    for (int i = 0; i < 4; ++i) {
      centroid_ += Vec2(x_[i], y_[i]);
    }
    centroid_ /= 4.0;
  }
 
  // The centroid of the four points representing the quad.
  const Vec2& Centroid() const { return centroid_; }
 
  // The average magnitude of the four points relative to the centroid.
  double Scale() const {
    double scale = 0.0;
    for (int i = 0; i < 4; ++i) {
      scale += (Vec2(x_[i], y_[i]) - Centroid()).norm();
    }
    return scale / 4.0;
  }
 
  Vec2 CornerRelativeToCentroid(int i) const {
    return Vec2(x_[i], y_[i]) - centroid_;
  }
 
 private:
  const double* x_;
  const double* y_;
  Vec2 centroid_;
};
 
struct TranslationWarp {
  TranslationWarp(const double* x1,
                  const double* y1,
                  const double* x2,
                  const double* y2) {
    Vec2 t = Quad(x2, y2).Centroid() - Quad(x1, y1).Centroid();
    parameters[0] = t[0];
    parameters[1] = t[1];
  }
 
  template <typename T>
  void Forward(
      const T* warp_parameters, const T& x1, const T& y1, T* x2, T* y2) const {
    *x2 = x1 + warp_parameters[0];
    *y2 = y1 + warp_parameters[1];
  }
 
  // Translation x, translation y.
  enum { NUM_PARAMETERS = 2 };
  double parameters[NUM_PARAMETERS];
};
 
struct TranslationScaleWarp {
  TranslationScaleWarp(const double* x1,
                       const double* y1,
                       const double* x2,
                       const double* y2)
      : q1(x1, y1) {
    Quad q2(x2, y2);
 
    // The difference in centroids is the best guess for translation.
    Vec2 t = q2.Centroid() - q1.Centroid();
    parameters[0] = t[0];
    parameters[1] = t[1];
 
    // The difference in scales is the estimate for the scale.
    parameters[2] = 1.0 - q2.Scale() / q1.Scale();
  }
 
  // The strange way of parameterizing the translation and scaling is to make
  // the knobs that the optimizer sees easy to adjust. This is less important
  // for the scaling case than the rotation case.
  template <typename T>
  void Forward(
      const T* warp_parameters, const T& x1, const T& y1, T* x2, T* y2) const {
    // Make the centroid of Q1 the origin.
    const T x1_origin = x1 - q1.Centroid()(0);
    const T y1_origin = y1 - q1.Centroid()(1);
 
    // Scale uniformly about the origin.
    const T scale = 1.0 + warp_parameters[2];
    const T x1_origin_scaled = scale * x1_origin;
    const T y1_origin_scaled = scale * y1_origin;
 
    // Translate back into the space of Q1 (but scaled).
    const T x1_scaled = x1_origin_scaled + q1.Centroid()(0);
    const T y1_scaled = y1_origin_scaled + q1.Centroid()(1);
 
    // Translate into the space of Q2.
    *x2 = x1_scaled + warp_parameters[0];
    *y2 = y1_scaled + warp_parameters[1];
  }
 
  // Translation x, translation y, scale.
  enum { NUM_PARAMETERS = 3 };
  double parameters[NUM_PARAMETERS];
 
  Quad q1;
};
 
// Assumes the given points are already zero-centroid and the same size.
Mat2 OrthogonalProcrustes(const Mat2& correlation_matrix) {
  Eigen::JacobiSVD<Mat2> svd(correlation_matrix,
                             Eigen::ComputeFullU | Eigen::ComputeFullV);
  return svd.matrixV() * svd.matrixU().transpose();
}
 
struct TranslationRotationWarp {
  TranslationRotationWarp(const double* x1,
                          const double* y1,
                          const double* x2,
                          const double* y2)
      : q1(x1, y1) {
    Quad q2(x2, y2);
 
    // The difference in centroids is the best guess for translation.
    Vec2 t = q2.Centroid() - q1.Centroid();
    parameters[0] = t[0];
    parameters[1] = t[1];
 
    // Obtain the rotation via orthorgonal procrustes.
    Mat2 correlation_matrix = Mat2::Zero();
    for (int i = 0; i < 4; ++i) {
      correlation_matrix += q1.CornerRelativeToCentroid(i) *
                            q2.CornerRelativeToCentroid(i).transpose();
    }
    Mat2 R = OrthogonalProcrustes(correlation_matrix);
    parameters[2] = atan2(R(1, 0), R(0, 0));
 
    LG << "Correlation_matrix:\n" << correlation_matrix;
    LG << "R:\n" << R;
    LG << "Theta:" << parameters[2];
  }
 
  // The strange way of parameterizing the translation and rotation is to make
  // the knobs that the optimizer sees easy to adjust. The reason is that while
  // it is always the case that it is possible to express composed rotations
  // and translations as a single translation and rotation, the numerical
  // values needed for the composition are often large in magnitude. This is
  // enough to throw off any minimizer, since it must do the equivalent of
  // compose rotations and translations.
  //
  // Instead, use the parameterization below that offers a parameterization
  // that exposes the degrees of freedom in a way amenable to optimization.
  template <typename T>
  void Forward(
      const T* warp_parameters, const T& x1, const T& y1, T* x2, T* y2) const {
    // Make the centroid of Q1 the origin.
    const T x1_origin = x1 - q1.Centroid()(0);
    const T y1_origin = y1 - q1.Centroid()(1);
 
    // Rotate about the origin (i.e. centroid of Q1).
    const T theta = warp_parameters[2];
    const T costheta = cos(theta);
    const T sintheta = sin(theta);
    const T x1_origin_rotated = costheta * x1_origin - sintheta * y1_origin;
    const T y1_origin_rotated = sintheta * x1_origin + costheta * y1_origin;
 
    // Translate back into the space of Q1 (but scaled).
    const T x1_rotated = x1_origin_rotated + q1.Centroid()(0);
    const T y1_rotated = y1_origin_rotated + q1.Centroid()(1);
 
    // Translate into the space of Q2.
    *x2 = x1_rotated + warp_parameters[0];
    *y2 = y1_rotated + warp_parameters[1];
  }
 
  // Translation x, translation y, rotation about the center of Q1 degrees.
  enum { NUM_PARAMETERS = 3 };
  double parameters[NUM_PARAMETERS];
 
  Quad q1;
};
 
struct TranslationRotationScaleWarp {
  TranslationRotationScaleWarp(const double* x1,
                               const double* y1,
                               const double* x2,
                               const double* y2)
      : q1(x1, y1) {
    Quad q2(x2, y2);
 
    // The difference in centroids is the best guess for translation.
    Vec2 t = q2.Centroid() - q1.Centroid();
    parameters[0] = t[0];
    parameters[1] = t[1];
 
    // The difference in scales is the estimate for the scale.
    parameters[2] = 1.0 - q2.Scale() / q1.Scale();
 
    // Obtain the rotation via orthorgonal procrustes.
    Mat2 correlation_matrix = Mat2::Zero();
    for (int i = 0; i < 4; ++i) {
      correlation_matrix += q1.CornerRelativeToCentroid(i) *
                            q2.CornerRelativeToCentroid(i).transpose();
    }
    Mat2 R = OrthogonalProcrustes(correlation_matrix);
    parameters[3] = atan2(R(1, 0), R(0, 0));
 
    LG << "Correlation_matrix:\n" << correlation_matrix;
    LG << "R:\n" << R;
    LG << "Theta:" << parameters[3];
  }
 
  // The strange way of parameterizing the translation and rotation is to make
  // the knobs that the optimizer sees easy to adjust. The reason is that while
  // it is always the case that it is possible to express composed rotations
  // and translations as a single translation and rotation, the numerical
  // values needed for the composition are often large in magnitude. This is
  // enough to throw off any minimizer, since it must do the equivalent of
  // compose rotations and translations.
  //
  // Instead, use the parameterization below that offers a parameterization
  // that exposes the degrees of freedom in a way amenable to optimization.
  template <typename T>
  void Forward(
      const T* warp_parameters, const T& x1, const T& y1, T* x2, T* y2) const {
    // Make the centroid of Q1 the origin.
    const T x1_origin = x1 - q1.Centroid()(0);
    const T y1_origin = y1 - q1.Centroid()(1);
 
    // Rotate about the origin (i.e. centroid of Q1).
    const T theta = warp_parameters[3];
    const T costheta = cos(theta);
    const T sintheta = sin(theta);
    const T x1_origin_rotated = costheta * x1_origin - sintheta * y1_origin;
    const T y1_origin_rotated = sintheta * x1_origin + costheta * y1_origin;
 
    // Scale uniformly about the origin.
    const T scale = 1.0 + warp_parameters[2];
    const T x1_origin_rotated_scaled = scale * x1_origin_rotated;
    const T y1_origin_rotated_scaled = scale * y1_origin_rotated;
 
    // Translate back into the space of Q1 (but scaled and rotated).
    const T x1_rotated_scaled = x1_origin_rotated_scaled + q1.Centroid()(0);
    const T y1_rotated_scaled = y1_origin_rotated_scaled + q1.Centroid()(1);
 
    // Translate into the space of Q2.
    *x2 = x1_rotated_scaled + warp_parameters[0];
    *y2 = y1_rotated_scaled + warp_parameters[1];
  }
 
  // Translation x, translation y, rotation about the center of Q1 degrees,
  // scale.
  enum { NUM_PARAMETERS = 4 };
  double parameters[NUM_PARAMETERS];
 
  Quad q1;
};
 
struct AffineWarp {
  AffineWarp(const double* x1,
             const double* y1,
             const double* x2,
             const double* y2)
      : q1(x1, y1) {
    Quad q2(x2, y2);
 
    // The difference in centroids is the best guess for translation.
    Vec2 t = q2.Centroid() - q1.Centroid();
    parameters[0] = t[0];
    parameters[1] = t[1];
 
    // Estimate the four affine parameters with the usual least squares.
    Mat Q1(8, 4);
    Vec Q2(8);
    for (int i = 0; i < 4; ++i) {
      Vec2 v1 = q1.CornerRelativeToCentroid(i);
      Vec2 v2 = q2.CornerRelativeToCentroid(i);
 
      Q1.row(2 * i + 0) << v1[0], v1[1], 0, 0;
      Q1.row(2 * i + 1) << 0, 0, v1[0], v1[1];
 
      Q2(2 * i + 0) = v2[0];
      Q2(2 * i + 1) = v2[1];
    }
 
    // TODO(keir): Check solution quality.
    Vec4 a = Q1.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Q2);
    parameters[2] = a[0];
    parameters[3] = a[1];
    parameters[4] = a[2];
    parameters[5] = a[3];
 
    LG << "a:" << a.transpose();
    LG << "t:" << t.transpose();
  }
 
  // See comments in other parameterizations about why the centroid is used.
  template <typename T>
  void Forward(const T* p, const T& x1, const T& y1, T* x2, T* y2) const {
    // Make the centroid of Q1 the origin.
    const T x1_origin = x1 - q1.Centroid()(0);
    const T y1_origin = y1 - q1.Centroid()(1);
 
    // Apply the affine transformation.
    const T x1_origin_affine = p[2] * x1_origin + p[3] * y1_origin;
    const T y1_origin_affine = p[4] * x1_origin + p[5] * y1_origin;
 
    // Translate back into the space of Q1 (but affine transformed).
    const T x1_affine = x1_origin_affine + q1.Centroid()(0);
    const T y1_affine = y1_origin_affine + q1.Centroid()(1);
 
    // Translate into the space of Q2.
    *x2 = x1_affine + p[0];
    *y2 = y1_affine + p[1];
  }
 
  // Translation x, translation y, rotation about the center of Q1 degrees,
  // scale.
  enum { NUM_PARAMETERS = 6 };
  double parameters[NUM_PARAMETERS];
 
  Quad q1;
};
 
struct HomographyWarp {
  HomographyWarp(const double* x1,
                 const double* y1,
                 const double* x2,
                 const double* y2) {
    Mat quad1(2, 4);
    // clang-format off
    quad1 << x1[0], x1[1], x1[2], x1[3],
             y1[0], y1[1], y1[2], y1[3];
    // clang-format on
 
    Mat quad2(2, 4);
    // clang-format off
    quad2 << x2[0], x2[1], x2[2], x2[3],
             y2[0], y2[1], y2[2], y2[3];
    // clang-format on
 
    Mat3 H;
    if (!Homography2DFromCorrespondencesLinear(quad1, quad2, &H, 1e-12)) {
      LG << "Couldn't construct homography.";
    }
 
    // Assume H(2, 2) != 0, and fix scale at H(2, 2) == 1.0.
    H /= H(2, 2);
 
    // Assume H is close to identity, so subtract out the diagonal.
    H(0, 0) -= 1.0;
    H(1, 1) -= 1.0;
 
    CHECK_NE(H(2, 2), 0.0) << H;
    for (int i = 0; i < 8; ++i) {
      parameters[i] = H(i / 3, i % 3);
      LG << "Parameters[" << i << "]: " << parameters[i];
    }
  }
 
  template <typename T>
  static void Forward(const T* p, const T& x1, const T& y1, T* x2, T* y2) {
    // Homography warp with manual 3x3 matrix multiply.
    const T xx2 = (1.0 + p[0]) * x1 + p[1] * y1 + p[2];
    const T yy2 = p[3] * x1 + (1.0 + p[4]) * y1 + p[5];
    const T zz2 = p[6] * x1 + p[7] * y1 + 1.0;
    *x2 = xx2 / zz2;
    *y2 = yy2 / zz2;
  }
 
  enum { NUM_PARAMETERS = 8 };
  double parameters[NUM_PARAMETERS];
};
 
// Determine the number of samples to use for x and y. Quad winding goes:
//
//    0 1
//    3 2
//
// The idea is to take the maximum x or y distance. This may be oversampling.
// TODO(keir): Investigate the various choices; perhaps average is better?
void PickSampling(const double* x1,
                  const double* y1,
                  const double* x2,
                  const double* y2,
                  int* num_samples_x,
                  int* num_samples_y) {
  (void)x2;  // Ignored.
  (void)y2;  // Ignored.
 
  Vec2 a0(x1[0], y1[0]);
  Vec2 a1(x1[1], y1[1]);
  Vec2 a2(x1[2], y1[2]);
  Vec2 a3(x1[3], y1[3]);
 
  Vec2 b0(x1[0], y1[0]);
  Vec2 b1(x1[1], y1[1]);
  Vec2 b2(x1[2], y1[2]);
  Vec2 b3(x1[3], y1[3]);
 
  double x_dimensions[4] = {
      (a1 - a0).norm(), (a3 - a2).norm(), (b1 - b0).norm(), (b3 - b2).norm()};
 
  double y_dimensions[4] = {
      (a3 - a0).norm(), (a1 - a2).norm(), (b3 - b0).norm(), (b1 - b2).norm()};
  const double kScaleFactor = 1.0;
  *num_samples_x = static_cast<int>(
      kScaleFactor * *std::max_element(x_dimensions, x_dimensions + 4));
  *num_samples_y = static_cast<int>(
      kScaleFactor * *std::max_element(y_dimensions, y_dimensions + 4));
  LG << "Automatic num_samples_x: " << *num_samples_x
     << ", num_samples_y: " << *num_samples_y;
}
 
bool SearchAreaTooBigForDescent(const FloatImage& image2,
                                const double* x2,
                                const double* y2) {
  // TODO(keir): Check the bounds and enable only when it makes sense.
  (void)image2;  // Ignored.
  (void)x2;      // Ignored.
  (void)y2;      // Ignored.
 
  return true;
}
 
bool PointOnRightHalfPlane(const Vec2& a, const Vec2& b, double x, double y) {
  Vec2 ba = b - a;
  return ((Vec2(x, y) - b).transpose() * Vec2(-ba.y(), ba.x())) > 0;
}
 
// Determine if a point is in a quad. The quad is arranged as:
//
//    +-------> x
//    |
//    |  a0------a1
//    |   |       |
//    |   |       |
//    |   |       |
//    |  a3------a2
//    v
//    y
//
// The implementation does up to four half-plane comparisons.
bool PointInQuad(const double* xs, const double* ys, double x, double y) {
  Vec2 a0(xs[0], ys[0]);
  Vec2 a1(xs[1], ys[1]);
  Vec2 a2(xs[2], ys[2]);
  Vec2 a3(xs[3], ys[3]);
 
  return PointOnRightHalfPlane(a0, a1, x, y) &&
         PointOnRightHalfPlane(a1, a2, x, y) &&
         PointOnRightHalfPlane(a2, a3, x, y) &&
         PointOnRightHalfPlane(a3, a0, x, y);
}
 
// This makes it possible to map between Eigen float arrays and FloatImage
// without using comparisons.
typedef Eigen::Array<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
    FloatArray;
 
// This creates a pattern in the frame of image2, from the pixel is image1,
// based on the initial guess represented by the two quads x1, y1, and x2, y2.
template <typename Warp>
void CreateBrutePattern(const double* x1,
                        const double* y1,
                        const double* x2,
                        const double* y2,
                        const FloatImage& image1,
                        const FloatImage* image1_mask,
                        FloatArray* pattern,
                        FloatArray* mask,
                        int* origin_x,
                        int* origin_y) {
  // Get integer bounding box of quad2 in image2.
  int min_x = static_cast<int>(floor(*std::min_element(x2, x2 + 4)));
  int min_y = static_cast<int>(floor(*std::min_element(y2, y2 + 4)));
  int max_x = static_cast<int>(ceil(*std::max_element(x2, x2 + 4)));
  int max_y = static_cast<int>(ceil(*std::max_element(y2, y2 + 4)));
 
  int w = max_x - min_x;
  int h = max_y - min_y;
 
  pattern->resize(h, w);
  mask->resize(h, w);
 
  Warp inverse_warp(x2, y2, x1, y1);
 
  // r,c are in the coordinate frame of image2.
  for (int r = min_y; r < max_y; ++r) {
    for (int c = min_x; c < max_x; ++c) {
      // i and j are in the coordinate frame of the pattern in image2.
      int i = r - min_y;
      int j = c - min_x;
 
      double dst_x = c;
      double dst_y = r;
      double src_x;
      double src_y;
      inverse_warp.Forward(
          inverse_warp.parameters, dst_x, dst_y, &src_x, &src_y);
 
      if (PointInQuad(x1, y1, src_x, src_y)) {
        (*pattern)(i, j) = SampleLinear(image1, src_y, src_x);
        (*mask)(i, j) = 1.0;
        if (image1_mask) {
          (*mask)(i, j) = SampleLinear(*image1_mask, src_y, src_x);
        }
      } else {
        (*pattern)(i, j) = 0.0;
        (*mask)(i, j) = 0.0;
      }
    }
  }
  *origin_x = min_x;
  *origin_y = min_y;
}
 
// Compute a translation-only estimate of the warp, using brute force search. A
// smarter implementation would use the FFT to compute the normalized cross
// correlation. Instead, this is a dumb implementation. Surprisingly, it is
// fast enough in practice.
//
// Returns true if any alignment was found, and false if the projected pattern
// is zero sized.
//
// TODO(keir): The normalization is less effective for the brute force search
// than it is with the Ceres solver. It's unclear if this is a bug or due to
// the original frame being too different from the reprojected reference in the
// destination frame.
//
// The likely solution is to use the previous frame, instead of the original
// pattern, when doing brute initialization. Unfortunately that implies a
// totally different warping interface, since access to more than a the source
// and current destination frame is necessary.
template <typename Warp>
bool BruteTranslationOnlyInitialize(const FloatImage& image1,
                                    const FloatImage* image1_mask,
                                    const FloatImage& image2,
                                    const int num_extra_points,
                                    const bool use_normalized_intensities,
                                    const double* x1,
                                    const double* y1,
                                    double* x2,
                                    double* y2) {
  // Create the pattern to match in the space of image2, assuming our inital
  // guess isn't too far from the template in image1. If there is no image1
  // mask, then the resulting mask is binary.
  FloatArray pattern;
  FloatArray mask;
  int origin_x = -1, origin_y = -1;
  CreateBrutePattern<Warp>(x1,
                           y1,
                           x2,
                           y2,
                           image1,
                           image1_mask,
                           &pattern,
                           &mask,
                           &origin_x,
                           &origin_y);
 
  // For normalization, premultiply the pattern by the inverse pattern mean.
  double mask_sum = 1.0;
  if (use_normalized_intensities) {
    mask_sum = mask.sum();
    double inverse_pattern_mean = mask_sum / ((mask * pattern).sum());
    pattern *= inverse_pattern_mean;
  }
 
  // Use Eigen on the images via maps for strong vectorization.
  Map<const FloatArray> search(image2.Data(), image2.Height(), image2.Width());
 
  // Try all possible locations inside the search area. Yes, everywhere.
  //
  // TODO(keir): There are a number of possible optimizations here. One choice
  // is to make a grid and only try one out of every N possible samples.
  //
  // Another, slightly more clever idea, is to compute some sort of spatial
  // frequency distribution of the pattern patch. If the spatial resolution is
  // high (e.g. a grating pattern or fine lines) then checking every possible
  // translation is necessary, since a 1-pixel shift may induce a massive
  // change in the cost function. If the image is a blob or splotch with blurry
  // edges, then fewer samples are necessary since a few pixels offset won't
  // change the cost function much.
  double best_sad = std::numeric_limits<double>::max();
  int best_r = -1;
  int best_c = -1;
  int w = pattern.cols();
  int h = pattern.rows();
 
  for (int r = 0; r < (image2.Height() - h); ++r) {
    for (int c = 0; c < (image2.Width() - w); ++c) {
      // Compute the weighted sum of absolute differences, Eigen style. Note
      // that the block from the search image is never stored in a variable, to
      // avoid copying overhead and permit inlining.
      double sad;
      if (use_normalized_intensities) {
        // TODO(keir): It's really dumb to recompute the search mean for every
        // shift. A smarter implementation would use summed area tables
        // instead, reducing the mean calculation to an O(1) operation.
        double inverse_search_mean =
            mask_sum / ((mask * search.block(r, c, h, w)).sum());
        sad = (mask *
               (pattern - (search.block(r, c, h, w) * inverse_search_mean)))
                  .abs()
                  .sum();
      } else {
        sad = (mask * (pattern - search.block(r, c, h, w))).abs().sum();
      }
      if (sad < best_sad) {
        best_r = r;
        best_c = c;
        best_sad = sad;
      }
    }
  }
 
  // This mean the effective pattern area is zero. This check could go earlier,
  // but this is less code.
  if (best_r == -1 || best_c == -1) {
    return false;
  }
 
  LG << "Brute force translation found a shift. "
     << "best_c: " << best_c << ", best_r: " << best_r << ", "
     << "origin_x: " << origin_x << ", origin_y: " << origin_y << ", "
     << "dc: " << (best_c - origin_x) << ", "
     << "dr: " << (best_r - origin_y) << ", tried "
     << ((image2.Height() - h) * (image2.Width() - w)) << " shifts.";
 
  // Apply the shift.
  for (int i = 0; i < 4 + num_extra_points; ++i) {
    x2[i] += best_c - origin_x;
    y2[i] += best_r - origin_y;
  }
  return true;
}
 
void CopyQuad(double* src_x,
              double* src_y,
              double* dst_x,
              double* dst_y,
              int num_extra_points) {
  for (int i = 0; i < 4 + num_extra_points; ++i) {
    dst_x[i] = src_x[i];
    dst_y[i] = src_y[i];
  }
}
 
}  // namespace
 
template <typename Warp>
void TemplatedTrackRegion(const FloatImage& image1,
                          const FloatImage& image2,
                          const double* x1,
                          const double* y1,
                          const TrackRegionOptions& options,
                          double* x2,
                          double* y2,
                          TrackRegionResult* result) {
  for (int i = 0; i < 4 + options.num_extra_points; ++i) {
    LG << "P" << i << ": (" << x1[i] << ", " << y1[i] << "); guess (" << x2[i]
       << ", " << y2[i] << "); (dx, dy): (" << (x2[i] - x1[i]) << ", "
       << (y2[i] - y1[i]) << ").";
  }
 
  // Since (x2, y2) contains a prediction for where the tracked point should
  // go, try a refinement immediately in the hope that the prediction is close
  // enough.
  if (options.attempt_refine_before_brute) {
    TrackRegionOptions modified_options = options;
    modified_options.use_brute_initialization = false;
    modified_options.attempt_refine_before_brute = false;
 
    double x2_first_try[5];
    double y2_first_try[5];
    CopyQuad(x2, y2, x2_first_try, y2_first_try, options.num_extra_points);
 
    TemplatedTrackRegion<Warp>(image1,
                               image2,
                               x1,
                               y1,
                               modified_options,
                               x2_first_try,
                               y2_first_try,
                               result);
 
    // Of the things that can happen in the first pass, don't try the brute
    // pass (and second attempt) if the error is one of the terminations below.
    if (result->termination == TrackRegionResult::CONVERGENCE ||
        result->termination == TrackRegionResult::SOURCE_OUT_OF_BOUNDS ||
        result->termination == TrackRegionResult::DESTINATION_OUT_OF_BOUNDS ||
        result->termination == TrackRegionResult::INSUFFICIENT_PATTERN_AREA) {
      LG << "Terminated with first try at refinement; no brute needed.";
      // TODO(keir): Also check correlation?
      CopyQuad(x2_first_try, y2_first_try, x2, y2, options.num_extra_points);
      LG << "Early termination correlation: " << result->correlation;
      return;
    } else {
      LG << "Initial eager-refinement failed; retrying normally.";
    }
  }
 
  if (options.use_normalized_intensities) {
    LG << "Using normalized intensities.";
  }
 
  // Bail early if the points are already outside.
  if (!AllInBounds(image1, x1, y1)) {
    result->termination = TrackRegionResult::SOURCE_OUT_OF_BOUNDS;
    return;
  }
  if (!AllInBounds(image2, x2, y2)) {
    result->termination = TrackRegionResult::DESTINATION_OUT_OF_BOUNDS;
    return;
  }
  // TODO(keir): Check quads to ensure there is some area.
 
  // Keep a copy of the "original" guess for regularization.
  double x2_original[4];
  double y2_original[4];
  for (int i = 0; i < 4; ++i) {
    x2_original[i] = x2[i];
    y2_original[i] = y2[i];
  }
 
  // Prepare the image and gradient.
  Array3Df image_and_gradient1;
  Array3Df image_and_gradient2;
  BlurredImageAndDerivativesChannels(
      image1, options.sigma, &image_and_gradient1);
  BlurredImageAndDerivativesChannels(
      image2, options.sigma, &image_and_gradient2);
 
  // Possibly do a brute-force translation-only initialization.
  if (SearchAreaTooBigForDescent(image2, x2, y2) &&
      options.use_brute_initialization) {
    LG << "Running brute initialization...";
    bool found_any_alignment =
        BruteTranslationOnlyInitialize<Warp>(image_and_gradient1,
                                             options.image1_mask,
                                             image2,
                                             options.num_extra_points,
                                             options.use_normalized_intensities,
                                             x1,
                                             y1,
                                             x2,
                                             y2);
    if (!found_any_alignment) {
      LG << "Brute failed to find an alignment; pattern too small. "
         << "Failing entire track operation.";
      result->termination = TrackRegionResult::INSUFFICIENT_PATTERN_AREA;
      return;
    }
    for (int i = 0; i < 4; ++i) {
      LG << "P" << i << ": (" << x1[i] << ", " << y1[i] << "); brute (" << x2[i]
         << ", " << y2[i] << "); (dx, dy): (" << (x2[i] - x1[i]) << ", "
         << (y2[i] - y1[i]) << ").";
    }
  }
 
  // Prepare the initial warp parameters from the four correspondences.
  // Note: This must happen after the brute initialization runs, since the
  // brute initialization mutates x2 and y2 in place.
  Warp warp(x1, y1, x2, y2);
 
  // Decide how many samples to use in the x and y dimensions.
  int num_samples_x;
  int num_samples_y;
  PickSampling(x1, y1, x2, y2, &num_samples_x, &num_samples_y);
 
  // Compute the warp from rectangular coordinates.
  Mat3 canonical_homography =
      ComputeCanonicalHomography(x1, y1, num_samples_x, num_samples_y);
 
  ceres::Problem problem;
 
  // Construct the warp cost function. AutoDiffCostFunction takes ownership.
  PixelDifferenceCostFunctor<Warp>* pixel_difference_cost_function =
      new PixelDifferenceCostFunctor<Warp>(options,
                                           image_and_gradient1,
                                           image_and_gradient2,
                                           canonical_homography,
                                           num_samples_x,
                                           num_samples_y,
                                           warp);
  problem.AddResidualBlock(
      new ceres::AutoDiffCostFunction<PixelDifferenceCostFunctor<Warp>,
                                      ceres::DYNAMIC,
                                      Warp::NUM_PARAMETERS>(
          pixel_difference_cost_function, num_samples_x * num_samples_y),
      NULL,
      warp.parameters);
 
  // Construct the regularizing cost function
  if (options.regularization_coefficient != 0.0) {
    WarpRegularizingCostFunctor<Warp>* regularizing_warp_cost_function =
        new WarpRegularizingCostFunctor<Warp>(
            options, x1, y2, x2_original, y2_original, warp);
 
    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<WarpRegularizingCostFunctor<Warp>,
                                        8 /* num_residuals */,
                                        Warp::NUM_PARAMETERS>(
            regularizing_warp_cost_function),
        NULL,
        warp.parameters);
  }
 
  // Configure the solve.
  ceres::Solver::Options solver_options;
  solver_options.linear_solver_type = ceres::DENSE_QR;
  solver_options.max_num_iterations = options.max_iterations;
  solver_options.update_state_every_iteration = true;
  solver_options.parameter_tolerance = 1e-16;
  solver_options.function_tolerance = 1e-16;
 
  // Prevent the corners from going outside the destination image and
  // terminate if the optimizer is making tiny moves (converged).
  TerminationCheckingCallback<Warp> callback(options, image2, warp, x1, y1);
  solver_options.callbacks.push_back(&callback);
 
  // Run the solve.
  ceres::Solver::Summary summary;
  ceres::Solve(solver_options, &problem, &summary);
 
  LG << "Summary:\n" << summary.FullReport();
 
  // Update the four points with the found solution; if the solver failed, then
  // the warp parameters are the identity (so ignore failure).
  //
  // Also warp any extra points on the end of the array.
  for (int i = 0; i < 4 + options.num_extra_points; ++i) {
    warp.Forward(warp.parameters, x1[i], y1[i], x2 + i, y2 + i);
    LG << "Warped point " << i << ": (" << x1[i] << ", " << y1[i] << ") -> ("
       << x2[i] << ", " << y2[i] << "); (dx, dy): (" << (x2[i] - x1[i]) << ", "
       << (y2[i] - y1[i]) << ").";
  }
 
  // TODO(keir): Update the result statistics.
  // TODO(keir): Add a normalize-cross-correlation variant.
 
  if (summary.termination_type == ceres::USER_FAILURE) {
    result->termination = TrackRegionResult::FELL_OUT_OF_BOUNDS;
    return;
  }
 
#define HANDLE_TERMINATION(termination_enum)                                   \
  if (summary.termination_type == ceres::termination_enum) {                   \
    result->termination = TrackRegionResult::termination_enum;                 \
    return;                                                                    \
  }
 
  // Avoid computing correlation for tracking failures.
  HANDLE_TERMINATION(FAILURE);
 
  // Otherwise, run a final correlation check.
  if (options.minimum_correlation > 0.0) {
    result->correlation =
        pixel_difference_cost_function
            ->PearsonProductMomentCorrelationCoefficient(warp.parameters);
    if (result->correlation < options.minimum_correlation) {
      LG << "Failing with insufficient correlation.";
      result->termination = TrackRegionResult::INSUFFICIENT_CORRELATION;
      return;
    }
  }
 
  // This happens when the minimum corner shift tolerance is reached. Due to
  // how the tolerance is computed this can't be done by Ceres. So return the
  // same termination enum as Ceres, even though this is slightly different
  // than Ceres's parameter tolerance, which operates on the raw parameter
  // values rather than the pixel shifts of the patch corners.
  if (summary.termination_type == ceres::USER_SUCCESS) {
    result->termination = TrackRegionResult::CONVERGENCE;
    return;
  }
 
  HANDLE_TERMINATION(CONVERGENCE);
  HANDLE_TERMINATION(NO_CONVERGENCE);
#undef HANDLE_TERMINATION
};
 
void TrackRegion(const FloatImage& image1,
                 const FloatImage& image2,
                 const double* x1,
                 const double* y1,
                 const TrackRegionOptions& options,
                 double* x2,
                 double* y2,
                 TrackRegionResult* result) {
  // Enum is necessary due to templated nature of autodiff.
#define HANDLE_MODE(mode_enum, mode_type)                                      \
  if (options.mode == TrackRegionOptions::mode_enum) {                         \
    TemplatedTrackRegion<mode_type>(                                           \
        image1, image2, x1, y1, options, x2, y2, result);                      \
    return;                                                                    \
  }
  HANDLE_MODE(TRANSLATION, TranslationWarp);
  HANDLE_MODE(TRANSLATION_SCALE, TranslationScaleWarp);
  HANDLE_MODE(TRANSLATION_ROTATION, TranslationRotationWarp);
  HANDLE_MODE(TRANSLATION_ROTATION_SCALE, TranslationRotationScaleWarp);
  HANDLE_MODE(AFFINE, AffineWarp);
  HANDLE_MODE(HOMOGRAPHY, HomographyWarp);
#undef HANDLE_MODE
}
 
bool SamplePlanarPatch(const FloatImage& image,
                       const double* xs,
                       const double* ys,
                       int num_samples_x,
                       int num_samples_y,
                       FloatImage* mask,
                       FloatImage* patch,
                       double* warped_position_x,
                       double* warped_position_y) {
  // Bail early if the points are outside the image.
  if (!AllInBounds(image, xs, ys)) {
    LG << "Can't sample patch: out of bounds.";
    return false;
  }
 
  // Make the patch have the appropriate size, and match the depth of image.
  patch->Resize(num_samples_y, num_samples_x, image.Depth());
 
  // Compute the warp from rectangular coordinates.
  Mat3 canonical_homography =
      ComputeCanonicalHomography(xs, ys, num_samples_x, num_samples_y);
 
  // Walk over the coordinates in the canonical space, sampling from the image
  // in the original space and copying the result into the patch.
  for (int r = 0; r < num_samples_y; ++r) {
    for (int c = 0; c < num_samples_x; ++c) {
      Vec3 image_position = canonical_homography * Vec3(c, r, 1);
      image_position /= image_position(2);
      SampleLinear(
          image, image_position(1), image_position(0), &(*patch)(r, c, 0));
      if (mask) {
        float mask_value =
            SampleLinear(*mask, image_position(1), image_position(0), 0);
 
        for (int d = 0; d < image.Depth(); d++)
          (*patch)(r, c, d) *= mask_value;
      }
    }
  }
 
  Vec3 warped_position = canonical_homography.inverse() * Vec3(xs[4], ys[4], 1);
  warped_position /= warped_position(2);
 
  *warped_position_x = warped_position(0);
  *warped_position_y = warped_position(1);
 
  return true;
}
 
}  // namespace libmv