diff options
Diffstat (limited to 'examples/libmv_homography.cc')
| -rw-r--r-- | examples/libmv_homography.cc | 414 |
1 files changed, 414 insertions, 0 deletions
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; +} |