// Ceres Solver - A fast non-linear least squares minimizer

// Copyright 2015 Google Inc. All rights reserved.

// http://ceres-solver.org/

//

// Redistribution and use in source and binary forms, with or without

// modification, are permitted provided that the following conditions are met:

//

// * Redistributions of source code must retain the above copyright notice,

//   this list of conditions and the following disclaimer.

// * Redistributions in binary form must reproduce the above copyright notice,

//   this list of conditions and the following disclaimer in the documentation

//   and/or other materials provided with the distribution.

// * Neither the name of Google Inc. nor the names of its contributors may be

//   used to endorse or promote products derived from this software without

//   specific prior written permission.

//

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

// POSSIBILITY OF SUCH DAMAGE.

//

// Copyright (c) 2014 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: sergey.vfx@gmail.com (Sergey Sharybin)

//

// This file demonstrates solving for a homography between two sets of points.

// A homography describes a transformation between a sets of points on a plane,

// perspectively projected into two images. The first step is to solve a

// homogeneous system of equations via singular value decompposition, giving an

// algebraic solution for the homography, then solving for a final solution by

// minimizing the symmetric transfer error in image space with Ceres (called the

// Gold Standard Solution in "Multiple View Geometry"). The routines are based on

// the routines from the Libmv library.

//

// This example demonstrates custom exit criterion by having a callback check

// for image-space error.

#include "ceres/ceres.h"

#include "glog/logging.h"

typedef Eigen::NumTraits<double> EigenDouble;

typedef Eigen::MatrixXd Mat;

typedef Eigen::VectorXd Vec;

typedef Eigen::Matrix<double, 3, 3> Mat3;

typedef Eigen::Matrix<double, 2, 1> Vec2;

typedef Eigen::Matrix<double, Eigen::Dynamic,  8> MatX8;

typedef Eigen::Vector3d Vec3;

namespace {

// This structure contains options that controls how the homography

// estimation operates.

//

// Defaults should be suitable for a wide range of use cases, but

// better performance and accuracy might require tweaking.

struct EstimateHomographyOptions {

  // Default settings for homography estimation which should be suitable

  // for a wide range of use cases.

  EstimateHomographyOptions()

    :  max_num_iterations(50),

       expected_average_symmetric_distance(1e-16) {}

  // Maximal number of iterations for the refinement step.

  int max_num_iterations;

  // Expected average of symmetric geometric distance between

  // actual destination points and original ones transformed by

  // estimated homography matrix.

  //

  // Refinement will finish as soon as average of symmetric

  // geometric distance is less or equal to this value.

  //

  // This distance is measured in the same units as input points are.

  double expected_average_symmetric_distance;

};

// Calculate symmetric geometric cost terms:

//

// forward_error = D(H * x1, x2)

// backward_error = D(H^-1 * x2, x1)

//

// Templated to be used with autodifferenciation.

template <typename T>

void SymmetricGeometricDistanceTerms(const Eigen::Matrix<T, 3, 3> &H,

                                     const Eigen::Matrix<T, 2, 1> &x1,

                                     const Eigen::Matrix<T, 2, 1> &x2,

                                     T forward_error[2],

                                     T backward_error[2]) {

  typedef Eigen::Matrix<T, 3, 1> Vec3;

  Vec3 x(x1(0), x1(1), T(1.0));

  Vec3 y(x2(0), x2(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);

  forward_error[0] = H_x(0) - y(0);

  forward_error[1] = H_x(1) - y(1);

  backward_error[0] = Hinv_y(0) - x(0);

  backward_error[1] = Hinv_y(1) - x(1);

}

// Calculate symmetric geometric cost:

//

//   D(H * x1, x2)^2 + D(H^-1 * x2, x1)^2

//

double SymmetricGeometricDistance(const Mat3 &H,

                                  const Vec2 &x1,

                                  const Vec2 &x2) {

  Vec2 forward_error, backward_error;

  SymmetricGeometricDistanceTerms<double>(H,

                                          x1,

                                          x2,

                                          forward_error.data(),

                                          backward_error.data());

  return forward_error.squaredNorm() +

         backward_error.squaredNorm();

}

// A parameterization of the 2D homography matrix that uses 8 parameters so

// that the matrix is normalized (H(2,2) == 1).

// The homography matrix H is built from a list of 8 parameters (a, b,...g, h)

// as follows

//

//         |a b c|

//     H = |d e f|

//         |g h 1|

//

template<typename T = double>

class Homography2DNormalizedParameterization {

 public:

  typedef Eigen::Matrix<T, 8, 1> Parameters;     // a, b, ... g, h

  typedef Eigen::Matrix<T, 3, 3> Parameterized;  // H

  // Convert from the 8 parameters to a H matrix.

  static void To(const Parameters &p, Parameterized *h) {

    *h << p(0), p(1), p(2),

          p(3), p(4), p(5),

          p(6), p(7), 1.0;

  }

  // Convert from a H matrix to the 8 parameters.

  static void From(const Parameterized &h, Parameters *p) {

    *p << h(0, 0), h(0, 1), h(0, 2),

          h(1, 0), h(1, 1), h(1, 2),

          h(2, 0), h(2, 1);

  }

};

// 2D Homography transformation estimation in the case that points are in

// euclidean coordinates.

//

//   x = H y

//

// x and y vector must have the same direction, we could write

//

//   crossproduct(|x|, * H * |y| ) = |0|

//

//   | 0 -1  x2|   |a b c|   |y1|    |0|

//   | 1  0 -x1| * |d e f| * |y2| =  |0|

//   |-x2  x1 0|   |g h 1|   |1 |    |0|

//

// That gives:

//

//   (-d+x2*g)*y1    + (-e+x2*h)*y2 + -f+x2          |0|

//   (a-x1*g)*y1     + (b-x1*h)*y2  + c-x1         = |0|

//   (-x2*a+x1*d)*y1 + (-x2*b+x1*e)*y2 + -x2*c+x1*f  |0|

//

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

  const Vec h = L.fullPivLu().solve(b);

  Homography2DNormalizedParameterization<double>::To(h, H);

  return (L * h).isApprox(b, expected_precision);

}

// 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, 2, 1> Vec2;

