mirror of
https://github.com/ceres-solver/ceres-solver.git
synced 2026-08-29 16:40:38 +08:00
Sameer Agarwal
f4d0164607
1. Add a line search based minimization loop. 2. Currently this loop supports steepest descent and three kinds of non-linear conjugate gradient algorithms. 3. Update SolverImpl to talk to LineSearchMinimizer. 4. Update IterationCallback to carry information about line search. 5. Update LineSearch to take the initial point as input, saving on one function evaluation. 6. Updates to the external API. Change-Id: I901a0e89fc948451ab34c743e70f3dec57c9405e
1441 lines
57 KiB
C++
1441 lines
57 KiB
C++
// 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/line_search_minimizer.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 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_;
|
|
};
|
|
|
|
} // namespace
|
|
|
|
void SolverImpl::TrustRegionMinimize(
|
|
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());
|
|
}
|
|
|
|
TrustRegionLoggingCallback 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::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());
|
|
}
|
|
|
|
LineSearchLoggingCallback 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;
|
|
|
|
LineSearchMinimizer 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& options,
|
|
ProblemImpl* problem_impl,
|
|
Solver::Summary* summary) {
|
|
if (options.minimizer_type == TRUST_REGION) {
|
|
TrustRegionSolve(options, problem_impl, summary);
|
|
} else {
|
|
LineSearchSolve(options, problem_impl, summary);
|
|
}
|
|
}
|
|
|
|
void SolverImpl::TrustRegionSolve(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.
|
|
TrustRegionMinimize(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;
|
|
}
|
|
|
|
void SolverImpl::LineSearchSolve(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);
|
|
|
|
// This ensures that we get a Block Jacobian Evaluator along with
|
|
// none of the Schur nonsense. This file will have to be extensively
|
|
// refactored to deal with the various bits of cleanups related to
|
|
// 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) {
|
|
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;
|
|
}
|
|
#endif
|
|
|
|
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;
|
|
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);
|
|
}
|
|
}
|
|
|
|
// 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();
|
|
}
|
|
|
|
// 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<Evaluator> evaluator(CreateEvaluator(options,
|
|
problem_impl->parameter_map(),
|
|
reduced_program.get(),
|
|
&summary->error));
|
|
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;
|
|
|
|
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;
|
|
}
|
|
|
|
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
|