diff options
Diffstat (limited to 'internal/ceres/solver_impl.cc')
| -rw-r--r-- | internal/ceres/solver_impl.cc | 1187 |
1 files changed, 175 insertions, 1012 deletions
diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc index 83faa05..a1cf4ca 100644 --- a/internal/ceres/solver_impl.cc +++ b/internal/ceres/solver_impl.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// Copyright 2014 Google Inc. All rights reserved. // http://code.google.com/p/ceres-solver/ // // Redistribution and use in source and binary forms, with or without @@ -34,6 +34,8 @@ #include <iostream> // NOLINT #include <numeric> #include <string> +#include "ceres/array_utils.h" +#include "ceres/callbacks.h" #include "ceres/coordinate_descent_minimizer.h" #include "ceres/cxsparse.h" #include "ceres/evaluator.h" @@ -47,168 +49,20 @@ #include "ceres/ordered_groups.h" #include "ceres/parameter_block.h" #include "ceres/parameter_block_ordering.h" +#include "ceres/preconditioner.h" #include "ceres/problem.h" #include "ceres/problem_impl.h" #include "ceres/program.h" +#include "ceres/reorder_program.h" #include "ceres/residual_block.h" #include "ceres/stringprintf.h" #include "ceres/suitesparse.h" +#include "ceres/summary_utils.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_; -}; - -void SetSummaryFinalCost(Solver::Summary* summary) { - summary->final_cost = summary->initial_cost; - // We need the loop here, instead of just looking at the last - // iteration because the minimizer maybe making non-monotonic steps. - for (int i = 0; i < summary->iterations.size(); ++i) { - const IterationSummary& iteration_summary = summary->iterations[i]; - summary->final_cost = min(iteration_summary.cost, summary->final_cost); - } -} - -// Callback for logging the state of the minimizer to STDERR or STDOUT -// depending on the user's preferences and logging level. -class TrustRegionLoggingCallback : public IterationCallback { - public: - explicit TrustRegionLoggingCallback(bool log_to_stdout) - : log_to_stdout_(log_to_stdout) {} - - ~TrustRegionLoggingCallback() {} - - 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_; -}; - -// Callback for logging the state of the minimizer to STDERR or STDOUT -// depending on the user's preferences and logging level. -class LineSearchLoggingCallback : public IterationCallback { - public: - explicit LineSearchLoggingCallback(bool log_to_stdout) - : log_to_stdout_(log_to_stdout) {} - - ~LineSearchLoggingCallback() {} - - CallbackReturnType operator()(const IterationSummary& summary) { - const char* kReportRowFormat = - "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e " - "s:% 3.2e e:% 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.step_size, - summary.line_search_function_evaluations, - 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_; -}; - -// Iterate over each of the groups in order of their priority and fill -// summary with their sizes. -void SummarizeOrdering(ParameterBlockOrdering* ordering, - vector<int>* summary) { - CHECK_NOTNULL(summary)->clear(); - if (ordering == NULL) { - return; - } - - const map<int, set<double*> >& group_to_elements = - ordering->group_to_elements(); - for (map<int, set<double*> >::const_iterator it = group_to_elements.begin(); - it != group_to_elements.end(); - ++it) { - summary->push_back(it->second.size()); - } -} - -} // namespace void SolverImpl::TrustRegionMinimize( const Solver::Options& options, @@ -216,27 +70,26 @@ void SolverImpl::TrustRegionMinimize( CoordinateDescentMinimizer* inner_iteration_minimizer, Evaluator* evaluator, LinearSolver* linear_solver, - double* parameters, Solver::Summary* summary) { Minimizer::Options minimizer_options(options); + minimizer_options.is_constrained = program->IsBoundsConstrained(); - // 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()); - } + // The optimizer works on contiguous parameter vectors; allocate + // some. + Vector parameters(program->NumParameters()); + + // Collect the discontiguous parameters into a contiguous state + // vector. + program->ParameterBlocksToStateVector(parameters.data()); - TrustRegionLoggingCallback logging_callback( - options.minimizer_progress_to_stdout); + LoggingCallback logging_callback(TRUST_REGION, + 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); + StateUpdatingCallback updating_callback(program, parameters.data()); 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. @@ -266,37 +119,42 @@ void SolverImpl::TrustRegionMinimize( TrustRegionMinimizer minimizer; double minimizer_start_time = WallTimeInSeconds(); - minimizer.Minimize(minimizer_options, parameters, summary); + minimizer.Minimize(minimizer_options, parameters.data(), summary); + + // If the user aborted mid-optimization or the optimization + // terminated because of a numerical failure, then do not update + // user state. + if (summary->termination_type != USER_FAILURE && + summary->termination_type != FAILURE) { + program->StateVectorToParameterBlocks(parameters.data()); + program->CopyParameterBlockStateToUserState(); + } + summary->minimizer_time_in_seconds = WallTimeInSeconds() - minimizer_start_time; } -#ifndef CERES_NO_LINE_SEARCH_MINIMIZER void SolverImpl::LineSearchMinimize( const Solver::Options& options, Program* program, Evaluator* evaluator, - 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()); - } + // The optimizer works on contiguous parameter vectors; allocate some. + Vector parameters(program->NumParameters()); + + // Collect the discontiguous parameters into a contiguous state vector. + program->ParameterBlocksToStateVector(parameters.data()); - LineSearchLoggingCallback logging_callback( - options.minimizer_progress_to_stdout); + LoggingCallback logging_callback(LINE_SEARCH, + 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); + StateUpdatingCallback updating_callback(program, parameters.data()); 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. @@ -308,11 +166,20 @@ void SolverImpl::LineSearchMinimize( LineSearchMinimizer minimizer; double minimizer_start_time = WallTimeInSeconds(); - minimizer.Minimize(minimizer_options, parameters, summary); + minimizer.Minimize(minimizer_options, parameters.data(), summary); + + // If the user aborted mid-optimization or the optimization + // terminated because of a numerical failure, then do not update + // user state. + if (summary->termination_type != USER_FAILURE && + summary->termination_type != FAILURE) { + program->StateVectorToParameterBlocks(parameters.data()); + program->CopyParameterBlockStateToUserState(); + } + summary->minimizer_time_in_seconds = WallTimeInSeconds() - minimizer_start_time; } -#endif // CERES_NO_LINE_SEARCH_MINIMIZER void SolverImpl::Solve(const Solver::Options& options, ProblemImpl* problem_impl, @@ -326,15 +193,10 @@ void SolverImpl::Solve(const Solver::Options& options, << " residual blocks, " << problem_impl->NumResiduals() << " residuals."; - if (options.minimizer_type == TRUST_REGION) { TrustRegionSolve(options, problem_impl, summary); } else { -#ifndef CERES_NO_LINE_SEARCH_MINIMIZER LineSearchSolve(options, problem_impl, summary); -#else - LOG(FATAL) << "Ceres Solver was compiled with -DLINE_SEARCH_MINIMIZER=OFF"; -#endif } } @@ -347,39 +209,15 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, 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->minimizer_type = TRUST_REGION; - summary->num_parameter_blocks = problem_impl->NumParameterBlocks(); - summary->num_parameters = problem_impl->NumParameters(); - summary->num_effective_parameters = - original_program->NumEffectiveParameters(); - 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; - } - - SummarizeOrdering(original_options.linear_solver_ordering, - &(summary->linear_solver_ordering_given)); - SummarizeOrdering(original_options.inner_iteration_ordering, - &(summary->inner_iteration_ordering_given)); + SummarizeGivenProgram(*original_program, summary); + OrderingToGroupSizes(original_options.linear_solver_ordering.get(), + &(summary->linear_solver_ordering_given)); + OrderingToGroupSizes(original_options.inner_iteration_ordering.get(), + &(summary->inner_iteration_ordering_given)); Solver::Options options(original_options); - options.linear_solver_ordering = NULL; - options.inner_iteration_ordering = NULL; #ifndef CERES_USE_OPENMP if (options.num_threads > 1) { @@ -404,9 +242,19 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, if (options.trust_region_minimizer_iterations_to_dump.size() > 0 && options.trust_region_problem_dump_format_type != CONSOLE && options.trust_region_problem_dump_directory.empty()) { - summary->error = + summary->message = "Solver::Options::trust_region_problem_dump_directory is empty."; - LOG(ERROR) << summary->error; + LOG(ERROR) << summary->message; + return; + } + + if (!original_program->ParameterBlocksAreFinite(&summary->message)) { + LOG(ERROR) << "Terminating: " << summary->message; + return; + } + + if (!original_program->IsFeasible(&summary->message)) { + LOG(ERROR) << "Terminating: " << summary->message; return; } @@ -433,17 +281,14 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, 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; + if (options.linear_solver_ordering.get() != NULL) { + if (!IsOrderingValid(options, problem_impl, &summary->message)) { + LOG(ERROR) << summary->message; return; } event_logger.AddEvent("CheckOrdering"); - options.linear_solver_ordering = - new ParameterBlockOrdering(*original_options.linear_solver_ordering); - event_logger.AddEvent("CopyOrdering"); } else { - options.linear_solver_ordering = new ParameterBlockOrdering; + options.linear_solver_ordering.reset(new ParameterBlockOrdering); const ProblemImpl::ParameterMap& parameter_map = problem_impl->parameter_map(); for (ProblemImpl::ParameterMap::const_iterator it = parameter_map.begin(); @@ -459,41 +304,35 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, scoped_ptr<Program> reduced_program(CreateReducedProgram(&options, problem_impl, &summary->fixed_cost, - &summary->error)); + &summary->message)); event_logger.AddEvent("CreateReducedProgram"); if (reduced_program == NULL) { return; } - SummarizeOrdering(options.linear_solver_ordering, - &(summary->linear_solver_ordering_used)); - - summary->num_parameter_blocks_reduced = reduced_program->NumParameterBlocks(); - summary->num_parameters_reduced = reduced_program->NumParameters(); - summary->num_effective_parameters_reduced = - reduced_program->NumEffectiveParameters(); - summary->num_residual_blocks_reduced = reduced_program->NumResidualBlocks(); - summary->num_residuals_reduced = reduced_program->NumResiduals(); + OrderingToGroupSizes(options.linear_solver_ordering.get(), + &(summary->linear_solver_ordering_used)); + SummarizeReducedProgram(*reduced_program, summary); if (summary->num_parameter_blocks_reduced == 0) { summary->preprocessor_time_in_seconds = WallTimeInSeconds() - solver_start_time; double post_process_start_time = WallTimeInSeconds(); - LOG(INFO) << "Terminating: FUNCTION_TOLERANCE reached. " - << "No non-constant parameter blocks found."; + + summary->message = + "Function tolerance reached. " + "No non-constant parameter blocks found."; + summary->termination_type = CONVERGENCE; + VLOG_IF(1, options.logging_type != SILENT) << summary->message; summary->initial_cost = summary->fixed_cost; summary->final_cost = summary->fixed_cost; - // 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; - // Ensure the program state is set to the user parameters on the way out. original_program->SetParameterBlockStatePtrsToUserStatePtrs(); + original_program->SetParameterOffsetsAndIndex(); summary->postprocessor_time_in_seconds = WallTimeInSeconds() - post_process_start_time; @@ -501,7 +340,7 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, } scoped_ptr<LinearSolver> - linear_solver(CreateLinearSolver(&options, &summary->error)); + linear_solver(CreateLinearSolver(&options, &summary->message)); event_logger.AddEvent("CreateLinearSolver"); if (linear_solver == NULL) { return; @@ -511,6 +350,7 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, summary->linear_solver_type_used = options.linear_solver_type; summary->preconditioner_type = options.preconditioner_type; + summary->visibility_clustering_type = options.visibility_clustering_type; summary->num_linear_solver_threads_given = original_options.num_linear_solver_threads; @@ -527,7 +367,7 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, scoped_ptr<Evaluator> evaluator(CreateEvaluator(options, problem_impl->parameter_map(), reduced_program.get(), - &summary->error)); + &summary->message)); event_logger.AddEvent("CreateEvaluator"); @@ -542,26 +382,18 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, << "Disabling inner iterations."; } else { inner_iteration_minimizer.reset( - CreateInnerIterationMinimizer(original_options, + CreateInnerIterationMinimizer(options, *reduced_program, problem_impl->parameter_map(), summary)); if (inner_iteration_minimizer == NULL) { - LOG(ERROR) << summary->error; + LOG(ERROR) << summary->message; return; } } } event_logger.AddEvent("CreateInnerIterationMinimizer"); - // 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; @@ -572,30 +404,17 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, inner_iteration_minimizer.get(), evaluator.get(), linear_solver.get(), - parameters.data(), summary); event_logger.AddEvent("Minimize"); - SetSummaryFinalCost(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(); + SetSummaryFinalCost(summary); // Ensure the program state is set to the user parameters on the way // out. original_program->SetParameterBlockStatePtrsToUserStatePtrs(); + original_program->SetParameterOffsetsAndIndex(); const map<string, double>& linear_solver_time_statistics = linear_solver->TimeStatistics(); @@ -618,8 +437,6 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, event_logger.AddEvent("PostProcess"); } - -#ifndef CERES_NO_LINE_SEARCH_MINIMIZER void SolverImpl::LineSearchSolve(const Solver::Options& original_options, ProblemImpl* original_problem_impl, Solver::Summary* summary) { @@ -628,9 +445,7 @@ void SolverImpl::LineSearchSolve(const Solver::Options& original_options, 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(); - + SummarizeGivenProgram(*original_program, summary); summary->minimizer_type = LINE_SEARCH; summary->line_search_direction_type = original_options.line_search_direction_type; @@ -641,104 +456,9 @@ void SolverImpl::LineSearchSolve(const Solver::Options& original_options, summary->nonlinear_conjugate_gradient_type = original_options.nonlinear_conjugate_gradient_type; - summary->num_parameter_blocks = original_program->NumParameterBlocks(); - summary->num_parameters = original_program->NumParameters(); - summary->num_residual_blocks = original_program->NumResidualBlocks(); - summary->num_residuals = original_program->NumResiduals(); - summary->num_effective_parameters = - original_program->NumEffectiveParameters(); - - // Validate values for configuration parameters supplied by user. - if ((original_options.line_search_direction_type == ceres::BFGS || - original_options.line_search_direction_type == ceres::LBFGS) && - original_options.line_search_type != ceres::WOLFE) { - summary->error = - string("Invalid configuration: require line_search_type == " - "ceres::WOLFE when using (L)BFGS to ensure that underlying " - "assumptions are guaranteed to be satisfied."); - LOG(ERROR) << summary->error; - return; - } - if (original_options.max_lbfgs_rank <= 0) { - summary->error = - string("Invalid configuration: require max_lbfgs_rank > 0"); - LOG(ERROR) << summary->error; - return; - } - if (original_options.min_line_search_step_size <= 0.0) { - summary->error = "Invalid configuration: min_line_search_step_size <= 0.0."; - LOG(ERROR) << summary->error; - return; - } - if (original_options.line_search_sufficient_function_decrease <= 0.0) { - summary->error = - string("Invalid configuration: require ") + - string("line_search_sufficient_function_decrease <= 0.0."); - LOG(ERROR) << summary->error; - return; - } - if (original_options.max_line_search_step_contraction <= 0.0 || - original_options.max_line_search_step_contraction >= 1.0) { - summary->error = string("Invalid configuration: require ") + - string("0.0 < max_line_search_step_contraction < 1.0."); - LOG(ERROR) << summary->error; - return; - } - if (original_options.min_line_search_step_contraction <= - original_options.max_line_search_step_contraction || - original_options.min_line_search_step_contraction > 1.0) { - summary->error = string("Invalid configuration: require ") + - string("max_line_search_step_contraction < ") + - string("min_line_search_step_contraction <= 1.0."); - LOG(ERROR) << summary->error; - return; - } - // Warn user if they have requested BISECTION interpolation, but constraints - // on max/min step size change during line search prevent bisection scaling - // from occurring. Warn only, as this is likely a user mistake, but one which - // does not prevent us from continuing. - LOG_IF(WARNING, - (original_options.line_search_interpolation_type == ceres::BISECTION && - (original_options.max_line_search_step_contraction > 0.5 || - original_options.min_line_search_step_contraction < 0.5))) - << "Line search interpolation type is BISECTION, but specified " - << "max_line_search_step_contraction: " - << original_options.max_line_search_step_contraction << ", and " - << "min_line_search_step_contraction: " - << original_options.min_line_search_step_contraction - << ", prevent bisection (0.5) scaling, continuing with solve regardless."; - if (original_options.max_num_line_search_step_size_iterations <= 0) { - summary->error = string("Invalid configuration: require ") + - string("max_num_line_search_step_size_iterations > 0."); - LOG(ERROR) << summary->error; - return; - } - if (original_options.line_search_sufficient_curvature_decrease <= - original_options.line_search_sufficient_function_decrease || - original_options.line_search_sufficient_curvature_decrease > 1.0) { - summary->error = string("Invalid configuration: require ") + - string("line_search_sufficient_function_decrease < ") + - string("line_search_sufficient_curvature_decrease < 1.0."); - LOG(ERROR) << summary->error; - return; - } - if (original_options.max_line_search_step_expansion <= 1.0) { - summary->error = string("Invalid configuration: require ") + - string("max_line_search_step_expansion > 1.0."); - LOG(ERROR) << summary->error; - return; - } - - // 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; + if (original_program->IsBoundsConstrained()) { + summary->message = "LINE_SEARCH Minimizer does not support bounds."; + LOG(ERROR) << "Terminating: " << summary->message; return; } @@ -750,8 +470,6 @@ void SolverImpl::LineSearchSolve(const Solver::Options& original_options, // line search. options.linear_solver_type = CGNR; - options.linear_solver_ordering = NULL; - options.inner_iteration_ordering = NULL; #ifndef CERES_USE_OPENMP if (options.num_threads > 1) { @@ -766,15 +484,18 @@ void SolverImpl::LineSearchSolve(const Solver::Options& original_options, summary->num_threads_given = original_options.num_threads; summary->num_threads_used = options.num_threads; - if (original_options.linear_solver_ordering != NULL) { - if (!IsOrderingValid(original_options, problem_impl, &summary->error)) { - LOG(ERROR) << summary->error; + if (!original_program->ParameterBlocksAreFinite(&summary->message)) { + LOG(ERROR) << "Terminating: " << summary->message; + return; + } + + if (options.linear_solver_ordering.get() != NULL) { + if (!IsOrderingValid(options, problem_impl, &summary->message)) { + LOG(ERROR) << summary->message; return; } - options.linear_solver_ordering = - new ParameterBlockOrdering(*original_options.linear_solver_ordering); } else { - options.linear_solver_ordering = new ParameterBlockOrdering; + options.linear_solver_ordering.reset(new ParameterBlockOrdering); const ProblemImpl::ParameterMap& parameter_map = problem_impl->parameter_map(); for (ProblemImpl::ParameterMap::const_iterator it = parameter_map.begin(); @@ -784,6 +505,7 @@ void SolverImpl::LineSearchSolve(const Solver::Options& original_options, } } + original_program->SetParameterBlockStatePtrsToUserStatePtrs(); // If the user requests gradient checking, construct a new @@ -809,36 +531,31 @@ void SolverImpl::LineSearchSolve(const Solver::Options& original_options, scoped_ptr<Program> reduced_program(CreateReducedProgram(&options, problem_impl, &summary->fixed_cost, - &summary->error)); + &summary->message)); 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_effective_parameters_reduced = - reduced_program->NumEffectiveParameters(); - summary->num_residuals_reduced = reduced_program->NumResiduals(); - + SummarizeReducedProgram(*reduced_program, summary); 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; + summary->message = + "Function tolerance reached. " + "No non-constant parameter blocks found."; + summary->termination_type = CONVERGENCE; + VLOG_IF(1, options.logging_type != SILENT) << summary->message; + summary->initial_cost = summary->fixed_cost; + summary->final_cost = summary->fixed_cost; const double post_process_start_time = WallTimeInSeconds(); - SetSummaryFinalCost(summary); // Ensure the program state is set to the user parameters on the way out. original_program->SetParameterBlockStatePtrsToUserStatePtrs(); + original_program->SetParameterOffsetsAndIndex(); + summary->postprocessor_time_in_seconds = WallTimeInSeconds() - post_process_start_time; return; @@ -847,48 +564,25 @@ void SolverImpl::LineSearchSolve(const Solver::Options& original_options, scoped_ptr<Evaluator> evaluator(CreateEvaluator(options, problem_impl->parameter_map(), reduced_program.get(), - &summary->error)); + &summary->message)); if (evaluator == NULL) { 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; - const double minimizer_start_time = WallTimeInSeconds(); summary->preprocessor_time_in_seconds = minimizer_start_time - solver_start_time; // Run the optimization. - LineSearchMinimize(options, - reduced_program.get(), - evaluator.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; - } + LineSearchMinimize(options, reduced_program.get(), evaluator.get(), summary); const 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(); - SetSummaryFinalCost(summary); // Ensure the program state is set to the user parameters on the way out. original_program->SetParameterBlockStatePtrsToUserStatePtrs(); + original_program->SetParameterOffsetsAndIndex(); const map<string, double>& evaluator_time_statistics = evaluator->TimeStatistics(); @@ -902,7 +596,6 @@ void SolverImpl::LineSearchSolve(const Solver::Options& original_options, summary->postprocessor_time_in_seconds = WallTimeInSeconds() - post_process_start_time; } -#endif // CERES_NO_LINE_SEARCH_MINIMIZER bool SolverImpl::IsOrderingValid(const Solver::Options& options, const ProblemImpl* problem_impl, @@ -966,133 +659,48 @@ bool SolverImpl::IsParameterBlockSetIndependent( 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(true, - &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); - } - - if (!(((program->NumResidualBlocks() == 0) && - (program->NumParameterBlocks() == 0)) || - ((program->NumResidualBlocks() != 0) && - (program->NumParameterBlocks() != 0)))) { - *error = "Congratulations, you found a bug in Ceres. Please report it."; - return false; - } - - return true; -} - Program* SolverImpl::CreateReducedProgram(Solver::Options* options, ProblemImpl* problem_impl, double* fixed_cost, string* error) { - CHECK_NOTNULL(options->linear_solver_ordering); + CHECK_NOTNULL(options->linear_solver_ordering.get()); 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; - if (!RemoveFixedBlocksFromProgram(transformed_program.get(), - linear_solver_ordering, - fixed_cost, - error)) { + vector<double*> removed_parameter_blocks; + scoped_ptr<Program> reduced_program( + original_program->CreateReducedProgram(&removed_parameter_blocks, + fixed_cost, + error)); + if (reduced_program.get() == NULL) { return NULL; } VLOG(2) << "Reduced problem: " - << transformed_program->NumParameterBlocks() + << reduced_program->NumParameterBlocks() << " parameter blocks, " - << transformed_program->NumParameters() + << reduced_program->NumParameters() << " parameters, " - << transformed_program->NumResidualBlocks() + << reduced_program->NumResidualBlocks() << " residual blocks, " - << transformed_program->NumResiduals() + << reduced_program->NumResiduals() << " residuals."; - if (transformed_program->NumParameterBlocks() == 0) { + if (reduced_program->NumParameterBlocks() == 0) { LOG(WARNING) << "No varying parameter blocks to optimize; " << "bailing early."; - return transformed_program.release(); + return reduced_program.release(); + } + + ParameterBlockOrdering* linear_solver_ordering = + options->linear_solver_ordering.get(); + const int min_group_id = + linear_solver_ordering->MinNonZeroGroup(); + linear_solver_ordering->Remove(removed_parameter_blocks); + + ParameterBlockOrdering* inner_iteration_ordering = + options->inner_iteration_ordering.get(); + if (inner_iteration_ordering != NULL) { + inner_iteration_ordering->Remove(removed_parameter_blocks); } if (IsSchurType(options->linear_solver_type) && @@ -1108,7 +716,15 @@ Program* SolverImpl::CreateReducedProgram(Solver::Options* options, // as they assume there is at least one e_block. Thus, we // automatically switch to the closest solver to the one indicated // by the user. - AlternateLinearSolverForSchurTypeLinearSolver(options); + if (options->linear_solver_type == ITERATIVE_SCHUR) { + options->preconditioner_type = + Preconditioner::PreconditionerForZeroEBlocks( + options->preconditioner_type); + } + + options->linear_solver_type = + LinearSolver::LinearSolverForZeroEBlocks( + options->linear_solver_type); } if (IsSchurType(options->linear_solver_type)) { @@ -1117,33 +733,34 @@ Program* SolverImpl::CreateReducedProgram(Solver::Options* options, options->sparse_linear_algebra_library_type, problem_impl->parameter_map(), linear_solver_ordering, - transformed_program.get(), + reduced_program.get(), error)) { return NULL; } - return transformed_program.release(); + return reduced_program.release(); } - if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY) { + if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY && + !options->dynamic_sparsity) { if (!ReorderProgramForSparseNormalCholesky( options->sparse_linear_algebra_library_type, - linear_solver_ordering, - transformed_program.get(), + *linear_solver_ordering, + reduced_program.get(), error)) { return NULL; } - return transformed_program.release(); + return reduced_program.release(); } - transformed_program->SetParameterOffsetsAndIndex(); - return transformed_program.release(); + reduced_program->SetParameterOffsetsAndIndex(); + return reduced_program.release(); } LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, string* error) { CHECK_NOTNULL(options); - CHECK_NOTNULL(options->linear_solver_ordering); + CHECK_NOTNULL(options->linear_solver_ordering.get()); CHECK_NOTNULL(error); if (options->trust_region_strategy_type == DOGLEG) { @@ -1209,14 +826,6 @@ LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, } #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->max_linear_solver_iterations <= 0) { *error = "Solver::Options::max_linear_solver_iterations is not positive."; return NULL; @@ -1239,11 +848,14 @@ LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, options->max_linear_solver_iterations; linear_solver_options.type = options->linear_solver_type; linear_solver_options.preconditioner_type = options->preconditioner_type; + linear_solver_options.visibility_clustering_type = + options->visibility_clustering_type; linear_solver_options.sparse_linear_algebra_library_type = options->sparse_linear_algebra_library_type; linear_solver_options.dense_linear_algebra_library_type = options->dense_linear_algebra_library_type; linear_solver_options.use_postordering = options->use_postordering; + linear_solver_options.dynamic_sparsity = options->dynamic_sparsity; // Ignore user's postordering preferences and force it to be true if // cholmod_camd is not available. This ensures that the linear @@ -1259,13 +871,8 @@ LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, linear_solver_options.num_threads = options->num_linear_solver_threads; options->num_linear_solver_threads = linear_solver_options.num_threads; - 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()); - } + OrderingToGroupSizes(options->linear_solver_ordering.get(), + &linear_solver_options.elimination_groups); // 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 @@ -1278,109 +885,6 @@ LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, return LinearSolver::Create(linear_solver_options); } - -// 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. -static 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, @@ -1396,6 +900,7 @@ Evaluator* SolverImpl::CreateEvaluator( ->second.size()) : 0; evaluator_options.num_threads = options.num_threads; + evaluator_options.dynamic_sparsity = options.dynamic_sparsity; return Evaluator::Create(evaluator_options, program, error); } @@ -1411,374 +916,32 @@ CoordinateDescentMinimizer* SolverImpl::CreateInnerIterationMinimizer( 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(); + if (options.inner_iteration_ordering.get() == NULL) { + inner_iteration_ordering.reset( + CoordinateDescentMinimizer::CreateOrdering(program)); 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())) { - summary->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.get(); + if (!CoordinateDescentMinimizer::IsOrderingValid(program, + *ordering_ptr, + &summary->message)) { + return NULL; } - ordering_ptr = options.inner_iteration_ordering; } if (!inner_iteration_minimizer->Init(program, parameter_map, *ordering_ptr, - &summary->error)) { + &summary->message)) { return NULL; } summary->inner_iterations_used = true; summary->inner_iteration_time_in_seconds = 0.0; - SummarizeOrdering(ordering_ptr, &(summary->inner_iteration_ordering_used)); + OrderingToGroupSizes(ordering_ptr, + &(summary->inner_iteration_ordering_used)); return inner_iteration_minimizer.release(); } -void SolverImpl::AlternateLinearSolverForSchurTypeLinearSolver( - Solver::Options* options) { - if (!IsSchurType(options->linear_solver_type)) { - return; - } - - 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) { - options->linear_solver_type = CGNR; - if (options->preconditioner_type != IDENTITY) { - msg += StringPrintf("ITERATIVE_SCHUR with %s preconditioner " - "to CGNR with JACOBI preconditioner.", - PreconditionerTypeToString( - options->preconditioner_type)); - // CGNR currently only supports the JACOBI preconditioner. - options->preconditioner_type = JACOBI; - } else { - msg += "ITERATIVE_SCHUR with IDENTITY preconditioner" - "to CGNR with IDENTITY preconditioner."; - } - } - LOG(WARNING) << msg; -} - -bool SolverImpl::ApplyUserOrdering( - const ProblemImpl::ParameterMap& parameter_map, - const ParameterBlockOrdering* parameter_block_ordering, - Program* program, - string* error) { - const int num_parameter_blocks = program->NumParameterBlocks(); - if (parameter_block_ordering->NumElements() != num_parameter_blocks) { - *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.", - num_parameter_blocks, - parameter_block_ordering->NumElements()); - return false; - } - - vector<ParameterBlock*>* parameter_blocks = - program->mutable_parameter_blocks(); - parameter_blocks->clear(); - - const map<int, set<double*> >& groups = - parameter_block_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; -} - - -TripletSparseMatrix* SolverImpl::CreateJacobianBlockSparsityTranspose( - const Program* program) { - - // Matrix to store the block sparsity structure of the Jacobian. - TripletSparseMatrix* tsm = - new TripletSparseMatrix(program->NumParameterBlocks(), - program->NumResidualBlocks(), - 10 * program->NumResidualBlocks()); - int num_nonzeros = 0; - int* rows = tsm->mutable_rows(); - int* cols = tsm->mutable_cols(); - double* values = tsm->mutable_values(); - - const vector<ResidualBlock*>& residual_blocks = program->residual_blocks(); - for (int c = 0; c < residual_blocks.size(); ++c) { - const ResidualBlock* residual_block = residual_blocks[c]; - const int num_parameter_blocks = residual_block->NumParameterBlocks(); - ParameterBlock* const* parameter_blocks = - residual_block->parameter_blocks(); - - for (int j = 0; j < num_parameter_blocks; ++j) { - if (parameter_blocks[j]->IsConstant()) { - continue; - } - - // Re-size the matrix if needed. - if (num_nonzeros >= tsm->max_num_nonzeros()) { - tsm->set_num_nonzeros(num_nonzeros); - tsm->Reserve(2 * num_nonzeros); - rows = tsm->mutable_rows(); - cols = tsm->mutable_cols(); - values = tsm->mutable_values(); - } - CHECK_LT(num_nonzeros, tsm->max_num_nonzeros()); - - const int r = parameter_blocks[j]->index(); - rows[num_nonzeros] = r; - cols[num_nonzeros] = c; - values[num_nonzeros] = 1.0; - ++num_nonzeros; - } - } - - tsm->set_num_nonzeros(num_nonzeros); - return tsm; -} - -bool SolverImpl::ReorderProgramForSchurTypeLinearSolver( - const LinearSolverType linear_solver_type, - const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, - const ProblemImpl::ParameterMap& parameter_map, - ParameterBlockOrdering* parameter_block_ordering, - Program* program, - string* error) { - if (parameter_block_ordering->NumGroups() == 1) { - // If the user supplied an parameter_block_ordering with just one - // group, it is equivalent to the user supplying NULL as an - // parameter_block_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. - vector<ParameterBlock*> schur_ordering; - const int num_eliminate_blocks = - ComputeStableSchurOrdering(*program, &schur_ordering); - - CHECK_EQ(schur_ordering.size(), program->NumParameterBlocks()) - << "Congratulations, you found a Ceres bug! Please report this error " - << "to the developers."; - - // Update the parameter_block_ordering object. - for (int i = 0; i < schur_ordering.size(); ++i) { - double* parameter_block = schur_ordering[i]->mutable_user_state(); - const int group_id = (i < num_eliminate_blocks) ? 0 : 1; - parameter_block_ordering->AddElementToGroup(parameter_block, group_id); - } - - // We could call ApplyUserOrdering but this is cheaper and - // simpler. - swap(*program->mutable_parameter_blocks(), schur_ordering); - } else { - // The user provided an ordering with more than one elimination - // group. Trust the user and apply the ordering. - if (!ApplyUserOrdering(parameter_map, - parameter_block_ordering, - program, - error)) { - return false; - } - } - - // Pre-order the columns corresponding to the schur complement if - // possible. -#if !defined(CERES_NO_SUITESPARSE) && !defined(CERES_NO_CAMD) - if (linear_solver_type == SPARSE_SCHUR && - sparse_linear_algebra_library_type == SUITE_SPARSE) { - vector<int> constraints; - vector<ParameterBlock*>& parameter_blocks = - *(program->mutable_parameter_blocks()); - - for (int i = 0; i < parameter_blocks.size(); ++i) { - constraints.push_back( - parameter_block_ordering->GroupId( - parameter_blocks[i]->mutable_user_state())); - } - - // Renumber the entries of constraints to be contiguous integers - // as camd requires that the group ids be in the range [0, - // parameter_blocks.size() - 1]. - SolverImpl::CompactifyArray(&constraints); - - // Set the offsets and index for CreateJacobianSparsityTranspose. - program->SetParameterOffsetsAndIndex(); - // Compute a block sparse presentation of J'. - scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( - SolverImpl::CreateJacobianBlockSparsityTranspose(program)); - - SuiteSparse ss; - cholmod_sparse* block_jacobian_transpose = - ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get()); - - vector<int> ordering(parameter_blocks.size(), 0); - ss.ConstrainedApproximateMinimumDegreeOrdering(block_jacobian_transpose, - &constraints[0], - &ordering[0]); - ss.Free(block_jacobian_transpose); - - const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks); - for (int i = 0; i < program->NumParameterBlocks(); ++i) { - parameter_blocks[i] = parameter_blocks_copy[ordering[i]]; - } - } -#endif - - program->SetParameterOffsetsAndIndex(); - // Schur type solvers also require that their residual blocks be - // lexicographically ordered. - const int num_eliminate_blocks = - parameter_block_ordering->group_to_elements().begin()->second.size(); - return LexicographicallyOrderResidualBlocks(num_eliminate_blocks, - program, - error); -} - -bool SolverImpl::ReorderProgramForSparseNormalCholesky( - const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, - const ParameterBlockOrdering* parameter_block_ordering, - Program* program, - string* error) { - // Set the offsets and index for CreateJacobianSparsityTranspose. - program->SetParameterOffsetsAndIndex(); - // Compute a block sparse presentation of J'. - scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose( - SolverImpl::CreateJacobianBlockSparsityTranspose(program)); - - vector<int> ordering(program->NumParameterBlocks(), 0); - vector<ParameterBlock*>& parameter_blocks = - *(program->mutable_parameter_blocks()); - - if (sparse_linear_algebra_library_type == SUITE_SPARSE) { -#ifdef CERES_NO_SUITESPARSE - *error = "Can't use SPARSE_NORMAL_CHOLESKY with SUITE_SPARSE because " - "SuiteSparse was not enabled when Ceres was built."; - return false; -#else - SuiteSparse ss; - cholmod_sparse* block_jacobian_transpose = - ss.CreateSparseMatrix(tsm_block_jacobian_transpose.get()); - -# ifdef CERES_NO_CAMD - // No cholmod_camd, so ignore user's parameter_block_ordering and - // use plain old AMD. - ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, &ordering[0]); -# else - if (parameter_block_ordering->NumGroups() > 1) { - // If the user specified more than one elimination groups use them - // to constrain the ordering. - vector<int> constraints; - for (int i = 0; i < parameter_blocks.size(); ++i) { - constraints.push_back( - parameter_block_ordering->GroupId( - parameter_blocks[i]->mutable_user_state())); - } - ss.ConstrainedApproximateMinimumDegreeOrdering( - block_jacobian_transpose, - &constraints[0], - &ordering[0]); - } else { - ss.ApproximateMinimumDegreeOrdering(block_jacobian_transpose, - &ordering[0]); - } -# endif // CERES_NO_CAMD - - ss.Free(block_jacobian_transpose); -#endif // CERES_NO_SUITESPARSE - - } else if (sparse_linear_algebra_library_type == CX_SPARSE) { -#ifndef CERES_NO_CXSPARSE - - // CXSparse works with J'J instead of J'. So compute the block - // sparsity for J'J and compute an approximate minimum degree - // ordering. - CXSparse cxsparse; - cs_di* block_jacobian_transpose; - block_jacobian_transpose = - cxsparse.CreateSparseMatrix(tsm_block_jacobian_transpose.get()); - cs_di* block_jacobian = cxsparse.TransposeMatrix(block_jacobian_transpose); - cs_di* block_hessian = - cxsparse.MatrixMatrixMultiply(block_jacobian_transpose, block_jacobian); - cxsparse.Free(block_jacobian); - cxsparse.Free(block_jacobian_transpose); - - cxsparse.ApproximateMinimumDegreeOrdering(block_hessian, &ordering[0]); - cxsparse.Free(block_hessian); -#else // CERES_NO_CXSPARSE - *error = "Can't use SPARSE_NORMAL_CHOLESKY with CX_SPARSE because " - "CXSparse was not enabled when Ceres was built."; - return false; -#endif // CERES_NO_CXSPARSE - } else { - *error = "Unknown sparse linear algebra library."; - return false; - } - - // Apply ordering. - const vector<ParameterBlock*> parameter_blocks_copy(parameter_blocks); - for (int i = 0; i < program->NumParameterBlocks(); ++i) { - parameter_blocks[i] = parameter_blocks_copy[ordering[i]]; - } - - program->SetParameterOffsetsAndIndex(); - return true; -} - -void SolverImpl::CompactifyArray(vector<int>* array_ptr) { - vector<int>& array = *array_ptr; - const set<int> unique_group_ids(array.begin(), array.end()); - map<int, int> group_id_map; - for (set<int>::const_iterator it = unique_group_ids.begin(); - it != unique_group_ids.end(); - ++it) { - InsertOrDie(&group_id_map, *it, group_id_map.size()); - } - - for (int i = 0; i < array.size(); ++i) { - array[i] = group_id_map[array[i]]; - } -} - } // namespace internal } // namespace ceres |