libmv/multiview/homography.cc

Go to the documentation of this file.

// Copyright (c) 2008, 2009 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/multiview/homography.h"
 
#include "ceres/ceres.h"
#include "libmv/logging/logging.h"
#include "libmv/multiview/conditioning.h"
#include "libmv/multiview/homography_parameterization.h"
 
namespace libmv {
static bool Homography2DFromCorrespondencesLinearEuc(
    const Mat& x1, const Mat& x2, Mat3* H, double expected_precision) {
  assert(2 == x1.rows());
  assert(4 <= x1.cols());
  assert(x1.rows() == x2.rows());
  assert(x1.cols() == x2.cols());
 
  int n = x1.cols();
  MatX8 L = Mat::Zero(n * 3, 8);
  Mat b = Mat::Zero(n * 3, 1);
  for (int i = 0; i < n; ++i) {
    int j = 3 * i;
    L(j, 0) = x1(0, i);              // a
    L(j, 1) = x1(1, i);              // b
    L(j, 2) = 1.0;                   // c
    L(j, 6) = -x2(0, i) * x1(0, i);  // g
    L(j, 7) = -x2(0, i) * x1(1, i);  // h
    b(j, 0) = x2(0, i);              // i
 
    ++j;
    L(j, 3) = x1(0, i);              // d
    L(j, 4) = x1(1, i);              // e
    L(j, 5) = 1.0;                   // f
    L(j, 6) = -x2(1, i) * x1(0, i);  // g
    L(j, 7) = -x2(1, i) * x1(1, i);  // h
    b(j, 0) = x2(1, i);              // i
 
    // This ensures better stability
    // TODO(julien) make a lite version without this 3rd set
    ++j;
    L(j, 0) = x2(1, i) * x1(0, i);   // a
    L(j, 1) = x2(1, i) * x1(1, i);   // b
    L(j, 2) = x2(1, i);              // c
    L(j, 3) = -x2(0, i) * x1(0, i);  // d
    L(j, 4) = -x2(0, i) * x1(1, i);  // e
    L(j, 5) = -x2(0, i);             // f
  }
  // Solve Lx=B
  Vec h = L.fullPivLu().solve(b);
  Homography2DNormalizedParameterization<double>::To(h, H);
  if ((L * h).isApprox(b, expected_precision)) {
    return true;
  } else {
    return false;
  }
}
 
// clang-format off
// clang-format on
bool Homography2DFromCorrespondencesLinear(const Mat& x1,
                                           const Mat& x2,
                                           Mat3* H,
                                           double expected_precision) {
  if (x1.rows() == 2) {
    return Homography2DFromCorrespondencesLinearEuc(
        x1, x2, H, expected_precision);
  }
  assert(3 == x1.rows());
  assert(4 <= x1.cols());
  assert(x1.rows() == x2.rows());
  assert(x1.cols() == x2.cols());
 
  const int x = 0;
  const int y = 1;
  const int w = 2;
  int n = x1.cols();
  MatX8 L = Mat::Zero(n * 3, 8);
  Mat b = Mat::Zero(n * 3, 1);
  for (int i = 0; i < n; ++i) {
    int j = 3 * i;
    L(j, 0) = x2(w, i) * x1(x, i);   // a
    L(j, 1) = x2(w, i) * x1(y, i);   // b
    L(j, 2) = x2(w, i) * x1(w, i);   // c
    L(j, 6) = -x2(x, i) * x1(x, i);  // g
    L(j, 7) = -x2(x, i) * x1(y, i);  // h
    b(j, 0) = x2(x, i) * x1(w, i);
 
    ++j;
    L(j, 3) = x2(w, i) * x1(x, i);   // d
    L(j, 4) = x2(w, i) * x1(y, i);   // e
    L(j, 5) = x2(w, i) * x1(w, i);   // f
    L(j, 6) = -x2(y, i) * x1(x, i);  // g
    L(j, 7) = -x2(y, i) * x1(y, i);  // h
    b(j, 0) = x2(y, i) * x1(w, i);
 
    // This ensures better stability
    ++j;
    L(j, 0) = x2(y, i) * x1(x, i);   // a
    L(j, 1) = x2(y, i) * x1(y, i);   // b
    L(j, 2) = x2(y, i) * x1(w, i);   // c
    L(j, 3) = -x2(x, i) * x1(x, i);  // d
    L(j, 4) = -x2(x, i) * x1(y, i);  // e
    L(j, 5) = -x2(x, i) * x1(w, i);  // f
  }
  // Solve Lx=B
  Vec h = L.fullPivLu().solve(b);
  if ((L * h).isApprox(b, expected_precision)) {
    Homography2DNormalizedParameterization<double>::To(h, H);
    return true;
  } else {
    return false;
  }
}
 
// Default settings for homography estimation which should be suitable
// for a wide range of use cases.
EstimateHomographyOptions::EstimateHomographyOptions()
    : use_normalization(true),
      max_num_iterations(50),
      expected_average_symmetric_distance(1e-16) {
}
 
namespace {
void GetNormalizedPoints(const Mat& original_points,
                         Mat* normalized_points,
                         Mat3* normalization_matrix) {
  IsotropicPreconditionerFromPoints(original_points, normalization_matrix);
  ApplyTransformationToPoints(
      original_points, *normalization_matrix, normalized_points);
}
 
// Cost functor which computes symmetric geometric distance
// used for homography matrix refinement.
class HomographySymmetricGeometricCostFunctor {
 public:
  HomographySymmetricGeometricCostFunctor(const Vec2& x, const Vec2& y)
      : x_(x), y_(y) {}
 
