1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2010, 2011, 2012 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 // Author: keir (at) google.com (Keir Mierle) 30 // sameeragarwal (at) google.com (Sameer Agarwal) 31 // 32 // System level tests for Ceres. The current suite of two tests. The 33 // first test is a small test based on Powell's Function. It is a 34 // scalar problem with 4 variables. The second problem is a bundle 35 // adjustment problem with 16 cameras and two thousand cameras. The 36 // first problem is to test the sanity test the factorization based 37 // solvers. The second problem is used to test the various 38 // combinations of solvers, orderings, preconditioners and 39 // multithreading. 40 41 #include <cmath> 42 #include <cstdio> 43 #include <cstdlib> 44 #include <string> 45 46 #include "ceres/internal/port.h" 47 48 #include "ceres/autodiff_cost_function.h" 49 #include "ceres/ordered_groups.h" 50 #include "ceres/problem.h" 51 #include "ceres/rotation.h" 52 #include "ceres/solver.h" 53 #include "ceres/stringprintf.h" 54 #include "ceres/test_util.h" 55 #include "ceres/types.h" 56 #include "gflags/gflags.h" 57 #include "glog/logging.h" 58 #include "gtest/gtest.h" 59 60 namespace ceres { 61 namespace internal { 62 63 const bool kAutomaticOrdering = true; 64 const bool kUserOrdering = false; 65 66 // Struct used for configuring the solver. 67 struct SolverConfig { 68 SolverConfig( 69 LinearSolverType linear_solver_type, 70 SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, 71 bool use_automatic_ordering) 72 : linear_solver_type(linear_solver_type), 73 sparse_linear_algebra_library_type(sparse_linear_algebra_library_type), 74 use_automatic_ordering(use_automatic_ordering), 75 preconditioner_type(IDENTITY), 76 num_threads(1) { 77 } 78 79 SolverConfig( 80 LinearSolverType linear_solver_type, 81 SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, 82 bool use_automatic_ordering, 83 PreconditionerType preconditioner_type) 84 : linear_solver_type(linear_solver_type), 85 sparse_linear_algebra_library_type(sparse_linear_algebra_library_type), 86 use_automatic_ordering(use_automatic_ordering), 87 preconditioner_type(preconditioner_type), 88 num_threads(1) { 89 } 90 91 string ToString() const { 92 return StringPrintf( 93 "(%s, %s, %s, %s, %d)", 94 LinearSolverTypeToString(linear_solver_type), 95 SparseLinearAlgebraLibraryTypeToString( 96 sparse_linear_algebra_library_type), 97 use_automatic_ordering ? "AUTOMATIC" : "USER", 98 PreconditionerTypeToString(preconditioner_type), 99 num_threads); 100 } 101 102 LinearSolverType linear_solver_type; 103 SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type; 104 bool use_automatic_ordering; 105 PreconditionerType preconditioner_type; 106 int num_threads; 107 }; 108 109 // Templated function that given a set of solver configurations, 110 // instantiates a new copy of SystemTestProblem for each configuration 111 // and solves it. The solutions are expected to have residuals with 112 // coordinate-wise maximum absolute difference less than or equal to 113 // max_abs_difference. 114 // 115 // The template parameter SystemTestProblem is expected to implement 116 // the following interface. 117 // 118 // class SystemTestProblem { 119 // public: 120 // SystemTestProblem(); 121 // Problem* mutable_problem(); 122 // Solver::Options* mutable_solver_options(); 123 // }; 124 template <typename SystemTestProblem> 125 void RunSolversAndCheckTheyMatch(const vector<SolverConfig>& configurations, 126 const double max_abs_difference) { 127 int num_configurations = configurations.size(); 128 vector<SystemTestProblem*> problems; 129 vector<vector<double> > final_residuals(num_configurations); 130 131 for (int i = 0; i < num_configurations; ++i) { 132 SystemTestProblem* system_test_problem = new SystemTestProblem(); 133 134 const SolverConfig& config = configurations[i]; 135 136 Solver::Options& options = *(system_test_problem->mutable_solver_options()); 137 options.linear_solver_type = config.linear_solver_type; 138 options.sparse_linear_algebra_library_type = 139 config.sparse_linear_algebra_library_type; 140 options.preconditioner_type = config.preconditioner_type; 141 options.num_threads = config.num_threads; 142 options.num_linear_solver_threads = config.num_threads; 143 144 if (config.use_automatic_ordering) { 145 options.linear_solver_ordering.reset(); 146 } 147 148 LOG(INFO) << "Running solver configuration: " 149 << config.ToString(); 150 151 Solver::Summary summary; 152 Solve(options, 153 system_test_problem->mutable_problem(), 154 &summary); 155 156 system_test_problem 157 ->mutable_problem() 158 ->Evaluate(Problem::EvaluateOptions(), 159 NULL, 160 &final_residuals[i], 161 NULL, 162 NULL); 163 164 CHECK_NE(summary.termination_type, ceres::FAILURE) 165 << "Solver configuration " << i << " failed."; 166 problems.push_back(system_test_problem); 167 168 // Compare the resulting solutions to each other. Arbitrarily take 169 // SPARSE_NORMAL_CHOLESKY as the golden solve. We compare 170 // solutions by comparing their residual vectors. We do not 171 // compare parameter vectors because it is much more brittle and 172 // error prone to do so, since the same problem can have nearly 173 // the same residuals at two completely different positions in 174 // parameter space. 175 if (i > 0) { 176 const vector<double>& reference_residuals = final_residuals[0]; 177 const vector<double>& current_residuals = final_residuals[i]; 178 179 for (int j = 0; j < reference_residuals.size(); ++j) { 180 EXPECT_NEAR(current_residuals[j], 181 reference_residuals[j], 182 max_abs_difference) 183 << "Not close enough residual:" << j 184 << " reference " << reference_residuals[j] 185 << " current " << current_residuals[j]; 186 } 187 } 188 } 189 190 for (int i = 0; i < num_configurations; ++i) { 191 delete problems[i]; 192 } 193 } 194 195 // This class implements the SystemTestProblem interface and provides 196 // access to an implementation of Powell's singular function. 197 // 198 // F = 1/2 (f1^2 + f2^2 + f3^2 + f4^2) 199 // 200 // f1 = x1 + 10*x2; 201 // f2 = sqrt(5) * (x3 - x4) 202 // f3 = (x2 - 2*x3)^2 203 // f4 = sqrt(10) * (x1 - x4)^2 204 // 205 // The starting values are x1 = 3, x2 = -1, x3 = 0, x4 = 1. 206 // The minimum is 0 at (x1, x2, x3, x4) = 0. 207 // 208 // From: Testing Unconstrained Optimization Software by Jorge J. More, Burton S. 209 // Garbow and Kenneth E. Hillstrom in ACM Transactions on Mathematical Software, 210 // Vol 7(1), March 1981. 211 class PowellsFunction { 212 public: 213 PowellsFunction() { 214 x_[0] = 3.0; 215 x_[1] = -1.0; 216 x_[2] = 0.0; 217 x_[3] = 1.0; 218 219 problem_.AddResidualBlock( 220 new AutoDiffCostFunction<F1, 1, 1, 1>(new F1), NULL, &x_[0], &x_[1]); 221 problem_.AddResidualBlock( 222 new AutoDiffCostFunction<F2, 1, 1, 1>(new F2), NULL, &x_[2], &x_[3]); 223 problem_.AddResidualBlock( 224 new AutoDiffCostFunction<F3, 1, 1, 1>(new F3), NULL, &x_[1], &x_[2]); 225 problem_.AddResidualBlock( 226 new AutoDiffCostFunction<F4, 1, 1, 1>(new F4), NULL, &x_[0], &x_[3]); 227 228 options_.max_num_iterations = 10; 229 } 230 231 Problem* mutable_problem() { return &problem_; } 232 Solver::Options* mutable_solver_options() { return &options_; } 233 234 private: 235 // Templated functions used for automatically differentiated cost 236 // functions. 237 class F1 { 238 public: 239 template <typename T> bool operator()(const T* const x1, 240 const T* const x2, 241 T* residual) const { 242 // f1 = x1 + 10 * x2; 243 *residual = *x1 + T(10.0) * *x2; 244 return true; 245 } 246 }; 247 248 class F2 { 249 public: 250 template <typename T> bool operator()(const T* const x3, 251 const T* const x4, 252 T* residual) const { 253 // f2 = sqrt(5) (x3 - x4) 254 *residual = T(sqrt(5.0)) * (*x3 - *x4); 255 return true; 256 } 257 }; 258 259 class F3 { 260 public: 261 template <typename T> bool operator()(const T* const x2, 262 const T* const x4, 263 T* residual) const { 264 // f3 = (x2 - 2 x3)^2 265 residual[0] = (x2[0] - T(2.0) * x4[0]) * (x2[0] - T(2.0) * x4[0]); 266 return true; 267 } 268 }; 269 270 class F4 { 271 public: 272 template <typename T> bool operator()(const T* const x1, 273 const T* const x4, 274 T* residual) const { 275 // f4 = sqrt(10) (x1 - x4)^2 276 residual[0] = T(sqrt(10.0)) * (x1[0] - x4[0]) * (x1[0] - x4[0]); 277 return true; 278 } 279 }; 280 281 double x_[4]; 282 Problem problem_; 283 Solver::Options options_; 284 }; 285 286 TEST(SystemTest, PowellsFunction) { 287 vector<SolverConfig> configs; 288 #define CONFIGURE(linear_solver, sparse_linear_algebra_library_type, ordering) \ 289 configs.push_back(SolverConfig(linear_solver, \ 290 sparse_linear_algebra_library_type, \ 291 ordering)) 292 293 CONFIGURE(DENSE_QR, SUITE_SPARSE, kAutomaticOrdering); 294 CONFIGURE(DENSE_NORMAL_CHOLESKY, SUITE_SPARSE, kAutomaticOrdering); 295 CONFIGURE(DENSE_SCHUR, SUITE_SPARSE, kAutomaticOrdering); 296 297 #ifndef CERES_NO_SUITESPARSE 298 CONFIGURE(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, kAutomaticOrdering); 299 #endif // CERES_NO_SUITESPARSE 300 301 #ifndef CERES_NO_CXSPARSE 302 CONFIGURE(SPARSE_NORMAL_CHOLESKY, CX_SPARSE, kAutomaticOrdering); 303 #endif // CERES_NO_CXSPARSE 304 305 CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kAutomaticOrdering); 306 307 #undef CONFIGURE 308 309 const double kMaxAbsoluteDifference = 1e-8; 310 RunSolversAndCheckTheyMatch<PowellsFunction>(configs, kMaxAbsoluteDifference); 311 } 312 313 // This class implements the SystemTestProblem interface and provides 314 // access to a bundle adjustment problem. It is based on 315 // examples/bundle_adjustment_example.cc. Currently a small 16 camera 316 // problem is hard coded in the constructor. Going forward we may 317 // extend this to a larger number of problems. 318 class BundleAdjustmentProblem { 319 public: 320 BundleAdjustmentProblem() { 321 const string input_file = TestFileAbsolutePath("problem-16-22106-pre.txt"); 322 ReadData(input_file); 323 BuildProblem(); 324 } 325 326 ~BundleAdjustmentProblem() { 327 delete []point_index_; 328 delete []camera_index_; 329 delete []observations_; 330 delete []parameters_; 331 } 332 333 Problem* mutable_problem() { return &problem_; } 334 Solver::Options* mutable_solver_options() { return &options_; } 335 336 int num_cameras() const { return num_cameras_; } 337 int num_points() const { return num_points_; } 338 int num_observations() const { return num_observations_; } 339 const int* point_index() const { return point_index_; } 340 const int* camera_index() const { return camera_index_; } 341 const double* observations() const { return observations_; } 342 double* mutable_cameras() { return parameters_; } 343 double* mutable_points() { return parameters_ + 9 * num_cameras_; } 344 345 private: 346 void ReadData(const string& filename) { 347 FILE * fptr = fopen(filename.c_str(), "r"); 348 349 if (!fptr) { 350 LOG(FATAL) << "File Error: unable to open file " << filename; 351 }; 352 353 // This will die horribly on invalid files. Them's the breaks. 354 FscanfOrDie(fptr, "%d", &num_cameras_); 355 FscanfOrDie(fptr, "%d", &num_points_); 356 FscanfOrDie(fptr, "%d", &num_observations_); 357 358 VLOG(1) << "Header: " << num_cameras_ 359 << " " << num_points_ 360 << " " << num_observations_; 361 362 point_index_ = new int[num_observations_]; 363 camera_index_ = new int[num_observations_]; 364 observations_ = new double[2 * num_observations_]; 365 366 num_parameters_ = 9 * num_cameras_ + 3 * num_points_; 367 parameters_ = new double[num_parameters_]; 368 369 for (int i = 0; i < num_observations_; ++i) { 370 FscanfOrDie(fptr, "%d", camera_index_ + i); 371 FscanfOrDie(fptr, "%d", point_index_ + i); 372 for (int j = 0; j < 2; ++j) { 373 FscanfOrDie(fptr, "%lf", observations_ + 2*i + j); 374 } 375 } 376 377 for (int i = 0; i < num_parameters_; ++i) { 378 FscanfOrDie(fptr, "%lf", parameters_ + i); 379 } 380 } 381 382 void BuildProblem() { 383 double* points = mutable_points(); 384 double* cameras = mutable_cameras(); 385 386 for (int i = 0; i < num_observations(); ++i) { 387 // Each Residual block takes a point and a camera as input and 388 // outputs a 2 dimensional residual. 389 CostFunction* cost_function = 390 new AutoDiffCostFunction<BundlerResidual, 2, 9, 3>( 391 new BundlerResidual(observations_[2*i + 0], 392 observations_[2*i + 1])); 393 394 // Each observation correponds to a pair of a camera and a point 395 // which are identified by camera_index()[i] and 396 // point_index()[i] respectively. 397 double* camera = cameras + 9 * camera_index_[i]; 398 double* point = points + 3 * point_index()[i]; 399 problem_.AddResidualBlock(cost_function, NULL, camera, point); 400 } 401 402 options_.linear_solver_ordering.reset(new ParameterBlockOrdering); 403 404 // The points come before the cameras. 405 for (int i = 0; i < num_points_; ++i) { 406 options_.linear_solver_ordering->AddElementToGroup(points + 3 * i, 0); 407 } 408 409 for (int i = 0; i < num_cameras_; ++i) { 410 options_.linear_solver_ordering->AddElementToGroup(cameras + 9 * i, 1); 411 } 412 413 options_.max_num_iterations = 25; 414 options_.function_tolerance = 1e-10; 415 options_.gradient_tolerance = 1e-10; 416 options_.parameter_tolerance = 1e-10; 417 } 418 419 template<typename T> 420 void FscanfOrDie(FILE *fptr, const char *format, T *value) { 421 int num_scanned = fscanf(fptr, format, value); 422 if (num_scanned != 1) { 423 LOG(FATAL) << "Invalid UW data file."; 424 } 425 } 426 427 // Templated pinhole camera model. The camera is parameterized 428 // using 9 parameters. 3 for rotation, 3 for translation, 1 for 429 // focal length and 2 for radial distortion. The principal point is 430 // not modeled (i.e. it is assumed be located at the image center). 431 struct BundlerResidual { 432 // (u, v): the position of the observation with respect to the image 433 // center point. 434 BundlerResidual(double u, double v): u(u), v(v) {} 435 436 template <typename T> 437 bool operator()(const T* const camera, 438 const T* const point, 439 T* residuals) const { 440 T p[3]; 441 AngleAxisRotatePoint(camera, point, p); 442 443 // Add the translation vector 444 p[0] += camera[3]; 445 p[1] += camera[4]; 446 p[2] += camera[5]; 447 448 const T& focal = camera[6]; 449 const T& l1 = camera[7]; 450 const T& l2 = camera[8]; 451 452 // Compute the center of distortion. The sign change comes from 453 // the camera model that Noah Snavely's Bundler assumes, whereby 454 // the camera coordinate system has a negative z axis. 455 T xp = - focal * p[0] / p[2]; 456 T yp = - focal * p[1] / p[2]; 457 458 // Apply second and fourth order radial distortion. 459 T r2 = xp*xp + yp*yp; 460 T distortion = T(1.0) + r2 * (l1 + l2 * r2); 461 462 residuals[0] = distortion * xp - T(u); 463 residuals[1] = distortion * yp - T(v); 464 465 return true; 466 } 467 468 double u; 469 double v; 470 }; 471 472 473 Problem problem_; 474 Solver::Options options_; 475 476 int num_cameras_; 477 int num_points_; 478 int num_observations_; 479 int num_parameters_; 480 481 int* point_index_; 482 int* camera_index_; 483 double* observations_; 484 // The parameter vector is laid out as follows 485 // [camera_1, ..., camera_n, point_1, ..., point_m] 486 double* parameters_; 487 }; 488 489 TEST(SystemTest, BundleAdjustmentProblem) { 490 vector<SolverConfig> configs; 491 492 #define CONFIGURE(linear_solver, sparse_linear_algebra_library_type, ordering, preconditioner) \ 493 configs.push_back(SolverConfig(linear_solver, \ 494 sparse_linear_algebra_library_type, \ 495 ordering, \ 496 preconditioner)) 497 498 CONFIGURE(DENSE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, IDENTITY); 499 CONFIGURE(DENSE_SCHUR, SUITE_SPARSE, kUserOrdering, IDENTITY); 500 501 CONFIGURE(CGNR, SUITE_SPARSE, kAutomaticOrdering, JACOBI); 502 503 CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kUserOrdering, JACOBI); 504 CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, JACOBI); 505 506 CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kUserOrdering, SCHUR_JACOBI); 507 CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, SCHUR_JACOBI); 508 509 #ifndef CERES_NO_SUITESPARSE 510 CONFIGURE(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, kAutomaticOrdering, IDENTITY); 511 CONFIGURE(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, kUserOrdering, IDENTITY); 512 513 CONFIGURE(SPARSE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, IDENTITY); 514 CONFIGURE(SPARSE_SCHUR, SUITE_SPARSE, kUserOrdering, IDENTITY); 515 516 CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, CLUSTER_JACOBI); 517 CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kUserOrdering, CLUSTER_JACOBI); 518 519 CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kAutomaticOrdering, CLUSTER_TRIDIAGONAL); 520 CONFIGURE(ITERATIVE_SCHUR, SUITE_SPARSE, kUserOrdering, CLUSTER_TRIDIAGONAL); 521 #endif // CERES_NO_SUITESPARSE 522 523 #ifndef CERES_NO_CXSPARSE 524 CONFIGURE(SPARSE_NORMAL_CHOLESKY, CX_SPARSE, kAutomaticOrdering, IDENTITY); 525 CONFIGURE(SPARSE_NORMAL_CHOLESKY, CX_SPARSE, kUserOrdering, IDENTITY); 526 527 CONFIGURE(SPARSE_SCHUR, CX_SPARSE, kAutomaticOrdering, IDENTITY); 528 CONFIGURE(SPARSE_SCHUR, CX_SPARSE, kUserOrdering, IDENTITY); 529 #endif // CERES_NO_CXSPARSE 530 531 #ifdef CERES_USE_EIGEN_SPARSE 532 CONFIGURE(SPARSE_SCHUR, EIGEN_SPARSE, kAutomaticOrdering, IDENTITY); 533 CONFIGURE(SPARSE_SCHUR, EIGEN_SPARSE, kUserOrdering, IDENTITY); 534 CONFIGURE(SPARSE_NORMAL_CHOLESKY, EIGEN_SPARSE, kAutomaticOrdering, IDENTITY); 535 CONFIGURE(SPARSE_NORMAL_CHOLESKY, EIGEN_SPARSE, kUserOrdering, IDENTITY); 536 #endif // CERES_USE_EIGEN_SPARSE 537 538 #undef CONFIGURE 539 540 // Single threaded evaluators and linear solvers. 541 const double kMaxAbsoluteDifference = 1e-4; 542 RunSolversAndCheckTheyMatch<BundleAdjustmentProblem>(configs, 543 kMaxAbsoluteDifference); 544 545 #ifdef CERES_USE_OPENMP 546 // Multithreaded evaluators and linear solvers. 547 for (int i = 0; i < configs.size(); ++i) { 548 configs[i].num_threads = 2; 549 } 550 RunSolversAndCheckTheyMatch<BundleAdjustmentProblem>(configs, 551 kMaxAbsoluteDifference); 552 #endif // CERES_USE_OPENMP 553 } 554 555 } // namespace internal 556 } // namespace ceres 557