1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2014 Google Inc. All rights reserved. 3 // http://code.google.com/p/ceres-solver/ 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are met: 7 // 8 // * Redistributions of source code must retain the above copyright notice, 9 // this list of conditions and the following disclaimer. 10 // * Redistributions in binary form must reproduce the above copyright notice, 11 // this list of conditions and the following disclaimer in the documentation 12 // and/or other materials provided with the distribution. 13 // * Neither the name of Google Inc. nor the names of its contributors may be 14 // used to endorse or promote products derived from this software without 15 // specific prior written permission. 16 // 17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 // POSSIBILITY OF SUCH DAMAGE. 28 // 29 // Copyright (c) 2014 libmv authors. 30 // 31 // Permission is hereby granted, free of charge, to any person obtaining a copy 32 // of this software and associated documentation files (the "Software"), to 33 // deal in the Software without restriction, including without limitation the 34 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 35 // sell copies of the Software, and to permit persons to whom the Software is 36 // furnished to do so, subject to the following conditions: 37 // 38 // The above copyright notice and this permission notice shall be included in 39 // all copies or substantial portions of the Software. 40 // 41 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 42 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 43 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 44 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 45 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 46 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 47 // IN THE SOFTWARE. 48 // 49 // Author: sergey.vfx (at) gmail.com (Sergey Sharybin) 50 // 51 // This file demonstrates solving for a homography between two sets of points. 52 // A homography describes a transformation between a sets of points on a plane, 53 // perspectively projected into two images. The first step is to solve a 54 // homogeneous system of equations via singular value decompposition, giving an 55 // algebraic solution for the homography, then solving for a final solution by 56 // minimizing the symmetric transfer error in image space with Ceres (called the 57 // Gold Standard Solution in "Multiple View Geometry"). The routines are based on 58 // the routines from the Libmv library. 59 // 60 // This example demonstrates custom exit criterion by having a callback check 61 // for image-space error. 62 63 #include "ceres/ceres.h" 64 #include "glog/logging.h" 65 66 typedef Eigen::NumTraits<double> EigenDouble; 67 68 typedef Eigen::MatrixXd Mat; 69 typedef Eigen::VectorXd Vec; 70 typedef Eigen::Matrix<double, 3, 3> Mat3; 71 typedef Eigen::Matrix<double, 2, 1> Vec2; 72 typedef Eigen::Matrix<double, Eigen::Dynamic, 8> MatX8; 73 typedef Eigen::Vector3d Vec3; 74 75 namespace { 76 77 // This structure contains options that controls how the homography 78 // estimation operates. 79 // 80 // Defaults should be suitable for a wide range of use cases, but 81 // better performance and accuracy might require tweaking. 82 struct EstimateHomographyOptions { 83 // Default settings for homography estimation which should be suitable 84 // for a wide range of use cases. 85 EstimateHomographyOptions() 86 : max_num_iterations(50), 87 expected_average_symmetric_distance(1e-16) {} 88 89 // Maximal number of iterations for the refinement step. 90 int max_num_iterations; 91 92 // Expected average of symmetric geometric distance between 93 // actual destination points and original ones transformed by 94 // estimated homography matrix. 95 // 96 // Refinement will finish as soon as average of symmetric 97 // geometric distance is less or equal to this value. 98 // 99 // This distance is measured in the same units as input points are. 100 double expected_average_symmetric_distance; 101 }; 102 103 // Calculate symmetric geometric cost terms: 104 // 105 // forward_error = D(H * x1, x2) 106 // backward_error = D(H^-1 * x2, x1) 107 // 108 // Templated to be used with autodifferenciation. 109 template <typename T> 110 void SymmetricGeometricDistanceTerms(const Eigen::Matrix<T, 3, 3> &H, 111 const Eigen::Matrix<T, 2, 1> &x1, 112 const Eigen::Matrix<T, 2, 1> &x2, 113 T forward_error[2], 114 T backward_error[2]) { 115 typedef Eigen::Matrix<T, 3, 1> Vec3; 116 Vec3 x(x1(0), x1(1), T(1.0)); 117 Vec3 y(x2(0), x2(1), T(1.0)); 118 119 Vec3 H_x = H * x; 120 Vec3 Hinv_y = H.inverse() * y; 121 122 H_x /= H_x(2); 123 Hinv_y /= Hinv_y(2); 124 125 forward_error[0] = H_x(0) - y(0); 126 forward_error[1] = H_x(1) - y(1); 127 backward_error[0] = Hinv_y(0) - x(0); 128 backward_error[1] = Hinv_y(1) - x(1); 129 } 130 131 // Calculate symmetric geometric cost: 132 // 133 // D(H * x1, x2)^2 + D(H^-1 * x2, x1)^2 134 // 135 double SymmetricGeometricDistance(const Mat3 &H, 136 const Vec2 &x1, 137 const Vec2 &x2) { 138 Vec2 forward_error, backward_error; 139 SymmetricGeometricDistanceTerms<double>(H, 140 x1, 141 x2, 142 forward_error.data(), 143 backward_error.data()); 144 return forward_error.squaredNorm() + 145 backward_error.squaredNorm(); 146 } 147 148 // A parameterization of the 2D homography matrix that uses 8 parameters so 149 // that the matrix is normalized (H(2,2) == 1). 150 // The homography matrix H is built from a list of 8 parameters (a, b,...g, h) 151 // as follows 152 // 153 // |a b c| 154 // H = |d e f| 155 // |g h 1| 156 // 157 template<typename T = double> 158 class Homography2DNormalizedParameterization { 159 public: 160 typedef Eigen::Matrix<T, 8, 1> Parameters; // a, b, ... g, h 161 typedef Eigen::Matrix<T, 3, 3> Parameterized; // H 162 163 // Convert from the 8 parameters to a H matrix. 164 static void To(const Parameters &p, Parameterized *h) { 165 *h << p(0), p(1), p(2), 166 p(3), p(4), p(5), 167 p(6), p(7), 1.0; 168 } 169 170 // Convert from a H matrix to the 8 parameters. 171 static void From(const Parameterized &h, Parameters *p) { 172 *p << h(0, 0), h(0, 1), h(0, 2), 173 h(1, 0), h(1, 1), h(1, 2), 174 h(2, 0), h(2, 1); 175 } 176 }; 177 178 // 2D Homography transformation estimation in the case that points are in 179 // euclidean coordinates. 180 // 181 // x = H y 182 // 183 // x and y vector must have the same direction, we could write 184 // 185 // crossproduct(|x|, * H * |y| ) = |0| 186 // 187 // | 0 -1 x2| |a b c| |y1| |0| 188 // | 1 0 -x1| * |d e f| * |y2| = |0| 189 // |-x2 x1 0| |g h 1| |1 | |0| 190 // 191 // That gives: 192 // 193 // (-d+x2*g)*y1 + (-e+x2*h)*y2 + -f+x2 |0| 194 // (a-x1*g)*y1 + (b-x1*h)*y2 + c-x1 = |0| 195 // (-x2*a+x1*d)*y1 + (-x2*b+x1*e)*y2 + -x2*c+x1*f |0| 196 // 197 bool Homography2DFromCorrespondencesLinearEuc( 198 const Mat &x1, 199 const Mat &x2, 200 Mat3 *H, 201 double expected_precision) { 202 assert(2 == x1.rows()); 203 assert(4 <= x1.cols()); 204 assert(x1.rows() == x2.rows()); 205 assert(x1.cols() == x2.cols()); 206 207 int n = x1.cols(); 208 MatX8 L = Mat::Zero(n * 3, 8); 209 Mat b = Mat::Zero(n * 3, 1); 210 for (int i = 0; i < n; ++i) { 211 int j = 3 * i; 212 L(j, 0) = x1(0, i); // a 213 L(j, 1) = x1(1, i); // b 214 L(j, 2) = 1.0; // c 215 L(j, 6) = -x2(0, i) * x1(0, i); // g 216 L(j, 7) = -x2(0, i) * x1(1, i); // h 217 b(j, 0) = x2(0, i); // i 218 219 ++j; 220 L(j, 3) = x1(0, i); // d 221 L(j, 4) = x1(1, i); // e 222 L(j, 5) = 1.0; // f 223 L(j, 6) = -x2(1, i) * x1(0, i); // g 224 L(j, 7) = -x2(1, i) * x1(1, i); // h 225 b(j, 0) = x2(1, i); // i 226 227 // This ensures better stability 228 // TODO(julien) make a lite version without this 3rd set 229 ++j; 230 L(j, 0) = x2(1, i) * x1(0, i); // a 231 L(j, 1) = x2(1, i) * x1(1, i); // b 232 L(j, 2) = x2(1, i); // c 233 L(j, 3) = -x2(0, i) * x1(0, i); // d 234 L(j, 4) = -x2(0, i) * x1(1, i); // e 235 L(j, 5) = -x2(0, i); // f 236 } 237 // Solve Lx=B 238 const Vec h = L.fullPivLu().solve(b); 239 Homography2DNormalizedParameterization<double>::To(h, H); 240 return (L * h).isApprox(b, expected_precision); 241 } 242 243 // Cost functor which computes symmetric geometric distance 244 // used for homography matrix refinement. 245 class HomographySymmetricGeometricCostFunctor { 246 public: 247 HomographySymmetricGeometricCostFunctor(const Vec2 &x, 248 const Vec2 &y) 249 : x_(x), y_(y) { } 250 251 template<typename T> 252 bool operator()(const T* homography_parameters, T* residuals) const { 253 typedef Eigen::Matrix<T, 3, 3> Mat3; 254 typedef Eigen::Matrix<T, 2, 1> Vec2; 255 256 Mat3 H(homography_parameters); 257 Vec2 x(T(x_(0)), T(x_(1))); 258 Vec2 y(T(y_(0)), T(y_(1))); 259 260 SymmetricGeometricDistanceTerms<T>(H, 261 x, 262 y, 263 &residuals[0], 264 &residuals[2]); 265 return true; 266 } 267 268 const Vec2 x_; 269 const Vec2 y_; 270 }; 271 272 // Termination checking callback. This is needed to finish the 273 // optimization when an absolute error threshold is met, as opposed 274 // to Ceres's function_tolerance, which provides for finishing when 275 // successful steps reduce the cost function by a fractional amount. 276 // In this case, the callback checks for the absolute average reprojection 277 // error and terminates when it's below a threshold (for example all 278 // points < 0.5px error). 279 class TerminationCheckingCallback : public ceres::IterationCallback { 280 public: 281 TerminationCheckingCallback(const Mat &x1, const Mat &x2, 282 const EstimateHomographyOptions &options, 283 Mat3 *H) 284 : options_(options), x1_(x1), x2_(x2), H_(H) {} 285 286 virtual ceres::CallbackReturnType operator()( 287 const ceres::IterationSummary& summary) { 288 // If the step wasn't successful, there's nothing to do. 289 if (!summary.step_is_successful) { 290 return ceres::SOLVER_CONTINUE; 291 } 292 293 // Calculate average of symmetric geometric distance. 294 double average_distance = 0.0; 295 for (int i = 0; i < x1_.cols(); i++) { 296 average_distance += SymmetricGeometricDistance(*H_, 297 x1_.col(i), 298 x2_.col(i)); 299 } 300 average_distance /= x1_.cols(); 301 302 if (average_distance <= options_.expected_average_symmetric_distance) { 303 return ceres::SOLVER_TERMINATE_SUCCESSFULLY; 304 } 305 306 return ceres::SOLVER_CONTINUE; 307 } 308 309 private: 310 const EstimateHomographyOptions &options_; 311 const Mat &x1_; 312 const Mat &x2_; 313 Mat3 *H_; 314 }; 315 316 bool EstimateHomography2DFromCorrespondences( 317 const Mat &x1, 318 const Mat &x2, 319 const EstimateHomographyOptions &options, 320 Mat3 *H) { 321 assert(2 == x1.rows()); 322 assert(4 <= x1.cols()); 323 assert(x1.rows() == x2.rows()); 324 assert(x1.cols() == x2.cols()); 325 326 // Step 1: Algebraic homography estimation. 327 // Assume algebraic estimation always succeeds. 328 Homography2DFromCorrespondencesLinearEuc(x1, 329 x2, 330 H, 331 EigenDouble::dummy_precision()); 332 333 LOG(INFO) << "Estimated matrix after algebraic estimation:\n" << *H; 334 335 // Step 2: Refine matrix using Ceres minimizer. 336 ceres::Problem problem; 337 for (int i = 0; i < x1.cols(); i++) { 338 HomographySymmetricGeometricCostFunctor 339 *homography_symmetric_geometric_cost_function = 340 new HomographySymmetricGeometricCostFunctor(x1.col(i), 341 x2.col(i)); 342 343 problem.AddResidualBlock( 344 new ceres::AutoDiffCostFunction< 345 HomographySymmetricGeometricCostFunctor, 346 4, // num_residuals 347 9>(homography_symmetric_geometric_cost_function), 348 NULL, 349 H->data()); 350 } 351 352 // Configure the solve. 353 ceres::Solver::Options solver_options; 354 solver_options.linear_solver_type = ceres::DENSE_QR; 355 solver_options.max_num_iterations = options.max_num_iterations; 356 solver_options.update_state_every_iteration = true; 357 358 // Terminate if the average symmetric distance is good enough. 359 TerminationCheckingCallback callback(x1, x2, options, H); 360 solver_options.callbacks.push_back(&callback); 361 362 // Run the solve. 363 ceres::Solver::Summary summary; 364 ceres::Solve(solver_options, &problem, &summary); 365 366 LOG(INFO) << "Summary:\n" << summary.FullReport(); 367 LOG(INFO) << "Final refined matrix:\n" << *H; 368 369 return summary.IsSolutionUsable(); 370 } 371 372 } // namespace libmv 373 374 int main(int argc, char **argv) { 375 google::InitGoogleLogging(argv[0]); 376 377 Mat x1(2, 100); 378 for (int i = 0; i < x1.cols(); ++i) { 379 x1(0, i) = rand() % 1024; 380 x1(1, i) = rand() % 1024; 381 } 382 383 Mat3 homography_matrix; 384 // This matrix has been dumped from a Blender test file of plane tracking. 385 homography_matrix << 1.243715, -0.461057, -111.964454, 386 0.0, 0.617589, -192.379252, 387 0.0, -0.000983, 1.0; 388 389 Mat x2 = x1; 390 for (int i = 0; i < x2.cols(); ++i) { 391 Vec3 homogenous_x1 = Vec3(x1(0, i), x1(1, i), 1.0); 392 Vec3 homogenous_x2 = homography_matrix * homogenous_x1; 393 x2(0, i) = homogenous_x2(0) / homogenous_x2(2); 394 x2(1, i) = homogenous_x2(1) / homogenous_x2(2); 395 396 // Apply some noise so algebraic estimation is not good enough. 397 x2(0, i) += static_cast<double>(rand() % 1000) / 5000.0; 398 x2(1, i) += static_cast<double>(rand() % 1000) / 5000.0; 399 } 400 401 Mat3 estimated_matrix; 402 403 EstimateHomographyOptions options; 404 options.expected_average_symmetric_distance = 0.02; 405 EstimateHomography2DFromCorrespondences(x1, x2, options, &estimated_matrix); 406 407 // Normalize the matrix for easier comparison. 408 estimated_matrix /= estimated_matrix(2 ,2); 409 410 std::cout << "Original matrix:\n" << homography_matrix << "\n"; 411 std::cout << "Estimated matrix:\n" << estimated_matrix << "\n"; 412 413 return EXIT_SUCCESS; 414 } 415