  template <typename T>
  bool operator()(const T* homography_parameters, T* residuals) const {
    typedef Eigen::Matrix<T, 3, 3> Mat3;
    typedef Eigen::Matrix<T, 3, 1> Vec3;
 
    Mat3 H(homography_parameters);
 
    Vec3 x(T(x_(0)), T(x_(1)), T(1.0));
    Vec3 y(T(y_(0)), T(y_(1)), T(1.0));
 
    Vec3 H_x = H * x;
    Vec3 Hinv_y = H.inverse() * y;
 
    H_x /= H_x(2);
    Hinv_y /= Hinv_y(2);
 
    // This is a forward error.
    residuals[0] = H_x(0) - T(y_(0));
    residuals[1] = H_x(1) - T(y_(1));
 
    // This is a backward error.
    residuals[2] = Hinv_y(0) - T(x_(0));
    residuals[3] = Hinv_y(1) - T(x_(1));
 
    return true;
  }
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  const Vec2 x_;
  const Vec2 y_;
};
 
// Termination checking callback used for homography estimation.
// It finished the minimization as soon as actual average of
// symmetric geometric distance is less or equal to the expected
// average value.
class TerminationCheckingCallback : public ceres::IterationCallback {
 public:
  TerminationCheckingCallback(const Mat& x1,
                              const Mat& x2,
                              const EstimateHomographyOptions& options,
                              Mat3* H)
      : options_(options), x1_(x1), x2_(x2), H_(H) {}
 
  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;
    }
 
    // Calculate average of symmetric geometric distance.
    double average_distance = 0.0;
    for (int i = 0; i < x1_.cols(); i++) {
      average_distance =
          SymmetricGeometricDistance(*H_, x1_.col(i), x2_.col(i));
    }
    average_distance /= x1_.cols();
 
    if (average_distance <= options_.expected_average_symmetric_distance) {
      return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
    }
 
    return ceres::SOLVER_CONTINUE;
  }
 
 private:
  const EstimateHomographyOptions& options_;
  const Mat& x1_;
  const Mat& x2_;
  Mat3* H_;
};
}  // namespace
 
bool EstimateHomography2DFromCorrespondences(
    const Mat& x1,
    const Mat& x2,
    const EstimateHomographyOptions& options,
    Mat3* H) {
  // TODO(sergey): Support homogenous coordinates, not just euclidean.
 
  assert(2 == x1.rows());
  assert(4 <= x1.cols());
  assert(x1.rows() == x2.rows());
  assert(x1.cols() == x2.cols());
 
  Mat3 T1 = Mat3::Identity(), T2 = Mat3::Identity();
 
  // Step 1: Algebraic homography estimation.
  Mat x1_normalized, x2_normalized;
 
  if (options.use_normalization) {
    LG << "Estimating homography using normalization.";
    GetNormalizedPoints(x1, &x1_normalized, &T1);
    GetNormalizedPoints(x2, &x2_normalized, &T2);
  } else {
    x1_normalized = x1;
    x2_normalized = x2;
  }
 
  // Assume algebraic estiation always suceeds,
  Homography2DFromCorrespondencesLinear(x1_normalized, x2_normalized, H);
 
  // Denormalize the homography matrix.
  if (options.use_normalization) {
    *H = T2.inverse() * (*H) * T1;
  }
 
  LG << "Estimated matrix after algebraic estimation:\n" << *H;
 
  // Step 2: Refine matrix using Ceres minimizer.
  ceres::Problem problem;
  for (int i = 0; i < x1.cols(); i++) {
    HomographySymmetricGeometricCostFunctor*
        homography_symmetric_geometric_cost_function =
            new HomographySymmetricGeometricCostFunctor(x1.col(i), x2.col(i));
 
    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<HomographySymmetricGeometricCostFunctor,
                                        4,  // num_residuals
                                        9>(
            homography_symmetric_geometric_cost_function),
        NULL,
        H->data());
  }
 
  // Configure the solve.
  ceres::Solver::Options solver_options;
  solver_options.linear_solver_type = ceres::DENSE_QR;
  solver_options.max_num_iterations = options.max_num_iterations;
  solver_options.update_state_every_iteration = true;
 
  // Terminate if the average symmetric distance is good enough.
  TerminationCheckingCallback callback(x1, x2, options, H);
  solver_options.callbacks.push_back(&callback);
 
  // Run the solve.
  ceres::Solver::Summary summary;
  ceres::Solve(solver_options, &problem, &summary);
 
  VLOG(1) << "Summary:\n" << summary.FullReport();
 
  LG << "Final refined matrix:\n" << *H;
 
  return summary.IsSolutionUsable();
}
 
