// 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;
}