diff options
Diffstat (limited to 'internal/ceres/solver_impl.cc')
| -rw-r--r-- | internal/ceres/solver_impl.cc | 1100 |
1 files changed, 1100 insertions, 0 deletions
diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc new file mode 100644 index 0000000..64e0f8e --- /dev/null +++ b/internal/ceres/solver_impl.cc @@ -0,0 +1,1100 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/solver_impl.h" + +#include <cstdio> +#include <iostream> // NOLINT +#include <numeric> +#include "ceres/coordinate_descent_minimizer.h" +#include "ceres/evaluator.h" +#include "ceres/gradient_checking_cost_function.h" +#include "ceres/iteration_callback.h" +#include "ceres/levenberg_marquardt_strategy.h" +#include "ceres/linear_solver.h" +#include "ceres/map_util.h" +#include "ceres/minimizer.h" +#include "ceres/ordered_groups.h" +#include "ceres/parameter_block.h" +#include "ceres/parameter_block_ordering.h" +#include "ceres/problem.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/stringprintf.h" +#include "ceres/trust_region_minimizer.h" +#include "ceres/wall_time.h" + +namespace ceres { +namespace internal { +namespace { + +// Callback for updating the user's parameter blocks. Updates are only +// done if the step is successful. +class StateUpdatingCallback : public IterationCallback { + public: + StateUpdatingCallback(Program* program, double* parameters) + : program_(program), parameters_(parameters) {} + + CallbackReturnType operator()(const IterationSummary& summary) { + if (summary.step_is_successful) { + program_->StateVectorToParameterBlocks(parameters_); + program_->CopyParameterBlockStateToUserState(); + } + return SOLVER_CONTINUE; + } + + private: + Program* program_; + double* parameters_; +}; + +// Callback for logging the state of the minimizer to STDERR or STDOUT +// depending on the user's preferences and logging level. +class LoggingCallback : public IterationCallback { + public: + explicit LoggingCallback(bool log_to_stdout) + : log_to_stdout_(log_to_stdout) {} + + ~LoggingCallback() {} + + CallbackReturnType operator()(const IterationSummary& summary) { + const char* kReportRowFormat = + "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e " + "rho:% 3.2e mu:% 3.2e li:% 3d it:% 3.2e tt:% 3.2e"; + string output = StringPrintf(kReportRowFormat, + summary.iteration, + summary.cost, + summary.cost_change, + summary.gradient_max_norm, + summary.step_norm, + summary.relative_decrease, + summary.trust_region_radius, + summary.linear_solver_iterations, + summary.iteration_time_in_seconds, + summary.cumulative_time_in_seconds); + if (log_to_stdout_) { + cout << output << endl; + } else { + VLOG(1) << output; + } + return SOLVER_CONTINUE; + } + + private: + const bool log_to_stdout_; +}; + +// Basic callback to record the execution of the solver to a file for +// offline analysis. +class FileLoggingCallback : public IterationCallback { + public: + explicit FileLoggingCallback(const string& filename) + : fptr_(NULL) { + fptr_ = fopen(filename.c_str(), "w"); + CHECK_NOTNULL(fptr_); + } + + virtual ~FileLoggingCallback() { + if (fptr_ != NULL) { + fclose(fptr_); + } + } + + virtual CallbackReturnType operator()(const IterationSummary& summary) { + fprintf(fptr_, + "%4d %e %e\n", + summary.iteration, + summary.cost, + summary.cumulative_time_in_seconds); + return SOLVER_CONTINUE; + } + private: + FILE* fptr_; +}; + +} // namespace + +void SolverImpl::Minimize(const Solver::Options& options, + Program* program, + CoordinateDescentMinimizer* inner_iteration_minimizer, + Evaluator* evaluator, + LinearSolver* linear_solver, + double* parameters, + Solver::Summary* summary) { + Minimizer::Options minimizer_options(options); + + // TODO(sameeragarwal): Add support for logging the configuration + // and more detailed stats. + scoped_ptr<IterationCallback> file_logging_callback; + if (!options.solver_log.empty()) { + file_logging_callback.reset(new FileLoggingCallback(options.solver_log)); + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + file_logging_callback.get()); + } + + LoggingCallback logging_callback(options.minimizer_progress_to_stdout); + if (options.logging_type != SILENT) { + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + &logging_callback); + } + + StateUpdatingCallback updating_callback(program, parameters); + if (options.update_state_every_iteration) { + // This must get pushed to the front of the callbacks so that it is run + // before any of the user callbacks. + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + &updating_callback); + } + + minimizer_options.evaluator = evaluator; + scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian()); + minimizer_options.jacobian = jacobian.get(); + minimizer_options.inner_iteration_minimizer = inner_iteration_minimizer; + + TrustRegionStrategy::Options trust_region_strategy_options; + trust_region_strategy_options.linear_solver = linear_solver; + trust_region_strategy_options.initial_radius = + options.initial_trust_region_radius; + trust_region_strategy_options.max_radius = options.max_trust_region_radius; + trust_region_strategy_options.lm_min_diagonal = options.lm_min_diagonal; + trust_region_strategy_options.lm_max_diagonal = options.lm_max_diagonal; + trust_region_strategy_options.trust_region_strategy_type = + options.trust_region_strategy_type; + trust_region_strategy_options.dogleg_type = options.dogleg_type; + scoped_ptr<TrustRegionStrategy> strategy( + TrustRegionStrategy::Create(trust_region_strategy_options)); + minimizer_options.trust_region_strategy = strategy.get(); + + TrustRegionMinimizer minimizer; + double minimizer_start_time = WallTimeInSeconds(); + minimizer.Minimize(minimizer_options, parameters, summary); + summary->minimizer_time_in_seconds = + WallTimeInSeconds() - minimizer_start_time; +} + +void SolverImpl::Solve(const Solver::Options& original_options, + ProblemImpl* original_problem_impl, + Solver::Summary* summary) { + double solver_start_time = WallTimeInSeconds(); + + Program* original_program = original_problem_impl->mutable_program(); + ProblemImpl* problem_impl = original_problem_impl; + + // Reset the summary object to its default values. + *CHECK_NOTNULL(summary) = Solver::Summary(); + + summary->num_parameter_blocks = problem_impl->NumParameterBlocks(); + summary->num_parameters = problem_impl->NumParameters(); + summary->num_residual_blocks = problem_impl->NumResidualBlocks(); + summary->num_residuals = problem_impl->NumResiduals(); + + // Empty programs are usually a user error. + if (summary->num_parameter_blocks == 0) { + summary->error = "Problem contains no parameter blocks."; + LOG(ERROR) << summary->error; + return; + } + + if (summary->num_residual_blocks == 0) { + summary->error = "Problem contains no residual blocks."; + LOG(ERROR) << summary->error; + return; + } + + Solver::Options options(original_options); + options.linear_solver_ordering = NULL; + options.inner_iteration_ordering = NULL; + +#ifndef CERES_USE_OPENMP + if (options.num_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_threads=1 is supported. Switching " + << "to single threaded mode."; + options.num_threads = 1; + } + if (options.num_linear_solver_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_linear_solver_threads=1 is supported. Switching " + << "to single threaded mode."; + options.num_linear_solver_threads = 1; + } +#endif + + summary->num_threads_given = original_options.num_threads; + summary->num_threads_used = options.num_threads; + + if (options.lsqp_iterations_to_dump.size() > 0) { + LOG(WARNING) << "Dumping linear least squares problems to disk is" + " currently broken. Ignoring Solver::Options::lsqp_iterations_to_dump"; + } + + // Evaluate the initial cost, residual vector and the jacobian + // matrix if requested by the user. The initial cost needs to be + // computed on the original unpreprocessed problem, as it is used to + // determine the value of the "fixed" part of the objective function + // after the problem has undergone reduction. + if (!Evaluator::Evaluate(original_program, + options.num_threads, + &(summary->initial_cost), + options.return_initial_residuals + ? &summary->initial_residuals + : NULL, + options.return_initial_gradient + ? &summary->initial_gradient + : NULL, + options.return_initial_jacobian + ? &summary->initial_jacobian + : NULL)) { + summary->termination_type = NUMERICAL_FAILURE; + summary->error = "Unable to evaluate the initial cost."; + LOG(ERROR) << summary->error; + return; + } + + original_program->SetParameterBlockStatePtrsToUserStatePtrs(); + + // If the user requests gradient checking, construct a new + // ProblemImpl by wrapping the CostFunctions of problem_impl inside + // GradientCheckingCostFunction and replacing problem_impl with + // gradient_checking_problem_impl. + scoped_ptr<ProblemImpl> gradient_checking_problem_impl; + if (options.check_gradients) { + VLOG(1) << "Checking Gradients"; + gradient_checking_problem_impl.reset( + CreateGradientCheckingProblemImpl( + problem_impl, + options.numeric_derivative_relative_step_size, + options.gradient_check_relative_precision)); + + // From here on, problem_impl will point to the gradient checking + // version. + problem_impl = gradient_checking_problem_impl.get(); + } + + if (original_options.linear_solver_ordering != NULL) { + if (!IsOrderingValid(original_options, problem_impl, &summary->error)) { + LOG(ERROR) << summary->error; + return; + } + options.linear_solver_ordering = + new ParameterBlockOrdering(*original_options.linear_solver_ordering); + } else { + options.linear_solver_ordering = new ParameterBlockOrdering; + const ProblemImpl::ParameterMap& parameter_map = + problem_impl->parameter_map(); + for (ProblemImpl::ParameterMap::const_iterator it = parameter_map.begin(); + it != parameter_map.end(); + ++it) { + options.linear_solver_ordering->AddElementToGroup(it->first, 0); + } + } + + // Create the three objects needed to minimize: the transformed program, the + // evaluator, and the linear solver. + scoped_ptr<Program> reduced_program(CreateReducedProgram(&options, + problem_impl, + &summary->fixed_cost, + &summary->error)); + if (reduced_program == NULL) { + return; + } + + summary->num_parameter_blocks_reduced = reduced_program->NumParameterBlocks(); + summary->num_parameters_reduced = reduced_program->NumParameters(); + summary->num_residual_blocks_reduced = reduced_program->NumResidualBlocks(); + summary->num_residuals_reduced = reduced_program->NumResiduals(); + + if (summary->num_parameter_blocks_reduced == 0) { + summary->preprocessor_time_in_seconds = + WallTimeInSeconds() - solver_start_time; + + LOG(INFO) << "Terminating: FUNCTION_TOLERANCE reached. " + << "No non-constant parameter blocks found."; + + // FUNCTION_TOLERANCE is the right convergence here, as we know + // that the objective function is constant and cannot be changed + // any further. + summary->termination_type = FUNCTION_TOLERANCE; + + double post_process_start_time = WallTimeInSeconds(); + // Evaluate the final cost, residual vector and the jacobian + // matrix if requested by the user. + if (!Evaluator::Evaluate(original_program, + options.num_threads, + &summary->final_cost, + options.return_final_residuals + ? &summary->final_residuals + : NULL, + options.return_final_gradient + ? &summary->final_gradient + : NULL, + options.return_final_jacobian + ? &summary->final_jacobian + : NULL)) { + summary->termination_type = NUMERICAL_FAILURE; + summary->error = "Unable to evaluate the final cost."; + LOG(ERROR) << summary->error; + return; + } + + // Ensure the program state is set to the user parameters on the way out. + original_program->SetParameterBlockStatePtrsToUserStatePtrs(); + + summary->postprocessor_time_in_seconds = + WallTimeInSeconds() - post_process_start_time; + return; + } + + scoped_ptr<LinearSolver> + linear_solver(CreateLinearSolver(&options, &summary->error)); + if (linear_solver == NULL) { + return; + } + + summary->linear_solver_type_given = original_options.linear_solver_type; + summary->linear_solver_type_used = options.linear_solver_type; + + summary->preconditioner_type = options.preconditioner_type; + + summary->num_linear_solver_threads_given = + original_options.num_linear_solver_threads; + summary->num_linear_solver_threads_used = options.num_linear_solver_threads; + + summary->sparse_linear_algebra_library = + options.sparse_linear_algebra_library; + + summary->trust_region_strategy_type = options.trust_region_strategy_type; + summary->dogleg_type = options.dogleg_type; + + // Only Schur types require the lexicographic reordering. + if (IsSchurType(options.linear_solver_type)) { + const int num_eliminate_blocks = + options.linear_solver_ordering + ->group_to_elements().begin() + ->second.size(); + if (!LexicographicallyOrderResidualBlocks(num_eliminate_blocks, + reduced_program.get(), + &summary->error)) { + return; + } + } + + scoped_ptr<Evaluator> evaluator(CreateEvaluator(options, + problem_impl->parameter_map(), + reduced_program.get(), + &summary->error)); + if (evaluator == NULL) { + return; + } + + scoped_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer; + if (options.use_inner_iterations) { + if (reduced_program->parameter_blocks().size() < 2) { + LOG(WARNING) << "Reduced problem only contains one parameter block." + << "Disabling inner iterations."; + } else { + inner_iteration_minimizer.reset( + CreateInnerIterationMinimizer(original_options, + *reduced_program, + problem_impl->parameter_map(), + &summary->error)); + if (inner_iteration_minimizer == NULL) { + LOG(ERROR) << summary->error; + return; + } + } + } + + // The optimizer works on contiguous parameter vectors; allocate some. + Vector parameters(reduced_program->NumParameters()); + + // Collect the discontiguous parameters into a contiguous state vector. + reduced_program->ParameterBlocksToStateVector(parameters.data()); + + Vector original_parameters = parameters; + + double minimizer_start_time = WallTimeInSeconds(); + summary->preprocessor_time_in_seconds = + minimizer_start_time - solver_start_time; + + // Run the optimization. + Minimize(options, + reduced_program.get(), + inner_iteration_minimizer.get(), + evaluator.get(), + linear_solver.get(), + parameters.data(), + summary); + + // If the user aborted mid-optimization or the optimization + // terminated because of a numerical failure, then return without + // updating user state. + if (summary->termination_type == USER_ABORT || + summary->termination_type == NUMERICAL_FAILURE) { + return; + } + + double post_process_start_time = WallTimeInSeconds(); + + // Push the contiguous optimized parameters back to the user's parameters. + reduced_program->StateVectorToParameterBlocks(parameters.data()); + reduced_program->CopyParameterBlockStateToUserState(); + + // Evaluate the final cost, residual vector and the jacobian + // matrix if requested by the user. + if (!Evaluator::Evaluate(original_program, + options.num_threads, + &summary->final_cost, + options.return_final_residuals + ? &summary->final_residuals + : NULL, + options.return_final_gradient + ? &summary->final_gradient + : NULL, + options.return_final_jacobian + ? &summary->final_jacobian + : NULL)) { + // This failure requires careful handling. + // + // At this point, we have modified the user's state, but the + // evaluation failed and we inform him of NUMERICAL_FAILURE. Ceres + // guarantees that user's state is not modified if the solver + // returns with NUMERICAL_FAILURE. Thus, we need to restore the + // user's state to their original values. + + reduced_program->StateVectorToParameterBlocks(original_parameters.data()); + reduced_program->CopyParameterBlockStateToUserState(); + + summary->termination_type = NUMERICAL_FAILURE; + summary->error = "Unable to evaluate the final cost."; + LOG(ERROR) << summary->error; + return; + } + + // Ensure the program state is set to the user parameters on the way out. + original_program->SetParameterBlockStatePtrsToUserStatePtrs(); + + // Stick a fork in it, we're done. + summary->postprocessor_time_in_seconds = + WallTimeInSeconds() - post_process_start_time; +} + +bool SolverImpl::IsOrderingValid(const Solver::Options& options, + const ProblemImpl* problem_impl, + string* error) { + if (options.linear_solver_ordering->NumElements() != + problem_impl->NumParameterBlocks()) { + *error = "Number of parameter blocks in user supplied ordering " + "does not match the number of parameter blocks in the problem"; + return false; + } + + const Program& program = problem_impl->program(); + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); + for (vector<ParameterBlock*>::const_iterator it = parameter_blocks.begin(); + it != parameter_blocks.end(); + ++it) { + if (!options.linear_solver_ordering + ->IsMember(const_cast<double*>((*it)->user_state()))) { + *error = "Problem contains a parameter block that is not in " + "the user specified ordering."; + return false; + } + } + + if (IsSchurType(options.linear_solver_type) && + options.linear_solver_ordering->NumGroups() > 1) { + const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); + const set<double*>& e_blocks = + options.linear_solver_ordering->group_to_elements().begin()->second; + if (!IsParameterBlockSetIndependent(e_blocks, residual_blocks)) { + *error = "The user requested the use of a Schur type solver. " + "But the first elimination group in the ordering is not an " + "independent set."; + return false; + } + } + return true; +} + +bool SolverImpl::IsParameterBlockSetIndependent(const set<double*>& parameter_block_ptrs, + const vector<ResidualBlock*>& residual_blocks) { + // Loop over each residual block and ensure that no two parameter + // blocks in the same residual block are part of + // parameter_block_ptrs as that would violate the assumption that it + // is an independent set in the Hessian matrix. + for (vector<ResidualBlock*>::const_iterator it = residual_blocks.begin(); + it != residual_blocks.end(); + ++it) { + ParameterBlock* const* parameter_blocks = (*it)->parameter_blocks(); + const int num_parameter_blocks = (*it)->NumParameterBlocks(); + int count = 0; + for (int i = 0; i < num_parameter_blocks; ++i) { + count += parameter_block_ptrs.count( + parameter_blocks[i]->mutable_user_state()); + } + if (count > 1) { + return false; + } + } + return true; +} + + +// Strips varying parameters and residuals, maintaining order, and updating +// num_eliminate_blocks. +bool SolverImpl::RemoveFixedBlocksFromProgram(Program* program, + ParameterBlockOrdering* ordering, + double* fixed_cost, + string* error) { + vector<ParameterBlock*>* parameter_blocks = + program->mutable_parameter_blocks(); + + scoped_array<double> residual_block_evaluate_scratch; + if (fixed_cost != NULL) { + residual_block_evaluate_scratch.reset( + new double[program->MaxScratchDoublesNeededForEvaluate()]); + *fixed_cost = 0.0; + } + + // Mark all the parameters as unused. Abuse the index member of the parameter + // blocks for the marking. + for (int i = 0; i < parameter_blocks->size(); ++i) { + (*parameter_blocks)[i]->set_index(-1); + } + + // Filter out residual that have all-constant parameters, and mark all the + // parameter blocks that appear in residuals. + { + vector<ResidualBlock*>* residual_blocks = + program->mutable_residual_blocks(); + int j = 0; + for (int i = 0; i < residual_blocks->size(); ++i) { + ResidualBlock* residual_block = (*residual_blocks)[i]; + int num_parameter_blocks = residual_block->NumParameterBlocks(); + + // Determine if the residual block is fixed, and also mark varying + // parameters that appear in the residual block. + bool all_constant = true; + for (int k = 0; k < num_parameter_blocks; k++) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[k]; + if (!parameter_block->IsConstant()) { + all_constant = false; + parameter_block->set_index(1); + } + } + + if (!all_constant) { + (*residual_blocks)[j++] = (*residual_blocks)[i]; + } else if (fixed_cost != NULL) { + // The residual is constant and will be removed, so its cost is + // added to the variable fixed_cost. + double cost = 0.0; + if (!residual_block->Evaluate( + &cost, NULL, NULL, residual_block_evaluate_scratch.get())) { + *error = StringPrintf("Evaluation of the residual %d failed during " + "removal of fixed residual blocks.", i); + return false; + } + *fixed_cost += cost; + } + } + residual_blocks->resize(j); + } + + // Filter out unused or fixed parameter blocks, and update + // the ordering. + { + vector<ParameterBlock*>* parameter_blocks = + program->mutable_parameter_blocks(); + int j = 0; + for (int i = 0; i < parameter_blocks->size(); ++i) { + ParameterBlock* parameter_block = (*parameter_blocks)[i]; + if (parameter_block->index() == 1) { + (*parameter_blocks)[j++] = parameter_block; + } else { + ordering->Remove(parameter_block->mutable_user_state()); + } + } + parameter_blocks->resize(j); + } + + CHECK(((program->NumResidualBlocks() == 0) && + (program->NumParameterBlocks() == 0)) || + ((program->NumResidualBlocks() != 0) && + (program->NumParameterBlocks() != 0))) + << "Congratulations, you found a bug in Ceres. Please report it."; + return true; +} + +Program* SolverImpl::CreateReducedProgram(Solver::Options* options, + ProblemImpl* problem_impl, + double* fixed_cost, + string* error) { + CHECK_NOTNULL(options->linear_solver_ordering); + Program* original_program = problem_impl->mutable_program(); + scoped_ptr<Program> transformed_program(new Program(*original_program)); + ParameterBlockOrdering* linear_solver_ordering = + options->linear_solver_ordering; + + const int min_group_id = + linear_solver_ordering->group_to_elements().begin()->first; + const int original_num_groups = linear_solver_ordering->NumGroups(); + + if (!RemoveFixedBlocksFromProgram(transformed_program.get(), + linear_solver_ordering, + fixed_cost, + error)) { + return NULL; + } + + if (transformed_program->NumParameterBlocks() == 0) { + if (transformed_program->NumResidualBlocks() > 0) { + *error = "Zero parameter blocks but non-zero residual blocks" + " in the reduced program. Congratulations, you found a " + "Ceres bug! Please report this error to the developers."; + return NULL; + } + + LOG(WARNING) << "No varying parameter blocks to optimize; " + << "bailing early."; + return transformed_program.release(); + } + + // If the user supplied an linear_solver_ordering with just one + // group, it is equivalent to the user supplying NULL as + // ordering. Ceres is completely free to choose the parameter block + // ordering as it sees fit. For Schur type solvers, this means that + // the user wishes for Ceres to identify the e_blocks, which we do + // by computing a maximal independent set. + if (original_num_groups == 1 && IsSchurType(options->linear_solver_type)) { + vector<ParameterBlock*> schur_ordering; + const int num_eliminate_blocks = ComputeSchurOrdering(*transformed_program, + &schur_ordering); + CHECK_EQ(schur_ordering.size(), transformed_program->NumParameterBlocks()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + for (int i = 0; i < schur_ordering.size(); ++i) { + linear_solver_ordering->AddElementToGroup( + schur_ordering[i]->mutable_user_state(), + (i < num_eliminate_blocks) ? 0 : 1); + } + } + + if (!ApplyUserOrdering(problem_impl->parameter_map(), + linear_solver_ordering, + transformed_program.get(), + error)) { + return NULL; + } + + // If the user requested the use of a Schur type solver, and + // supplied a non-NULL linear_solver_ordering object with more than + // one elimination group, then it can happen that after all the + // parameter blocks which are fixed or unused have been removed from + // the program and the ordering, there are no more parameter blocks + // in the first elimination group. + // + // In such a case, the use of a Schur type solver is not possible, + // as they assume there is at least one e_block. Thus, we + // automatically switch to one of the other solvers, depending on + // the user's indicated preferences. + if (IsSchurType(options->linear_solver_type) && + original_num_groups > 1 && + linear_solver_ordering->GroupSize(min_group_id) == 0) { + string msg = "No e_blocks remaining. Switching from "; + if (options->linear_solver_type == SPARSE_SCHUR) { + options->linear_solver_type = SPARSE_NORMAL_CHOLESKY; + msg += "SPARSE_SCHUR to SPARSE_NORMAL_CHOLESKY."; + } else if (options->linear_solver_type == DENSE_SCHUR) { + // TODO(sameeragarwal): This is probably not a great choice. + // Ideally, we should have a DENSE_NORMAL_CHOLESKY, that can + // take a BlockSparseMatrix as input. + options->linear_solver_type = DENSE_QR; + msg += "DENSE_SCHUR to DENSE_QR."; + } else if (options->linear_solver_type == ITERATIVE_SCHUR) { + msg += StringPrintf("ITERATIVE_SCHUR with %s preconditioner " + "to CGNR with JACOBI preconditioner.", + PreconditionerTypeToString( + options->preconditioner_type)); + options->linear_solver_type = CGNR; + if (options->preconditioner_type != IDENTITY) { + // CGNR currently only supports the JACOBI preconditioner. + options->preconditioner_type = JACOBI; + } + } + + LOG(WARNING) << msg; + } + + // Since the transformed program is the "active" program, and it is mutated, + // update the parameter offsets and indices. + transformed_program->SetParameterOffsetsAndIndex(); + return transformed_program.release(); +} + +LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, + string* error) { + CHECK_NOTNULL(options); + CHECK_NOTNULL(options->linear_solver_ordering); + CHECK_NOTNULL(error); + + if (options->trust_region_strategy_type == DOGLEG) { + if (options->linear_solver_type == ITERATIVE_SCHUR || + options->linear_solver_type == CGNR) { + *error = "DOGLEG only supports exact factorization based linear " + "solvers. If you want to use an iterative solver please " + "use LEVENBERG_MARQUARDT as the trust_region_strategy_type"; + return NULL; + } + } + +#ifdef CERES_NO_SUITESPARSE + if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY && + options->sparse_linear_algebra_library == SUITE_SPARSE) { + *error = "Can't use SPARSE_NORMAL_CHOLESKY with SUITESPARSE because " + "SuiteSparse was not enabled when Ceres was built."; + return NULL; + } + + if (options->preconditioner_type == SCHUR_JACOBI) { + *error = "SCHUR_JACOBI preconditioner not suppored. Please build Ceres " + "with SuiteSparse support."; + return NULL; + } + + if (options->preconditioner_type == CLUSTER_JACOBI) { + *error = "CLUSTER_JACOBI preconditioner not suppored. Please build Ceres " + "with SuiteSparse support."; + return NULL; + } + + if (options->preconditioner_type == CLUSTER_TRIDIAGONAL) { + *error = "CLUSTER_TRIDIAGONAL preconditioner not suppored. Please build " + "Ceres with SuiteSparse support."; + return NULL; + } +#endif + +#ifdef CERES_NO_CXSPARSE + if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY && + options->sparse_linear_algebra_library == CX_SPARSE) { + *error = "Can't use SPARSE_NORMAL_CHOLESKY with CXSPARSE because " + "CXSparse was not enabled when Ceres was built."; + return NULL; + } +#endif + +#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) + if (options->linear_solver_type == SPARSE_SCHUR) { + *error = "Can't use SPARSE_SCHUR because neither SuiteSparse nor" + "CXSparse was enabled when Ceres was compiled."; + return NULL; + } +#endif + + if (options->linear_solver_max_num_iterations <= 0) { + *error = "Solver::Options::linear_solver_max_num_iterations is 0."; + return NULL; + } + if (options->linear_solver_min_num_iterations <= 0) { + *error = "Solver::Options::linear_solver_min_num_iterations is 0."; + return NULL; + } + if (options->linear_solver_min_num_iterations > + options->linear_solver_max_num_iterations) { + *error = "Solver::Options::linear_solver_min_num_iterations > " + "Solver::Options::linear_solver_max_num_iterations."; + return NULL; + } + + LinearSolver::Options linear_solver_options; + linear_solver_options.min_num_iterations = + options->linear_solver_min_num_iterations; + linear_solver_options.max_num_iterations = + options->linear_solver_max_num_iterations; + linear_solver_options.type = options->linear_solver_type; + linear_solver_options.preconditioner_type = options->preconditioner_type; + linear_solver_options.sparse_linear_algebra_library = + options->sparse_linear_algebra_library; + + linear_solver_options.num_threads = options->num_linear_solver_threads; + // The matrix used for storing the dense Schur complement has a + // single lock guarding the whole matrix. Running the + // SchurComplementSolver with multiple threads leads to maximum + // contention and slowdown. If the problem is large enough to + // benefit from a multithreaded schur eliminator, you should be + // using a SPARSE_SCHUR solver anyways. + if ((linear_solver_options.num_threads > 1) && + (linear_solver_options.type == DENSE_SCHUR)) { + LOG(WARNING) << "Warning: Solver::Options::num_linear_solver_threads = " + << options->num_linear_solver_threads + << " with DENSE_SCHUR will result in poor performance; " + << "switching to single-threaded."; + linear_solver_options.num_threads = 1; + } + options->num_linear_solver_threads = linear_solver_options.num_threads; + + linear_solver_options.use_block_amd = options->use_block_amd; + const map<int, set<double*> >& groups = + options->linear_solver_ordering->group_to_elements(); + for (map<int, set<double*> >::const_iterator it = groups.begin(); + it != groups.end(); + ++it) { + linear_solver_options.elimination_groups.push_back(it->second.size()); + } + // Schur type solvers, expect at least two elimination groups. If + // there is only one elimination group, then CreateReducedProgram + // guarantees that this group only contains e_blocks. Thus we add a + // dummy elimination group with zero blocks in it. + if (IsSchurType(linear_solver_options.type) && + linear_solver_options.elimination_groups.size() == 1) { + linear_solver_options.elimination_groups.push_back(0); + } + + return LinearSolver::Create(linear_solver_options); +} + +bool SolverImpl::ApplyUserOrdering(const ProblemImpl::ParameterMap& parameter_map, + const ParameterBlockOrdering* ordering, + Program* program, + string* error) { + if (ordering->NumElements() != program->NumParameterBlocks()) { + *error = StringPrintf("User specified ordering does not have the same " + "number of parameters as the problem. The problem" + "has %d blocks while the ordering has %d blocks.", + program->NumParameterBlocks(), + ordering->NumElements()); + return false; + } + + vector<ParameterBlock*>* parameter_blocks = + program->mutable_parameter_blocks(); + parameter_blocks->clear(); + + const map<int, set<double*> >& groups = + ordering->group_to_elements(); + + for (map<int, set<double*> >::const_iterator group_it = groups.begin(); + group_it != groups.end(); + ++group_it) { + const set<double*>& group = group_it->second; + for (set<double*>::const_iterator parameter_block_ptr_it = group.begin(); + parameter_block_ptr_it != group.end(); + ++parameter_block_ptr_it) { + ProblemImpl::ParameterMap::const_iterator parameter_block_it = + parameter_map.find(*parameter_block_ptr_it); + if (parameter_block_it == parameter_map.end()) { + *error = StringPrintf("User specified ordering contains a pointer " + "to a double that is not a parameter block in the " + "problem. The invalid double is in group: %d", + group_it->first); + return false; + } + parameter_blocks->push_back(parameter_block_it->second); + } + } + return true; +} + +// Find the minimum index of any parameter block to the given residual. +// Parameter blocks that have indices greater than num_eliminate_blocks are +// considered to have an index equal to num_eliminate_blocks. +int MinParameterBlock(const ResidualBlock* residual_block, + int num_eliminate_blocks) { + int min_parameter_block_position = num_eliminate_blocks; + for (int i = 0; i < residual_block->NumParameterBlocks(); ++i) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[i]; + if (!parameter_block->IsConstant()) { + CHECK_NE(parameter_block->index(), -1) + << "Did you forget to call Program::SetParameterOffsetsAndIndex()? " + << "This is a Ceres bug; please contact the developers!"; + min_parameter_block_position = std::min(parameter_block->index(), + min_parameter_block_position); + } + } + return min_parameter_block_position; +} + +// Reorder the residuals for program, if necessary, so that the residuals +// involving each E block occur together. This is a necessary condition for the +// Schur eliminator, which works on these "row blocks" in the jacobian. +bool SolverImpl::LexicographicallyOrderResidualBlocks(const int num_eliminate_blocks, + Program* program, + string* error) { + CHECK_GE(num_eliminate_blocks, 1) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Create a histogram of the number of residuals for each E block. There is an + // extra bucket at the end to catch all non-eliminated F blocks. + vector<int> residual_blocks_per_e_block(num_eliminate_blocks + 1); + vector<ResidualBlock*>* residual_blocks = program->mutable_residual_blocks(); + vector<int> min_position_per_residual(residual_blocks->size()); + for (int i = 0; i < residual_blocks->size(); ++i) { + ResidualBlock* residual_block = (*residual_blocks)[i]; + int position = MinParameterBlock(residual_block, num_eliminate_blocks); + min_position_per_residual[i] = position; + DCHECK_LE(position, num_eliminate_blocks); + residual_blocks_per_e_block[position]++; + } + + // Run a cumulative sum on the histogram, to obtain offsets to the start of + // each histogram bucket (where each bucket is for the residuals for that + // E-block). + vector<int> offsets(num_eliminate_blocks + 1); + std::partial_sum(residual_blocks_per_e_block.begin(), + residual_blocks_per_e_block.end(), + offsets.begin()); + CHECK_EQ(offsets.back(), residual_blocks->size()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + CHECK(find(residual_blocks_per_e_block.begin(), + residual_blocks_per_e_block.end() - 1, 0) != + residual_blocks_per_e_block.end()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Fill in each bucket with the residual blocks for its corresponding E block. + // Each bucket is individually filled from the back of the bucket to the front + // of the bucket. The filling order among the buckets is dictated by the + // residual blocks. This loop uses the offsets as counters; subtracting one + // from each offset as a residual block is placed in the bucket. When the + // filling is finished, the offset pointerts should have shifted down one + // entry (this is verified below). + vector<ResidualBlock*> reordered_residual_blocks( + (*residual_blocks).size(), static_cast<ResidualBlock*>(NULL)); + for (int i = 0; i < residual_blocks->size(); ++i) { + int bucket = min_position_per_residual[i]; + + // Decrement the cursor, which should now point at the next empty position. + offsets[bucket]--; + + // Sanity. + CHECK(reordered_residual_blocks[offsets[bucket]] == NULL) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + reordered_residual_blocks[offsets[bucket]] = (*residual_blocks)[i]; + } + + // Sanity check #1: The difference in bucket offsets should match the + // histogram sizes. + for (int i = 0; i < num_eliminate_blocks; ++i) { + CHECK_EQ(residual_blocks_per_e_block[i], offsets[i + 1] - offsets[i]) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + } + // Sanity check #2: No NULL's left behind. + for (int i = 0; i < reordered_residual_blocks.size(); ++i) { + CHECK(reordered_residual_blocks[i] != NULL) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + } + + // Now that the residuals are collected by E block, swap them in place. + swap(*program->mutable_residual_blocks(), reordered_residual_blocks); + return true; +} + +Evaluator* SolverImpl::CreateEvaluator(const Solver::Options& options, + const ProblemImpl::ParameterMap& parameter_map, + Program* program, + string* error) { + Evaluator::Options evaluator_options; + evaluator_options.linear_solver_type = options.linear_solver_type; + evaluator_options.num_eliminate_blocks = + (options.linear_solver_ordering->NumGroups() > 0 && + IsSchurType(options.linear_solver_type)) + ? (options.linear_solver_ordering + ->group_to_elements().begin() + ->second.size()) + : 0; + evaluator_options.num_threads = options.num_threads; + return Evaluator::Create(evaluator_options, program, error); +} + +CoordinateDescentMinimizer* SolverImpl::CreateInnerIterationMinimizer( + const Solver::Options& options, + const Program& program, + const ProblemImpl::ParameterMap& parameter_map, + string* error) { + scoped_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer( + new CoordinateDescentMinimizer); + scoped_ptr<ParameterBlockOrdering> inner_iteration_ordering; + ParameterBlockOrdering* ordering_ptr = NULL; + + if (options.inner_iteration_ordering == NULL) { + // Find a recursive decomposition of the Hessian matrix as a set + // of independent sets of decreasing size and invert it. This + // seems to work better in practice, i.e., Cameras before + // points. + inner_iteration_ordering.reset(new ParameterBlockOrdering); + ComputeRecursiveIndependentSetOrdering(program, + inner_iteration_ordering.get()); + inner_iteration_ordering->Reverse(); + ordering_ptr = inner_iteration_ordering.get(); + } else { + const map<int, set<double*> >& group_to_elements = + options.inner_iteration_ordering->group_to_elements(); + + // Iterate over each group and verify that it is an independent + // set. + map<int, set<double*> >::const_iterator it = group_to_elements.begin(); + for ( ;it != group_to_elements.end(); ++it) { + if (!IsParameterBlockSetIndependent(it->second, + program.residual_blocks())) { + *error = + StringPrintf("The user-provided " + "parameter_blocks_for_inner_iterations does not " + "form an independent set. Group Id: %d", it->first); + return NULL; + } + } + ordering_ptr = options.inner_iteration_ordering; + } + + if (!inner_iteration_minimizer->Init(program, + parameter_map, + *ordering_ptr, + error)) { + return NULL; + } + + return inner_iteration_minimizer.release(); +} + +} // namespace internal +} // namespace ceres |