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 31 #include "ceres/solver_impl.h" 32 33 #include <cstdio> 34 #include <iostream> // NOLINT 35 #include <numeric> 36 #include "ceres/coordinate_descent_minimizer.h" 37 #include "ceres/evaluator.h" 38 #include "ceres/gradient_checking_cost_function.h" 39 #include "ceres/iteration_callback.h" 40 #include "ceres/levenberg_marquardt_strategy.h" 41 #include "ceres/linear_solver.h" 42 #include "ceres/map_util.h" 43 #include "ceres/minimizer.h" 44 #include "ceres/ordered_groups.h" 45 #include "ceres/parameter_block.h" 46 #include "ceres/parameter_block_ordering.h" 47 #include "ceres/problem.h" 48 #include "ceres/problem_impl.h" 49 #include "ceres/program.h" 50 #include "ceres/residual_block.h" 51 #include "ceres/stringprintf.h" 52 #include "ceres/trust_region_minimizer.h" 53 #include "ceres/wall_time.h" 54 55 namespace ceres { 56 namespace internal { 57 namespace { 58 59 // Callback for updating the user's parameter blocks. Updates are only 60 // done if the step is successful. 61 class StateUpdatingCallback : public IterationCallback { 62 public: 63 StateUpdatingCallback(Program* program, double* parameters) 64 : program_(program), parameters_(parameters) {} 65 66 CallbackReturnType operator()(const IterationSummary& summary) { 67 if (summary.step_is_successful) { 68 program_->StateVectorToParameterBlocks(parameters_); 69 program_->CopyParameterBlockStateToUserState(); 70 } 71 return SOLVER_CONTINUE; 72 } 73 74 private: 75 Program* program_; 76 double* parameters_; 77 }; 78 79 // Callback for logging the state of the minimizer to STDERR or STDOUT 80 // depending on the user's preferences and logging level. 81 class LoggingCallback : public IterationCallback { 82 public: 83 explicit LoggingCallback(bool log_to_stdout) 84 : log_to_stdout_(log_to_stdout) {} 85 86 ~LoggingCallback() {} 87 88 CallbackReturnType operator()(const IterationSummary& summary) { 89 const char* kReportRowFormat = 90 "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e " 91 "rho:% 3.2e mu:% 3.2e li:% 3d it:% 3.2e tt:% 3.2e"; 92 string output = StringPrintf(kReportRowFormat, 93 summary.iteration, 94 summary.cost, 95 summary.cost_change, 96 summary.gradient_max_norm, 97 summary.step_norm, 98 summary.relative_decrease, 99 summary.trust_region_radius, 100 summary.linear_solver_iterations, 101 summary.iteration_time_in_seconds, 102 summary.cumulative_time_in_seconds); 103 if (log_to_stdout_) { 104 cout << output << endl; 105 } else { 106 VLOG(1) << output; 107 } 108 return SOLVER_CONTINUE; 109 } 110 111 private: 112 const bool log_to_stdout_; 113 }; 114 115 // Basic callback to record the execution of the solver to a file for 116 // offline analysis. 117 class FileLoggingCallback : public IterationCallback { 118 public: 119 explicit FileLoggingCallback(const string& filename) 120 : fptr_(NULL) { 121 fptr_ = fopen(filename.c_str(), "w"); 122 CHECK_NOTNULL(fptr_); 123 } 124 125 virtual ~FileLoggingCallback() { 126 if (fptr_ != NULL) { 127 fclose(fptr_); 128 } 129 } 130 131 virtual CallbackReturnType operator()(const IterationSummary& summary) { 132 fprintf(fptr_, 133 "%4d %e %e\n", 134 summary.iteration, 135 summary.cost, 136 summary.cumulative_time_in_seconds); 137 return SOLVER_CONTINUE; 138 } 139 private: 140 FILE* fptr_; 141 }; 142 143 } // namespace 144 145 void SolverImpl::Minimize(const Solver::Options& options, 146 Program* program, 147 CoordinateDescentMinimizer* inner_iteration_minimizer, 148 Evaluator* evaluator, 149 LinearSolver* linear_solver, 150 double* parameters, 151 Solver::Summary* summary) { 152 Minimizer::Options minimizer_options(options); 153 154 // TODO(sameeragarwal): Add support for logging the configuration 155 // and more detailed stats. 156 scoped_ptr<IterationCallback> file_logging_callback; 157 if (!options.solver_log.empty()) { 158 file_logging_callback.reset(new FileLoggingCallback(options.solver_log)); 159 minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), 160 file_logging_callback.get()); 161 } 162 163 LoggingCallback logging_callback(options.minimizer_progress_to_stdout); 164 if (options.logging_type != SILENT) { 165 minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), 166 &logging_callback); 167 } 168 169 StateUpdatingCallback updating_callback(program, parameters); 170 if (options.update_state_every_iteration) { 171 // This must get pushed to the front of the callbacks so that it is run 172 // before any of the user callbacks. 173 minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), 174 &updating_callback); 175 } 176 177 minimizer_options.evaluator = evaluator; 178 scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian()); 179 minimizer_options.jacobian = jacobian.get(); 180 minimizer_options.inner_iteration_minimizer = inner_iteration_minimizer; 181 182 TrustRegionStrategy::Options trust_region_strategy_options; 183 trust_region_strategy_options.linear_solver = linear_solver; 184 trust_region_strategy_options.initial_radius = 185 options.initial_trust_region_radius; 186 trust_region_strategy_options.max_radius = options.max_trust_region_radius; 187 trust_region_strategy_options.lm_min_diagonal = options.lm_min_diagonal; 188 trust_region_strategy_options.lm_max_diagonal = options.lm_max_diagonal; 189 trust_region_strategy_options.trust_region_strategy_type = 190 options.trust_region_strategy_type; 191 trust_region_strategy_options.dogleg_type = options.dogleg_type; 192 scoped_ptr<TrustRegionStrategy> strategy( 193 TrustRegionStrategy::Create(trust_region_strategy_options)); 194 minimizer_options.trust_region_strategy = strategy.get(); 195 196 TrustRegionMinimizer minimizer; 197 double minimizer_start_time = WallTimeInSeconds(); 198 minimizer.Minimize(minimizer_options, parameters, summary); 199 summary->minimizer_time_in_seconds = 200 WallTimeInSeconds() - minimizer_start_time; 201 } 202 203 void SolverImpl::Solve(const Solver::Options& original_options, 204 ProblemImpl* original_problem_impl, 205 Solver::Summary* summary) { 206 double solver_start_time = WallTimeInSeconds(); 207 208 Program* original_program = original_problem_impl->mutable_program(); 209 ProblemImpl* problem_impl = original_problem_impl; 210 211 // Reset the summary object to its default values. 212 *CHECK_NOTNULL(summary) = Solver::Summary(); 213 214 summary->num_parameter_blocks = problem_impl->NumParameterBlocks(); 215 summary->num_parameters = problem_impl->NumParameters(); 216 summary->num_residual_blocks = problem_impl->NumResidualBlocks(); 217 summary->num_residuals = problem_impl->NumResiduals(); 218 219 // Empty programs are usually a user error. 220 if (summary->num_parameter_blocks == 0) { 221 summary->error = "Problem contains no parameter blocks."; 222 LOG(ERROR) << summary->error; 223 return; 224 } 225 226 if (summary->num_residual_blocks == 0) { 227 summary->error = "Problem contains no residual blocks."; 228 LOG(ERROR) << summary->error; 229 return; 230 } 231 232 Solver::Options options(original_options); 233 options.linear_solver_ordering = NULL; 234 options.inner_iteration_ordering = NULL; 235 236 #ifndef CERES_USE_OPENMP 237 if (options.num_threads > 1) { 238 LOG(WARNING) 239 << "OpenMP support is not compiled into this binary; " 240 << "only options.num_threads=1 is supported. Switching " 241 << "to single threaded mode."; 242 options.num_threads = 1; 243 } 244 if (options.num_linear_solver_threads > 1) { 245 LOG(WARNING) 246 << "OpenMP support is not compiled into this binary; " 247 << "only options.num_linear_solver_threads=1 is supported. Switching " 248 << "to single threaded mode."; 249 options.num_linear_solver_threads = 1; 250 } 251 #endif 252 253 summary->num_threads_given = original_options.num_threads; 254 summary->num_threads_used = options.num_threads; 255 256 if (options.lsqp_iterations_to_dump.size() > 0) { 257 LOG(WARNING) << "Dumping linear least squares problems to disk is" 258 " currently broken. Ignoring Solver::Options::lsqp_iterations_to_dump"; 259 } 260 261 // Evaluate the initial cost, residual vector and the jacobian 262 // matrix if requested by the user. The initial cost needs to be 263 // computed on the original unpreprocessed problem, as it is used to 264 // determine the value of the "fixed" part of the objective function 265 // after the problem has undergone reduction. 266 if (!Evaluator::Evaluate(original_program, 267 options.num_threads, 268 &(summary->initial_cost), 269 options.return_initial_residuals 270 ? &summary->initial_residuals 271 : NULL, 272 options.return_initial_gradient 273 ? &summary->initial_gradient 274 : NULL, 275 options.return_initial_jacobian 276 ? &summary->initial_jacobian 277 : NULL)) { 278 summary->termination_type = NUMERICAL_FAILURE; 279 summary->error = "Unable to evaluate the initial cost."; 280 LOG(ERROR) << summary->error; 281 return; 282 } 283 284 original_program->SetParameterBlockStatePtrsToUserStatePtrs(); 285 286 // If the user requests gradient checking, construct a new 287 // ProblemImpl by wrapping the CostFunctions of problem_impl inside 288 // GradientCheckingCostFunction and replacing problem_impl with 289 // gradient_checking_problem_impl. 290 scoped_ptr<ProblemImpl> gradient_checking_problem_impl; 291 if (options.check_gradients) { 292 VLOG(1) << "Checking Gradients"; 293 gradient_checking_problem_impl.reset( 294 CreateGradientCheckingProblemImpl( 295 problem_impl, 296 options.numeric_derivative_relative_step_size, 297 options.gradient_check_relative_precision)); 298 299 // From here on, problem_impl will point to the gradient checking 300 // version. 301 problem_impl = gradient_checking_problem_impl.get(); 302 } 303 304 if (original_options.linear_solver_ordering != NULL) { 305 if (!IsOrderingValid(original_options, problem_impl, &summary->error)) { 306 LOG(ERROR) << summary->error; 307 return; 308 } 309 options.linear_solver_ordering = 310 new ParameterBlockOrdering(*original_options.linear_solver_ordering); 311 } else { 312 options.linear_solver_ordering = new ParameterBlockOrdering; 313 const ProblemImpl::ParameterMap& parameter_map = 314 problem_impl->parameter_map(); 315 for (ProblemImpl::ParameterMap::const_iterator it = parameter_map.begin(); 316 it != parameter_map.end(); 317 ++it) { 318 options.linear_solver_ordering->AddElementToGroup(it->first, 0); 319 } 320 } 321 322 // Create the three objects needed to minimize: the transformed program, the 323 // evaluator, and the linear solver. 324 scoped_ptr<Program> reduced_program(CreateReducedProgram(&options, 325 problem_impl, 326 &summary->fixed_cost, 327 &summary->error)); 328 if (reduced_program == NULL) { 329 return; 330 } 331 332 summary->num_parameter_blocks_reduced = reduced_program->NumParameterBlocks(); 333 summary->num_parameters_reduced = reduced_program->NumParameters(); 334 summary->num_residual_blocks_reduced = reduced_program->NumResidualBlocks(); 335 summary->num_residuals_reduced = reduced_program->NumResiduals(); 336 337 if (summary->num_parameter_blocks_reduced == 0) { 338 summary->preprocessor_time_in_seconds = 339 WallTimeInSeconds() - solver_start_time; 340 341 LOG(INFO) << "Terminating: FUNCTION_TOLERANCE reached. " 342 << "No non-constant parameter blocks found."; 343 344 // FUNCTION_TOLERANCE is the right convergence here, as we know 345 // that the objective function is constant and cannot be changed 346 // any further. 347 summary->termination_type = FUNCTION_TOLERANCE; 348 349 double post_process_start_time = WallTimeInSeconds(); 350 // Evaluate the final cost, residual vector and the jacobian 351 // matrix if requested by the user. 352 if (!Evaluator::Evaluate(original_program, 353 options.num_threads, 354 &summary->final_cost, 355 options.return_final_residuals 356 ? &summary->final_residuals 357 : NULL, 358 options.return_final_gradient 359 ? &summary->final_gradient 360 : NULL, 361 options.return_final_jacobian 362 ? &summary->final_jacobian 363 : NULL)) { 364 summary->termination_type = NUMERICAL_FAILURE; 365 summary->error = "Unable to evaluate the final cost."; 366 LOG(ERROR) << summary->error; 367 return; 368 } 369 370 // Ensure the program state is set to the user parameters on the way out. 371 original_program->SetParameterBlockStatePtrsToUserStatePtrs(); 372 373 summary->postprocessor_time_in_seconds = 374 WallTimeInSeconds() - post_process_start_time; 375 return; 376 } 377 378 scoped_ptr<LinearSolver> 379 linear_solver(CreateLinearSolver(&options, &summary->error)); 380 if (linear_solver == NULL) { 381 return; 382 } 383 384 summary->linear_solver_type_given = original_options.linear_solver_type; 385 summary->linear_solver_type_used = options.linear_solver_type; 386 387 summary->preconditioner_type = options.preconditioner_type; 388 389 summary->num_linear_solver_threads_given = 390 original_options.num_linear_solver_threads; 391 summary->num_linear_solver_threads_used = options.num_linear_solver_threads; 392 393 summary->sparse_linear_algebra_library = 394 options.sparse_linear_algebra_library; 395 396 summary->trust_region_strategy_type = options.trust_region_strategy_type; 397 summary->dogleg_type = options.dogleg_type; 398 399 // Only Schur types require the lexicographic reordering. 400 if (IsSchurType(options.linear_solver_type)) { 401 const int num_eliminate_blocks = 402 options.linear_solver_ordering 403 ->group_to_elements().begin() 404 ->second.size(); 405 if (!LexicographicallyOrderResidualBlocks(num_eliminate_blocks, 406 reduced_program.get(), 407 &summary->error)) { 408 return; 409 } 410 } 411 412 scoped_ptr<Evaluator> evaluator(CreateEvaluator(options, 413 problem_impl->parameter_map(), 414 reduced_program.get(), 415 &summary->error)); 416 if (evaluator == NULL) { 417 return; 418 } 419 420 scoped_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer; 421 if (options.use_inner_iterations) { 422 if (reduced_program->parameter_blocks().size() < 2) { 423 LOG(WARNING) << "Reduced problem only contains one parameter block." 424 << "Disabling inner iterations."; 425 } else { 426 inner_iteration_minimizer.reset( 427 CreateInnerIterationMinimizer(original_options, 428 *reduced_program, 429 problem_impl->parameter_map(), 430 &summary->error)); 431 if (inner_iteration_minimizer == NULL) { 432 LOG(ERROR) << summary->error; 433 return; 434 } 435 } 436 } 437 438 // The optimizer works on contiguous parameter vectors; allocate some. 439 Vector parameters(reduced_program->NumParameters()); 440 441 // Collect the discontiguous parameters into a contiguous state vector. 442 reduced_program->ParameterBlocksToStateVector(parameters.data()); 443 444 Vector original_parameters = parameters; 445 446 double minimizer_start_time = WallTimeInSeconds(); 447 summary->preprocessor_time_in_seconds = 448 minimizer_start_time - solver_start_time; 449 450 // Run the optimization. 451 Minimize(options, 452 reduced_program.get(), 453 inner_iteration_minimizer.get(), 454 evaluator.get(), 455 linear_solver.get(), 456 parameters.data(), 457 summary); 458 459 // If the user aborted mid-optimization or the optimization 460 // terminated because of a numerical failure, then return without 461 // updating user state. 462 if (summary->termination_type == USER_ABORT || 463 summary->termination_type == NUMERICAL_FAILURE) { 464 return; 465 } 466 467 double post_process_start_time = WallTimeInSeconds(); 468 469 // Push the contiguous optimized parameters back to the user's parameters. 470 reduced_program->StateVectorToParameterBlocks(parameters.data()); 471 reduced_program->CopyParameterBlockStateToUserState(); 472 473 // Evaluate the final cost, residual vector and the jacobian 474 // matrix if requested by the user. 475 if (!Evaluator::Evaluate(original_program, 476 options.num_threads, 477 &summary->final_cost, 478 options.return_final_residuals 479 ? &summary->final_residuals 480 : NULL, 481 options.return_final_gradient 482 ? &summary->final_gradient 483 : NULL, 484 options.return_final_jacobian 485 ? &summary->final_jacobian 486 : NULL)) { 487 // This failure requires careful handling. 488 // 489 // At this point, we have modified the user's state, but the 490 // evaluation failed and we inform him of NUMERICAL_FAILURE. Ceres 491 // guarantees that user's state is not modified if the solver 492 // returns with NUMERICAL_FAILURE. Thus, we need to restore the 493 // user's state to their original values. 494 495 reduced_program->StateVectorToParameterBlocks(original_parameters.data()); 496 reduced_program->CopyParameterBlockStateToUserState(); 497 498 summary->termination_type = NUMERICAL_FAILURE; 499 summary->error = "Unable to evaluate the final cost."; 500 LOG(ERROR) << summary->error; 501 return; 502 } 503 504 // Ensure the program state is set to the user parameters on the way out. 505 original_program->SetParameterBlockStatePtrsToUserStatePtrs(); 506 507 // Stick a fork in it, we're done. 508 summary->postprocessor_time_in_seconds = 509 WallTimeInSeconds() - post_process_start_time; 510 } 511 512 bool SolverImpl::IsOrderingValid(const Solver::Options& options, 513 const ProblemImpl* problem_impl, 514 string* error) { 515 if (options.linear_solver_ordering->NumElements() != 516 problem_impl->NumParameterBlocks()) { 517 *error = "Number of parameter blocks in user supplied ordering " 518 "does not match the number of parameter blocks in the problem"; 519 return false; 520 } 521 522 const Program& program = problem_impl->program(); 523 const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); 524 for (vector<ParameterBlock*>::const_iterator it = parameter_blocks.begin(); 525 it != parameter_blocks.end(); 526 ++it) { 527 if (!options.linear_solver_ordering 528 ->IsMember(const_cast<double*>((*it)->user_state()))) { 529 *error = "Problem contains a parameter block that is not in " 530 "the user specified ordering."; 531 return false; 532 } 533 } 534 535 if (IsSchurType(options.linear_solver_type) && 536 options.linear_solver_ordering->NumGroups() > 1) { 537 const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); 538 const set<double*>& e_blocks = 539 options.linear_solver_ordering->group_to_elements().begin()->second; 540 if (!IsParameterBlockSetIndependent(e_blocks, residual_blocks)) { 541 *error = "The user requested the use of a Schur type solver. " 542 "But the first elimination group in the ordering is not an " 543 "independent set."; 544 return false; 545 } 546 } 547 return true; 548 } 549 550 bool SolverImpl::IsParameterBlockSetIndependent(const set<double*>& parameter_block_ptrs, 551 const vector<ResidualBlock*>& residual_blocks) { 552 // Loop over each residual block and ensure that no two parameter 553 // blocks in the same residual block are part of 554 // parameter_block_ptrs as that would violate the assumption that it 555 // is an independent set in the Hessian matrix. 556 for (vector<ResidualBlock*>::const_iterator it = residual_blocks.begin(); 557 it != residual_blocks.end(); 558 ++it) { 559 ParameterBlock* const* parameter_blocks = (*it)->parameter_blocks(); 560 const int num_parameter_blocks = (*it)->NumParameterBlocks(); 561 int count = 0; 562 for (int i = 0; i < num_parameter_blocks; ++i) { 563 count += parameter_block_ptrs.count( 564 parameter_blocks[i]->mutable_user_state()); 565 } 566 if (count > 1) { 567 return false; 568 } 569 } 570 return true; 571 } 572 573 574 // Strips varying parameters and residuals, maintaining order, and updating 575 // num_eliminate_blocks. 576 bool SolverImpl::RemoveFixedBlocksFromProgram(Program* program, 577 ParameterBlockOrdering* ordering, 578 double* fixed_cost, 579 string* error) { 580 vector<ParameterBlock*>* parameter_blocks = 581 program->mutable_parameter_blocks(); 582 583 scoped_array<double> residual_block_evaluate_scratch; 584 if (fixed_cost != NULL) { 585 residual_block_evaluate_scratch.reset( 586 new double[program->MaxScratchDoublesNeededForEvaluate()]); 587 *fixed_cost = 0.0; 588 } 589 590 // Mark all the parameters as unused. Abuse the index member of the parameter 591 // blocks for the marking. 592 for (int i = 0; i < parameter_blocks->size(); ++i) { 593 (*parameter_blocks)[i]->set_index(-1); 594 } 595 596 // Filter out residual that have all-constant parameters, and mark all the 597 // parameter blocks that appear in residuals. 598 { 599 vector<ResidualBlock*>* residual_blocks = 600 program->mutable_residual_blocks(); 601 int j = 0; 602 for (int i = 0; i < residual_blocks->size(); ++i) { 603 ResidualBlock* residual_block = (*residual_blocks)[i]; 604 int num_parameter_blocks = residual_block->NumParameterBlocks(); 605 606 // Determine if the residual block is fixed, and also mark varying 607 // parameters that appear in the residual block. 608 bool all_constant = true; 609 for (int k = 0; k < num_parameter_blocks; k++) { 610 ParameterBlock* parameter_block = residual_block->parameter_blocks()[k]; 611 if (!parameter_block->IsConstant()) { 612 all_constant = false; 613 parameter_block->set_index(1); 614 } 615 } 616 617 if (!all_constant) { 618 (*residual_blocks)[j++] = (*residual_blocks)[i]; 619 } else if (fixed_cost != NULL) { 620 // The residual is constant and will be removed, so its cost is 621 // added to the variable fixed_cost. 622 double cost = 0.0; 623 if (!residual_block->Evaluate( 624 &cost, NULL, NULL, residual_block_evaluate_scratch.get())) { 625 *error = StringPrintf("Evaluation of the residual %d failed during " 626 "removal of fixed residual blocks.", i); 627 return false; 628 } 629 *fixed_cost += cost; 630 } 631 } 632 residual_blocks->resize(j); 633 } 634 635 // Filter out unused or fixed parameter blocks, and update 636 // the ordering. 637 { 638 vector<ParameterBlock*>* parameter_blocks = 639 program->mutable_parameter_blocks(); 640 int j = 0; 641 for (int i = 0; i < parameter_blocks->size(); ++i) { 642 ParameterBlock* parameter_block = (*parameter_blocks)[i]; 643 if (parameter_block->index() == 1) { 644 (*parameter_blocks)[j++] = parameter_block; 645 } else { 646 ordering->Remove(parameter_block->mutable_user_state()); 647 } 648 } 649 parameter_blocks->resize(j); 650 } 651 652 CHECK(((program->NumResidualBlocks() == 0) && 653 (program->NumParameterBlocks() == 0)) || 654 ((program->NumResidualBlocks() != 0) && 655 (program->NumParameterBlocks() != 0))) 656 << "Congratulations, you found a bug in Ceres. Please report it."; 657 return true; 658 } 659 660 Program* SolverImpl::CreateReducedProgram(Solver::Options* options, 661 ProblemImpl* problem_impl, 662 double* fixed_cost, 663 string* error) { 664 CHECK_NOTNULL(options->linear_solver_ordering); 665 Program* original_program = problem_impl->mutable_program(); 666 scoped_ptr<Program> transformed_program(new Program(*original_program)); 667 ParameterBlockOrdering* linear_solver_ordering = 668 options->linear_solver_ordering; 669 670 const int min_group_id = 671 linear_solver_ordering->group_to_elements().begin()->first; 672 const int original_num_groups = linear_solver_ordering->NumGroups(); 673 674 if (!RemoveFixedBlocksFromProgram(transformed_program.get(), 675 linear_solver_ordering, 676 fixed_cost, 677 error)) { 678 return NULL; 679 } 680 681 if (transformed_program->NumParameterBlocks() == 0) { 682 if (transformed_program->NumResidualBlocks() > 0) { 683 *error = "Zero parameter blocks but non-zero residual blocks" 684 " in the reduced program. Congratulations, you found a " 685 "Ceres bug! Please report this error to the developers."; 686 return NULL; 687 } 688 689 LOG(WARNING) << "No varying parameter blocks to optimize; " 690 << "bailing early."; 691 return transformed_program.release(); 692 } 693 694 // If the user supplied an linear_solver_ordering with just one 695 // group, it is equivalent to the user supplying NULL as 696 // ordering. Ceres is completely free to choose the parameter block 697 // ordering as it sees fit. For Schur type solvers, this means that 698 // the user wishes for Ceres to identify the e_blocks, which we do 699 // by computing a maximal independent set. 700 if (original_num_groups == 1 && IsSchurType(options->linear_solver_type)) { 701 vector<ParameterBlock*> schur_ordering; 702 const int num_eliminate_blocks = ComputeSchurOrdering(*transformed_program, 703 &schur_ordering); 704 CHECK_EQ(schur_ordering.size(), transformed_program->NumParameterBlocks()) 705 << "Congratulations, you found a Ceres bug! Please report this error " 706 << "to the developers."; 707 708 for (int i = 0; i < schur_ordering.size(); ++i) { 709 linear_solver_ordering->AddElementToGroup( 710 schur_ordering[i]->mutable_user_state(), 711 (i < num_eliminate_blocks) ? 0 : 1); 712 } 713 } 714 715 if (!ApplyUserOrdering(problem_impl->parameter_map(), 716 linear_solver_ordering, 717 transformed_program.get(), 718 error)) { 719 return NULL; 720 } 721 722 // If the user requested the use of a Schur type solver, and 723 // supplied a non-NULL linear_solver_ordering object with more than 724 // one elimination group, then it can happen that after all the 725 // parameter blocks which are fixed or unused have been removed from 726 // the program and the ordering, there are no more parameter blocks 727 // in the first elimination group. 728 // 729 // In such a case, the use of a Schur type solver is not possible, 730 // as they assume there is at least one e_block. Thus, we 731 // automatically switch to one of the other solvers, depending on 732 // the user's indicated preferences. 733 if (IsSchurType(options->linear_solver_type) && 734 original_num_groups > 1 && 735 linear_solver_ordering->GroupSize(min_group_id) == 0) { 736 string msg = "No e_blocks remaining. Switching from "; 737 if (options->linear_solver_type == SPARSE_SCHUR) { 738 options->linear_solver_type = SPARSE_NORMAL_CHOLESKY; 739 msg += "SPARSE_SCHUR to SPARSE_NORMAL_CHOLESKY."; 740 } else if (options->linear_solver_type == DENSE_SCHUR) { 741 // TODO(sameeragarwal): This is probably not a great choice. 742 // Ideally, we should have a DENSE_NORMAL_CHOLESKY, that can 743 // take a BlockSparseMatrix as input. 744 options->linear_solver_type = DENSE_QR; 745 msg += "DENSE_SCHUR to DENSE_QR."; 746 } else if (options->linear_solver_type == ITERATIVE_SCHUR) { 747 msg += StringPrintf("ITERATIVE_SCHUR with %s preconditioner " 748 "to CGNR with JACOBI preconditioner.", 749 PreconditionerTypeToString( 750 options->preconditioner_type)); 751 options->linear_solver_type = CGNR; 752 if (options->preconditioner_type != IDENTITY) { 753 // CGNR currently only supports the JACOBI preconditioner. 754 options->preconditioner_type = JACOBI; 755 } 756 } 757 758 LOG(WARNING) << msg; 759 } 760 761 // Since the transformed program is the "active" program, and it is mutated, 762 // update the parameter offsets and indices. 763 transformed_program->SetParameterOffsetsAndIndex(); 764 return transformed_program.release(); 765 } 766 767 LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, 768 string* error) { 769 CHECK_NOTNULL(options); 770 CHECK_NOTNULL(options->linear_solver_ordering); 771 CHECK_NOTNULL(error); 772 773 if (options->trust_region_strategy_type == DOGLEG) { 774 if (options->linear_solver_type == ITERATIVE_SCHUR || 775 options->linear_solver_type == CGNR) { 776 *error = "DOGLEG only supports exact factorization based linear " 777 "solvers. If you want to use an iterative solver please " 778 "use LEVENBERG_MARQUARDT as the trust_region_strategy_type"; 779 return NULL; 780 } 781 } 782 783 #ifdef CERES_NO_SUITESPARSE 784 if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY && 785 options->sparse_linear_algebra_library == SUITE_SPARSE) { 786 *error = "Can't use SPARSE_NORMAL_CHOLESKY with SUITESPARSE because " 787 "SuiteSparse was not enabled when Ceres was built."; 788 return NULL; 789 } 790 791 if (options->preconditioner_type == SCHUR_JACOBI) { 792 *error = "SCHUR_JACOBI preconditioner not suppored. Please build Ceres " 793 "with SuiteSparse support."; 794 return NULL; 795 } 796 797 if (options->preconditioner_type == CLUSTER_JACOBI) { 798 *error = "CLUSTER_JACOBI preconditioner not suppored. Please build Ceres " 799 "with SuiteSparse support."; 800 return NULL; 801 } 802 803 if (options->preconditioner_type == CLUSTER_TRIDIAGONAL) { 804 *error = "CLUSTER_TRIDIAGONAL preconditioner not suppored. Please build " 805 "Ceres with SuiteSparse support."; 806 return NULL; 807 } 808 #endif 809 810 #ifdef CERES_NO_CXSPARSE 811 if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY && 812 options->sparse_linear_algebra_library == CX_SPARSE) { 813 *error = "Can't use SPARSE_NORMAL_CHOLESKY with CXSPARSE because " 814 "CXSparse was not enabled when Ceres was built."; 815 return NULL; 816 } 817 #endif 818 819 #if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) 820 if (options->linear_solver_type == SPARSE_SCHUR) { 821 *error = "Can't use SPARSE_SCHUR because neither SuiteSparse nor" 822 "CXSparse was enabled when Ceres was compiled."; 823 return NULL; 824 } 825 #endif 826 827 if (options->linear_solver_max_num_iterations <= 0) { 828 *error = "Solver::Options::linear_solver_max_num_iterations is 0."; 829 return NULL; 830 } 831 if (options->linear_solver_min_num_iterations <= 0) { 832 *error = "Solver::Options::linear_solver_min_num_iterations is 0."; 833 return NULL; 834 } 835 if (options->linear_solver_min_num_iterations > 836 options->linear_solver_max_num_iterations) { 837 *error = "Solver::Options::linear_solver_min_num_iterations > " 838 "Solver::Options::linear_solver_max_num_iterations."; 839 return NULL; 840 } 841 842 LinearSolver::Options linear_solver_options; 843 linear_solver_options.min_num_iterations = 844 options->linear_solver_min_num_iterations; 845 linear_solver_options.max_num_iterations = 846 options->linear_solver_max_num_iterations; 847 linear_solver_options.type = options->linear_solver_type; 848 linear_solver_options.preconditioner_type = options->preconditioner_type; 849 linear_solver_options.sparse_linear_algebra_library = 850 options->sparse_linear_algebra_library; 851 852 linear_solver_options.num_threads = options->num_linear_solver_threads; 853 // The matrix used for storing the dense Schur complement has a 854 // single lock guarding the whole matrix. Running the 855 // SchurComplementSolver with multiple threads leads to maximum 856 // contention and slowdown. If the problem is large enough to 857 // benefit from a multithreaded schur eliminator, you should be 858 // using a SPARSE_SCHUR solver anyways. 859 if ((linear_solver_options.num_threads > 1) && 860 (linear_solver_options.type == DENSE_SCHUR)) { 861 LOG(WARNING) << "Warning: Solver::Options::num_linear_solver_threads = " 862 << options->num_linear_solver_threads 863 << " with DENSE_SCHUR will result in poor performance; " 864 << "switching to single-threaded."; 865 linear_solver_options.num_threads = 1; 866 } 867 options->num_linear_solver_threads = linear_solver_options.num_threads; 868 869 linear_solver_options.use_block_amd = options->use_block_amd; 870 const map<int, set<double*> >& groups = 871 options->linear_solver_ordering->group_to_elements(); 872 for (map<int, set<double*> >::const_iterator it = groups.begin(); 873 it != groups.end(); 874 ++it) { 875 linear_solver_options.elimination_groups.push_back(it->second.size()); 876 } 877 // Schur type solvers, expect at least two elimination groups. If 878 // there is only one elimination group, then CreateReducedProgram 879 // guarantees that this group only contains e_blocks. Thus we add a 880 // dummy elimination group with zero blocks in it. 881 if (IsSchurType(linear_solver_options.type) && 882 linear_solver_options.elimination_groups.size() == 1) { 883 linear_solver_options.elimination_groups.push_back(0); 884 } 885 886 return LinearSolver::Create(linear_solver_options); 887 } 888 889 bool SolverImpl::ApplyUserOrdering(const ProblemImpl::ParameterMap& parameter_map, 890 const ParameterBlockOrdering* ordering, 891 Program* program, 892 string* error) { 893 if (ordering->NumElements() != program->NumParameterBlocks()) { 894 *error = StringPrintf("User specified ordering does not have the same " 895 "number of parameters as the problem. The problem" 896 "has %d blocks while the ordering has %d blocks.", 897 program->NumParameterBlocks(), 898 ordering->NumElements()); 899 return false; 900 } 901 902 vector<ParameterBlock*>* parameter_blocks = 903 program->mutable_parameter_blocks(); 904 parameter_blocks->clear(); 905 906 const map<int, set<double*> >& groups = 907 ordering->group_to_elements(); 908 909 for (map<int, set<double*> >::const_iterator group_it = groups.begin(); 910 group_it != groups.end(); 911 ++group_it) { 912 const set<double*>& group = group_it->second; 913 for (set<double*>::const_iterator parameter_block_ptr_it = group.begin(); 914 parameter_block_ptr_it != group.end(); 915 ++parameter_block_ptr_it) { 916 ProblemImpl::ParameterMap::const_iterator parameter_block_it = 917 parameter_map.find(*parameter_block_ptr_it); 918 if (parameter_block_it == parameter_map.end()) { 919 *error = StringPrintf("User specified ordering contains a pointer " 920 "to a double that is not a parameter block in the " 921 "problem. The invalid double is in group: %d", 922 group_it->first); 923 return false; 924 } 925 parameter_blocks->push_back(parameter_block_it->second); 926 } 927 } 928 return true; 929 } 930 931 // Find the minimum index of any parameter block to the given residual. 932 // Parameter blocks that have indices greater than num_eliminate_blocks are 933 // considered to have an index equal to num_eliminate_blocks. 934 int MinParameterBlock(const ResidualBlock* residual_block, 935 int num_eliminate_blocks) { 936 int min_parameter_block_position = num_eliminate_blocks; 937 for (int i = 0; i < residual_block->NumParameterBlocks(); ++i) { 938 ParameterBlock* parameter_block = residual_block->parameter_blocks()[i]; 939 if (!parameter_block->IsConstant()) { 940 CHECK_NE(parameter_block->index(), -1) 941 << "Did you forget to call Program::SetParameterOffsetsAndIndex()? " 942 << "This is a Ceres bug; please contact the developers!"; 943 min_parameter_block_position = std::min(parameter_block->index(), 944 min_parameter_block_position); 945 } 946 } 947 return min_parameter_block_position; 948 } 949 950 // Reorder the residuals for program, if necessary, so that the residuals 951 // involving each E block occur together. This is a necessary condition for the 952 // Schur eliminator, which works on these "row blocks" in the jacobian. 953 bool SolverImpl::LexicographicallyOrderResidualBlocks(const int num_eliminate_blocks, 954 Program* program, 955 string* error) { 956 CHECK_GE(num_eliminate_blocks, 1) 957 << "Congratulations, you found a Ceres bug! Please report this error " 958 << "to the developers."; 959 960 // Create a histogram of the number of residuals for each E block. There is an 961 // extra bucket at the end to catch all non-eliminated F blocks. 962 vector<int> residual_blocks_per_e_block(num_eliminate_blocks + 1); 963 vector<ResidualBlock*>* residual_blocks = program->mutable_residual_blocks(); 964 vector<int> min_position_per_residual(residual_blocks->size()); 965 for (int i = 0; i < residual_blocks->size(); ++i) { 966 ResidualBlock* residual_block = (*residual_blocks)[i]; 967 int position = MinParameterBlock(residual_block, num_eliminate_blocks); 968 min_position_per_residual[i] = position; 969 DCHECK_LE(position, num_eliminate_blocks); 970 residual_blocks_per_e_block[position]++; 971 } 972 973 // Run a cumulative sum on the histogram, to obtain offsets to the start of 974 // each histogram bucket (where each bucket is for the residuals for that 975 // E-block). 976 vector<int> offsets(num_eliminate_blocks + 1); 977 std::partial_sum(residual_blocks_per_e_block.begin(), 978 residual_blocks_per_e_block.end(), 979 offsets.begin()); 980 CHECK_EQ(offsets.back(), residual_blocks->size()) 981 << "Congratulations, you found a Ceres bug! Please report this error " 982 << "to the developers."; 983 984 CHECK(find(residual_blocks_per_e_block.begin(), 985 residual_blocks_per_e_block.end() - 1, 0) != 986 residual_blocks_per_e_block.end()) 987 << "Congratulations, you found a Ceres bug! Please report this error " 988 << "to the developers."; 989 990 // Fill in each bucket with the residual blocks for its corresponding E block. 991 // Each bucket is individually filled from the back of the bucket to the front 992 // of the bucket. The filling order among the buckets is dictated by the 993 // residual blocks. This loop uses the offsets as counters; subtracting one 994 // from each offset as a residual block is placed in the bucket. When the 995 // filling is finished, the offset pointerts should have shifted down one 996 // entry (this is verified below). 997 vector<ResidualBlock*> reordered_residual_blocks( 998 (*residual_blocks).size(), static_cast<ResidualBlock*>(NULL)); 999 for (int i = 0; i < residual_blocks->size(); ++i) { 1000 int bucket = min_position_per_residual[i]; 1001 1002 // Decrement the cursor, which should now point at the next empty position. 1003 offsets[bucket]--; 1004 1005 // Sanity. 1006 CHECK(reordered_residual_blocks[offsets[bucket]] == NULL) 1007 << "Congratulations, you found a Ceres bug! Please report this error " 1008 << "to the developers."; 1009 1010 reordered_residual_blocks[offsets[bucket]] = (*residual_blocks)[i]; 1011 } 1012 1013 // Sanity check #1: The difference in bucket offsets should match the 1014 // histogram sizes. 1015 for (int i = 0; i < num_eliminate_blocks; ++i) { 1016 CHECK_EQ(residual_blocks_per_e_block[i], offsets[i + 1] - offsets[i]) 1017 << "Congratulations, you found a Ceres bug! Please report this error " 1018 << "to the developers."; 1019 } 1020 // Sanity check #2: No NULL's left behind. 1021 for (int i = 0; i < reordered_residual_blocks.size(); ++i) { 1022 CHECK(reordered_residual_blocks[i] != NULL) 1023 << "Congratulations, you found a Ceres bug! Please report this error " 1024 << "to the developers."; 1025 } 1026 1027 // Now that the residuals are collected by E block, swap them in place. 1028 swap(*program->mutable_residual_blocks(), reordered_residual_blocks); 1029 return true; 1030 } 1031 1032 Evaluator* SolverImpl::CreateEvaluator(const Solver::Options& options, 1033 const ProblemImpl::ParameterMap& parameter_map, 1034 Program* program, 1035 string* error) { 1036 Evaluator::Options evaluator_options; 1037 evaluator_options.linear_solver_type = options.linear_solver_type; 1038 evaluator_options.num_eliminate_blocks = 1039 (options.linear_solver_ordering->NumGroups() > 0 && 1040 IsSchurType(options.linear_solver_type)) 1041 ? (options.linear_solver_ordering 1042 ->group_to_elements().begin() 1043 ->second.size()) 1044 : 0; 1045 evaluator_options.num_threads = options.num_threads; 1046 return Evaluator::Create(evaluator_options, program, error); 1047 } 1048 1049 CoordinateDescentMinimizer* SolverImpl::CreateInnerIterationMinimizer( 1050 const Solver::Options& options, 1051 const Program& program, 1052 const ProblemImpl::ParameterMap& parameter_map, 1053 string* error) { 1054 scoped_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer( 1055 new CoordinateDescentMinimizer); 1056 scoped_ptr<ParameterBlockOrdering> inner_iteration_ordering; 1057 ParameterBlockOrdering* ordering_ptr = NULL; 1058 1059 if (options.inner_iteration_ordering == NULL) { 1060 // Find a recursive decomposition of the Hessian matrix as a set 1061 // of independent sets of decreasing size and invert it. This 1062 // seems to work better in practice, i.e., Cameras before 1063 // points. 1064 inner_iteration_ordering.reset(new ParameterBlockOrdering); 1065 ComputeRecursiveIndependentSetOrdering(program, 1066 inner_iteration_ordering.get()); 1067 inner_iteration_ordering->Reverse(); 1068 ordering_ptr = inner_iteration_ordering.get(); 1069 } else { 1070 const map<int, set<double*> >& group_to_elements = 1071 options.inner_iteration_ordering->group_to_elements(); 1072 1073 // Iterate over each group and verify that it is an independent 1074 // set. 1075 map<int, set<double*> >::const_iterator it = group_to_elements.begin(); 1076 for ( ;it != group_to_elements.end(); ++it) { 1077 if (!IsParameterBlockSetIndependent(it->second, 1078 program.residual_blocks())) { 1079 *error = 1080 StringPrintf("The user-provided " 1081 "parameter_blocks_for_inner_iterations does not " 1082 "form an independent set. Group Id: %d", it->first); 1083 return NULL; 1084 } 1085 } 1086 ordering_ptr = options.inner_iteration_ordering; 1087 } 1088 1089 if (!inner_iteration_minimizer->Init(program, 1090 parameter_map, 1091 *ordering_ptr, 1092 error)) { 1093 return NULL; 1094 } 1095 1096 return inner_iteration_minimizer.release(); 1097 } 1098 1099 } // namespace internal 1100 } // namespace ceres 1101
