415 lines
15 KiB
C++
415 lines
15 KiB
C++
// 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;
|
|
}
|