// clang-format off
// clang-format on
bool Homography3DFromCorrespondencesLinear(const Mat& x1,
                                           const Mat& x2,
                                           Mat4* H,
                                           double expected_precision) {
  assert(4 == x1.rows());
  assert(5 <= x1.cols());
  assert(x1.rows() == x2.rows());
  assert(x1.cols() == x2.cols());
  const int x = 0;
  const int y = 1;
  const int z = 2;
  const int w = 3;
  int n = x1.cols();
  MatX15 L = Mat::Zero(n * 6, 15);
  Mat b = Mat::Zero(n * 6, 1);
  for (int i = 0; i < n; ++i) {
    int j = 6 * i;
    L(j, 0) = -x2(w, i) * x1(x, i);  // a
    L(j, 1) = -x2(w, i) * x1(y, i);  // b
    L(j, 2) = -x2(w, i) * x1(z, i);  // c
    L(j, 3) = -x2(w, i) * x1(w, i);  // d
    L(j, 12) = x2(x, i) * x1(x, i);  // m
    L(j, 13) = x2(x, i) * x1(y, i);  // n
    L(j, 14) = x2(x, i) * x1(z, i);  // o
    b(j, 0) = -x2(x, i) * x1(w, i);
 
    ++j;
    L(j, 4) = -x2(z, i) * x1(x, i);  // e
    L(j, 5) = -x2(z, i) * x1(y, i);  // f
    L(j, 6) = -x2(z, i) * x1(z, i);  // g
    L(j, 7) = -x2(z, i) * x1(w, i);  // h
    L(j, 8) = x2(y, i) * x1(x, i);   // i
    L(j, 9) = x2(y, i) * x1(y, i);   // j
    L(j, 10) = x2(y, i) * x1(z, i);  // k
    L(j, 11) = x2(y, i) * x1(w, i);  // l
 
    ++j;
    L(j, 0) = -x2(z, i) * x1(x, i);  // a
    L(j, 1) = -x2(z, i) * x1(y, i);  // b
    L(j, 2) = -x2(z, i) * x1(z, i);  // c
    L(j, 3) = -x2(z, i) * x1(w, i);  // d
    L(j, 8) = x2(x, i) * x1(x, i);   // i
    L(j, 9) = x2(x, i) * x1(y, i);   // j
    L(j, 10) = x2(x, i) * x1(z, i);  // k
    L(j, 11) = x2(x, i) * x1(w, i);  // l
 
    ++j;
    L(j, 4) = -x2(w, i) * x1(x, i);  // e
    L(j, 5) = -x2(w, i) * x1(y, i);  // f
    L(j, 6) = -x2(w, i) * x1(z, i);  // g
    L(j, 7) = -x2(w, i) * x1(w, i);  // h
    L(j, 12) = x2(y, i) * x1(x, i);  // m
    L(j, 13) = x2(y, i) * x1(y, i);  // n
    L(j, 14) = x2(y, i) * x1(z, i);  // o
    b(j, 0) = -x2(y, i) * x1(w, i);
 
    ++j;
    L(j, 0) = -x2(y, i) * x1(x, i);  // a
    L(j, 1) = -x2(y, i) * x1(y, i);  // b
    L(j, 2) = -x2(y, i) * x1(z, i);  // c
    L(j, 3) = -x2(y, i) * x1(w, i);  // d
    L(j, 4) = x2(x, i) * x1(x, i);   // e
    L(j, 5) = x2(x, i) * x1(y, i);   // f
    L(j, 6) = x2(x, i) * x1(z, i);   // g
    L(j, 7) = x2(x, i) * x1(w, i);   // h
 
    ++j;
    L(j, 8) = -x2(w, i) * x1(x, i);   // i
    L(j, 9) = -x2(w, i) * x1(y, i);   // j
    L(j, 10) = -x2(w, i) * x1(z, i);  // k
    L(j, 11) = -x2(w, i) * x1(w, i);  // l
    L(j, 12) = x2(z, i) * x1(x, i);   // m
    L(j, 13) = x2(z, i) * x1(y, i);   // n
    L(j, 14) = x2(z, i) * x1(z, i);   // o
    b(j, 0) = -x2(z, i) * x1(w, i);
  }
  // Solve Lx=B
  Vec h = L.fullPivLu().solve(b);
  if ((L * h).isApprox(b, expected_precision)) {
    Homography3DNormalizedParameterization<double>::To(h, H);
    return true;
  } else {
    return false;
  }
}
 
double SymmetricGeometricDistance(const Mat3& H,
                                  const Vec2& x1,
                                  const Vec2& x2) {
  Vec3 x(x1(0), x1(1), 1.0);
  Vec3 y(x2(0), x2(1), 1.0);
 
  Vec3 H_x = H * x;
  Vec3 Hinv_y = H.inverse() * y;
 
  H_x /= H_x(2);
  Hinv_y /= Hinv_y(2);
 
  return (H_x.head<2>() - y.head<2>()).squaredNorm() +
         (Hinv_y.head<2>() - x.head<2>()).squaredNorm();
}
 
}  // namespace libmv