179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Ceres Solver - A fast non-linear least squares minimizer 279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Copyright 2014 Google Inc. All rights reserved. 379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// http://code.google.com/p/ceres-solver/ 479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Redistribution and use in source and binary forms, with or without 679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// modification, are permitted provided that the following conditions are met: 779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// * Redistributions of source code must retain the above copyright notice, 979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// this list of conditions and the following disclaimer. 1079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// * Redistributions in binary form must reproduce the above copyright notice, 1179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// this list of conditions and the following disclaimer in the documentation 1279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// and/or other materials provided with the distribution. 1379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// * Neither the name of Google Inc. nor the names of its contributors may be 1479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// used to endorse or promote products derived from this software without 1579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// specific prior written permission. 1679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 1779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 1879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 1979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 2079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 2179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 2279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 2379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 2479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 2579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 2679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 2779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// POSSIBILITY OF SUCH DAMAGE. 2879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 2979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Copyright (c) 2014 libmv authors. 3079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 3179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Permission is hereby granted, free of charge, to any person obtaining a copy 3279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// of this software and associated documentation files (the "Software"), to 3379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// deal in the Software without restriction, including without limitation the 3479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 3579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// sell copies of the Software, and to permit persons to whom the Software is 3679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// furnished to do so, subject to the following conditions: 3779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 3879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// The above copyright notice and this permission notice shall be included in 3979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// all copies or substantial portions of the Software. 4079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 4179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 4279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 4379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 4479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 4579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 4679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 4779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// IN THE SOFTWARE. 4879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 4979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Author: sergey.vfx@gmail.com (Sergey Sharybin) 5079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 5179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// This file demonstrates solving for a homography between two sets of points. 5279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// A homography describes a transformation between a sets of points on a plane, 5379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// perspectively projected into two images. The first step is to solve a 5479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// homogeneous system of equations via singular value decompposition, giving an 5579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// algebraic solution for the homography, then solving for a final solution by 5679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// minimizing the symmetric transfer error in image space with Ceres (called the 5779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Gold Standard Solution in "Multiple View Geometry"). The routines are based on 5879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// the routines from the Libmv library. 5979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 6079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// This example demonstrates custom exit criterion by having a callback check 6179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// for image-space error. 6279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 6379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/ceres.h" 6479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "glog/logging.h" 6579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 6679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeztypedef Eigen::NumTraits<double> EigenDouble; 6779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 6879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeztypedef Eigen::MatrixXd Mat; 6979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeztypedef Eigen::VectorXd Vec; 7079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeztypedef Eigen::Matrix<double, 3, 3> Mat3; 7179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeztypedef Eigen::Matrix<double, 2, 1> Vec2; 7279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeztypedef Eigen::Matrix<double, Eigen::Dynamic, 8> MatX8; 7379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeztypedef Eigen::Vector3d Vec3; 7479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 7579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeznamespace { 7679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 7779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// This structure contains options that controls how the homography 7879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// estimation operates. 7979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 8079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Defaults should be suitable for a wide range of use cases, but 8179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// better performance and accuracy might require tweaking. 8279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezstruct EstimateHomographyOptions { 8379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Default settings for homography estimation which should be suitable 8479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // for a wide range of use cases. 8579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EstimateHomographyOptions() 8679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez : max_num_iterations(50), 8779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez expected_average_symmetric_distance(1e-16) {} 8879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 8979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Maximal number of iterations for the refinement step. 9079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int max_num_iterations; 9179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 9279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Expected average of symmetric geometric distance between 9379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // actual destination points and original ones transformed by 9479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // estimated homography matrix. 9579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // 9679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Refinement will finish as soon as average of symmetric 9779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // geometric distance is less or equal to this value. 9879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // 9979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // This distance is measured in the same units as input points are. 10079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double expected_average_symmetric_distance; 10179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez}; 10279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 10379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Calculate symmetric geometric cost terms: 10479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 10579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// forward_error = D(H * x1, x2) 10679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// backward_error = D(H^-1 * x2, x1) 10779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 10879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Templated to be used with autodifferenciation. 10979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeztemplate <typename T> 11079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid SymmetricGeometricDistanceTerms(const Eigen::Matrix<T, 3, 3> &H, 11179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Eigen::Matrix<T, 2, 1> &x1, 11279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Eigen::Matrix<T, 2, 1> &x2, 11379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez T forward_error[2], 11479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez T backward_error[2]) { 11579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez typedef Eigen::Matrix<T, 3, 1> Vec3; 11679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vec3 x(x1(0), x1(1), T(1.0)); 11779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vec3 y(x2(0), x2(1), T(1.0)); 11879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 11979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vec3 H_x = H * x; 12079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vec3 Hinv_y = H.inverse() * y; 12179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 12279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez H_x /= H_x(2); 12379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Hinv_y /= Hinv_y(2); 12479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 12579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez forward_error[0] = H_x(0) - y(0); 12679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez forward_error[1] = H_x(1) - y(1); 12779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez backward_error[0] = Hinv_y(0) - x(0); 12879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez backward_error[1] = Hinv_y(1) - x(1); 12979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 13079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 13179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Calculate symmetric geometric cost: 13279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 13379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// D(H * x1, x2)^2 + D(H^-1 * x2, x1)^2 13479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 13579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezdouble SymmetricGeometricDistance(const Mat3 &H, 13679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Vec2 &x1, 13779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Vec2 &x2) { 13879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vec2 forward_error, backward_error; 13979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez SymmetricGeometricDistanceTerms<double>(H, 14079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez x1, 14179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez x2, 14279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez forward_error.data(), 14379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez backward_error.data()); 14479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return forward_error.squaredNorm() + 14579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez backward_error.squaredNorm(); 14679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 14779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 14879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// A parameterization of the 2D homography matrix that uses 8 parameters so 14979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// that the matrix is normalized (H(2,2) == 1). 15079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// The homography matrix H is built from a list of 8 parameters (a, b,...g, h) 15179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// as follows 15279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 15379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// |a b c| 15479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// H = |d e f| 15579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// |g h 1| 15679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 15779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeztemplate<typename T = double> 15879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezclass Homography2DNormalizedParameterization { 15979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez public: 16079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez typedef Eigen::Matrix<T, 8, 1> Parameters; // a, b, ... g, h 16179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez typedef Eigen::Matrix<T, 3, 3> Parameterized; // H 16279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 16379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Convert from the 8 parameters to a H matrix. 16479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez static void To(const Parameters &p, Parameterized *h) { 16579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *h << p(0), p(1), p(2), 16679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez p(3), p(4), p(5), 16779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez p(6), p(7), 1.0; 16879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 16979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 17079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Convert from a H matrix to the 8 parameters. 17179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez static void From(const Parameterized &h, Parameters *p) { 17279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *p << h(0, 0), h(0, 1), h(0, 2), 17379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez h(1, 0), h(1, 1), h(1, 2), 17479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez h(2, 0), h(2, 1); 17579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 17679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez}; 17779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 17879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 2D Homography transformation estimation in the case that points are in 17979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// euclidean coordinates. 18079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 18179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// x = H y 18279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 18379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// x and y vector must have the same direction, we could write 18479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 18579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// crossproduct(|x|, * H * |y| ) = |0| 18679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 18779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// | 0 -1 x2| |a b c| |y1| |0| 18879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// | 1 0 -x1| * |d e f| * |y2| = |0| 18979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// |-x2 x1 0| |g h 1| |1 | |0| 19079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 19179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// That gives: 19279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 19379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// (-d+x2*g)*y1 + (-e+x2*h)*y2 + -f+x2 |0| 19479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// (a-x1*g)*y1 + (b-x1*h)*y2 + c-x1 = |0| 19579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// (-x2*a+x1*d)*y1 + (-x2*b+x1*e)*y2 + -x2*c+x1*f |0| 19679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 19779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezbool Homography2DFromCorrespondencesLinearEuc( 19879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Mat &x1, 19979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Mat &x2, 20079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Mat3 *H, 20179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double expected_precision) { 20279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez assert(2 == x1.rows()); 20379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez assert(4 <= x1.cols()); 20479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez assert(x1.rows() == x2.rows()); 20579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez assert(x1.cols() == x2.cols()); 20679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 20779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int n = x1.cols(); 20879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez MatX8 L = Mat::Zero(n * 3, 8); 20979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Mat b = Mat::Zero(n * 3, 1); 21079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < n; ++i) { 21179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int j = 3 * i; 21279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez L(j, 0) = x1(0, i); // a 21379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez L(j, 1) = x1(1, i); // b 21479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez L(j, 2) = 1.0; // c 21579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez L(j, 6) = -x2(0, i) * x1(0, i); // g 21679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez L(j, 7) = -x2(0, i) * x1(1, i); // h 21779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez b(j, 0) = x2(0, i); // i 21879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 21979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ++j; 22079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez L(j, 3) = x1(0, i); // d 22179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez L(j, 4) = x1(1, i); // e 22279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez L(j, 5) = 1.0; // f 22379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez L(j, 6) = -x2(1, i) * x1(0, i); // g 22479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez L(j, 7) = -x2(1, i) * x1(1, i); // h 22579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez b(j, 0) = x2(1, i); // i 22679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 22779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // This ensures better stability 22879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // TODO(julien) make a lite version without this 3rd set 22979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ++j; 23079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez L(j, 0) = x2(1, i) * x1(0, i); // a 23179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez L(j, 1) = x2(1, i) * x1(1, i); // b 23279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez L(j, 2) = x2(1, i); // c 23379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez L(j, 3) = -x2(0, i) * x1(0, i); // d 23479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez L(j, 4) = -x2(0, i) * x1(1, i); // e 23579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez L(j, 5) = -x2(0, i); // f 23679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 23779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Solve Lx=B 23879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Vec h = L.fullPivLu().solve(b); 23979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Homography2DNormalizedParameterization<double>::To(h, H); 24079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return (L * h).isApprox(b, expected_precision); 24179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 24279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 24379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Cost functor which computes symmetric geometric distance 24479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// used for homography matrix refinement. 24579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezclass HomographySymmetricGeometricCostFunctor { 24679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez public: 24779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez HomographySymmetricGeometricCostFunctor(const Vec2 &x, 24879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Vec2 &y) 24979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez : x_(x), y_(y) { } 25079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 25179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez template<typename T> 25279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez bool operator()(const T* homography_parameters, T* residuals) const { 25379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez typedef Eigen::Matrix<T, 3, 3> Mat3; 25479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez typedef Eigen::Matrix<T, 2, 1> Vec2; 25579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 25679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Mat3 H(homography_parameters); 25779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vec2 x(T(x_(0)), T(x_(1))); 25879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vec2 y(T(y_(0)), T(y_(1))); 25979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 26079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez SymmetricGeometricDistanceTerms<T>(H, 26179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez x, 26279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez y, 26379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez &residuals[0], 26479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez &residuals[2]); 26579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return true; 26679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 26779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 26879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Vec2 x_; 26979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Vec2 y_; 27079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez}; 27179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 27279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Termination checking callback. This is needed to finish the 27379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// optimization when an absolute error threshold is met, as opposed 27479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// to Ceres's function_tolerance, which provides for finishing when 27579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// successful steps reduce the cost function by a fractional amount. 27679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// In this case, the callback checks for the absolute average reprojection 27779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// error and terminates when it's below a threshold (for example all 27879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// points < 0.5px error). 27979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezclass TerminationCheckingCallback : public ceres::IterationCallback { 28079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez public: 28179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez TerminationCheckingCallback(const Mat &x1, const Mat &x2, 28279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const EstimateHomographyOptions &options, 28379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Mat3 *H) 28479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez : options_(options), x1_(x1), x2_(x2), H_(H) {} 28579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 28679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez virtual ceres::CallbackReturnType operator()( 28779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const ceres::IterationSummary& summary) { 28879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // If the step wasn't successful, there's nothing to do. 28979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (!summary.step_is_successful) { 29079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return ceres::SOLVER_CONTINUE; 29179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 29279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 29379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Calculate average of symmetric geometric distance. 29479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double average_distance = 0.0; 29579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < x1_.cols(); i++) { 29679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez average_distance += SymmetricGeometricDistance(*H_, 29779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez x1_.col(i), 29879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez x2_.col(i)); 29979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 30079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez average_distance /= x1_.cols(); 30179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 30279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (average_distance <= options_.expected_average_symmetric_distance) { 30379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return ceres::SOLVER_TERMINATE_SUCCESSFULLY; 30479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 30579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 30679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return ceres::SOLVER_CONTINUE; 30779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 30879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 30979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez private: 31079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const EstimateHomographyOptions &options_; 31179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Mat &x1_; 31279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Mat &x2_; 31379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Mat3 *H_; 31479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez}; 31579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 31679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezbool EstimateHomography2DFromCorrespondences( 31779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Mat &x1, 31879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Mat &x2, 31979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const EstimateHomographyOptions &options, 32079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Mat3 *H) { 32179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez assert(2 == x1.rows()); 32279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez assert(4 <= x1.cols()); 32379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez assert(x1.rows() == x2.rows()); 32479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez assert(x1.cols() == x2.cols()); 32579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 32679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Step 1: Algebraic homography estimation. 32779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Assume algebraic estimation always succeeds. 32879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Homography2DFromCorrespondencesLinearEuc(x1, 32979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez x2, 33079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez H, 33179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EigenDouble::dummy_precision()); 33279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 33379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LOG(INFO) << "Estimated matrix after algebraic estimation:\n" << *H; 33479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 33579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Step 2: Refine matrix using Ceres minimizer. 33679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ceres::Problem problem; 33779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < x1.cols(); i++) { 33879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez HomographySymmetricGeometricCostFunctor 33979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *homography_symmetric_geometric_cost_function = 34079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez new HomographySymmetricGeometricCostFunctor(x1.col(i), 34179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez x2.col(i)); 34279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 34379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddResidualBlock( 34479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez new ceres::AutoDiffCostFunction< 34579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez HomographySymmetricGeometricCostFunctor, 34679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 4, // num_residuals 34779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 9>(homography_symmetric_geometric_cost_function), 34879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez NULL, 34979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez H->data()); 35079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 35179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 35279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Configure the solve. 35379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ceres::Solver::Options solver_options; 35479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez solver_options.linear_solver_type = ceres::DENSE_QR; 35579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez solver_options.max_num_iterations = options.max_num_iterations; 35679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez solver_options.update_state_every_iteration = true; 35779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 35879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Terminate if the average symmetric distance is good enough. 35979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez TerminationCheckingCallback callback(x1, x2, options, H); 36079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez solver_options.callbacks.push_back(&callback); 36179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 36279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Run the solve. 36379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ceres::Solver::Summary summary; 36479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ceres::Solve(solver_options, &problem, &summary); 36579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 36679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LOG(INFO) << "Summary:\n" << summary.FullReport(); 36779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LOG(INFO) << "Final refined matrix:\n" << *H; 36879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 36979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary.IsSolutionUsable(); 37079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 37179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 37279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} // namespace libmv 37379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 37479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezint main(int argc, char **argv) { 37579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez google::InitGoogleLogging(argv[0]); 37679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 37779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Mat x1(2, 100); 37879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < x1.cols(); ++i) { 37979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez x1(0, i) = rand() % 1024; 38079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez x1(1, i) = rand() % 1024; 38179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 38279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 38379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Mat3 homography_matrix; 38479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // This matrix has been dumped from a Blender test file of plane tracking. 38579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez homography_matrix << 1.243715, -0.461057, -111.964454, 38679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 0.0, 0.617589, -192.379252, 38779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 0.0, -0.000983, 1.0; 38879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 38979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Mat x2 = x1; 39079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < x2.cols(); ++i) { 39179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vec3 homogenous_x1 = Vec3(x1(0, i), x1(1, i), 1.0); 39279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vec3 homogenous_x2 = homography_matrix * homogenous_x1; 39379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez x2(0, i) = homogenous_x2(0) / homogenous_x2(2); 39479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez x2(1, i) = homogenous_x2(1) / homogenous_x2(2); 39579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 39679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Apply some noise so algebraic estimation is not good enough. 39779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez x2(0, i) += static_cast<double>(rand() % 1000) / 5000.0; 39879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez x2(1, i) += static_cast<double>(rand() % 1000) / 5000.0; 39979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 40079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 40179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Mat3 estimated_matrix; 40279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 40379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EstimateHomographyOptions options; 40479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez options.expected_average_symmetric_distance = 0.02; 40579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EstimateHomography2DFromCorrespondences(x1, x2, options, &estimated_matrix); 40679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 40779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Normalize the matrix for easier comparison. 40879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez estimated_matrix /= estimated_matrix(2 ,2); 40979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 41079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez std::cout << "Original matrix:\n" << homography_matrix << "\n"; 41179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez std::cout << "Estimated matrix:\n" << estimated_matrix << "\n"; 41279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 41379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return EXIT_SUCCESS; 41479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 415