    Mat3 H(homography_parameters);

    Vec2 x(T(x_(0)), T(x_(1)));

    Vec2 y(T(y_(0)), T(y_(1)));

    SymmetricGeometricDistanceTerms<T>(H,

                                       x,

                                       y,

                                       &residuals[0],

                                       &residuals[2]);

    return true;

  }

  const Vec2 x_;

  const Vec2 y_;

};

// Termination checking callback. This is needed to finish the

// optimization when an absolute error threshold is met, as opposed

// to Ceres's function_tolerance, which provides for finishing when

// successful steps reduce the cost function by a fractional amount.

// In this case, the callback checks for the absolute average reprojection

// error and terminates when it's below a threshold (for example all

// points < 0.5px error).

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_;

};

bool EstimateHomography2DFromCorrespondences(

    const Mat &x1,

    const Mat &x2,

    const EstimateHomographyOptions &options,

    Mat3 *H) {

  assert(2 == x1.rows());

  assert(4 <= x1.cols());

  assert(x1.rows() == x2.rows());

  assert(x1.cols() == x2.cols());

  // Step 1: Algebraic homography estimation.

  // Assume algebraic estimation always succeeds.

  Homography2DFromCorrespondencesLinearEuc(x1,

                                           x2,

                                           H,

                                           EigenDouble::dummy_precision());

  LOG(INFO) << "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);

  LOG(INFO) << "Summary:\n" << summary.FullReport();

  LOG(INFO) << "Final refined matrix:\n" << *H;

  return summary.IsSolutionUsable();

}

}  // namespace libmv

int main(int argc, char **argv) {

  google::InitGoogleLogging(argv[0]);

  Mat x1(2, 100);

  for (int i = 0; i < x1.cols(); ++i) {

    x1(0, i) = rand() % 1024;

    x1(1, i) = rand() % 1024;

  }

  Mat3 homography_matrix;

  // This matrix has been dumped from a Blender test file of plane tracking.

  homography_matrix << 1.243715, -0.461057, -111.964454,

                       0.0,       0.617589, -192.379252,

                       0.0,      -0.000983,    1.0;

  Mat x2 = x1;

  for (int i = 0; i < x2.cols(); ++i) {

    Vec3 homogenous_x1 = Vec3(x1(0, i), x1(1, i), 1.0);

    Vec3 homogenous_x2 = homography_matrix * homogenous_x1;

    x2(0, i) = homogenous_x2(0) / homogenous_x2(2);

    x2(1, i) = homogenous_x2(1) / homogenous_x2(2);

    // Apply some noise so algebraic estimation is not good enough.

    x2(0, i) += static_cast<double>(rand() % 1000) / 5000.0;

    x2(1, i) += static_cast<double>(rand() % 1000) / 5000.0;

  }

  Mat3 estimated_matrix;

  EstimateHomographyOptions options;

  options.expected_average_symmetric_distance = 0.02;

  EstimateHomography2DFromCorrespondences(x1, x2, options, &estimated_matrix);

  // Normalize the matrix for easier comparison.

  estimated_matrix /= estimated_matrix(2 ,2);

  std::cout << "Original matrix:\n" << homography_matrix << "\n";

  std::cout << "Estimated matrix:\n" << estimated_matrix << "\n";

  return EXIT_SUCCESS;

}