Libmv 2D homography estimation example application
Add an example application of homography matrix estimation
from a 2D euclidean correspondences which is done in two
steps:
- Coarse algebraic estimation
- Fine refinement using Ceres minimizer
Nothing terribly exciting apart from an example of how to
use user callbacks.
User callback is used here to stop minimizer when average
of symmetric geometric distance becomes good enough.
This might be arguable whether it's the best way to go
(in some cases you would want to stop minimizer when
maximal symmetric distance is lower than a threshold) but
for a callback usage example it's good enough to stick
to current logic.
Change-Id: I60c8559cb10b001a0eb64ab71920c08bd68455b8
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
index dff6ef0..dbbcb81 100644
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@@ -88,6 +88,10 @@
libmv_bundle_adjuster.cc)
TARGET_LINK_LIBRARIES(libmv_bundle_adjuster ceres ${GFLAGS_LIBRARIES})
+ ADD_EXECUTABLE(libmv_homography
+ libmv_homography.cc)
+ TARGET_LINK_LIBRARIES(libmv_homography ceres ${GFLAGS_LIBRARIES})
+
ADD_EXECUTABLE(denoising
denoising.cc
fields_of_experts.cc)
diff --git a/examples/libmv_homography.cc b/examples/libmv_homography.cc
new file mode 100644
index 0000000..8bc7136
--- /dev/null
+++ b/examples/libmv_homography.cc
@@ -0,0 +1,414 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// 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;
+}
