Google Git
Sign in
ceres-solver / ceres-solver / 6726cd50732157e12e476445c8e3081379e5e2a1 / . / internal / ceres / trust_region_minimizer.cc
blob: 068cc8d2e4f6c20b7e2ce26d7494a4997ff81d2f [file] [log] [blame]
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// 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 ab%ove 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: sameeragarwal@google.com (Sameer Agarwal)
#include "ceres/trust_region_minimizer.h"
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <cstring>
#include <limits>
#include <memory>
#include <string>
#include <vector>
#include "Eigen/Core"
#include "absl/log/check.h"
#include "absl/log/log.h"
#include "absl/strings/str_format.h"
#include "absl/time/clock.h"
#include "absl/time/time.h"
#include "ceres/array_utils.h"
#include "ceres/coordinate_descent_minimizer.h"
#include "ceres/eigen_vector_ops.h"
#include "ceres/evaluator.h"
#include "ceres/file.h"
#include "ceres/line_search.h"
#include "ceres/parallel_for.h"
#include "ceres/types.h"
// Helper macro to simplify some of the control flow.
#define RETURN_IF_ERROR_AND_LOG(expr) \
do { \
if (!(expr)) { \
LOG(ERROR) << "Terminating: " << solver_summary_->message; \
return; \
} \
} while (0)
namespace ceres::internal {
void TrustRegionMinimizer::Minimize(const Minimizer::Options& options,
double* parameters,
Solver::Summary* solver_summary) {
start_time_ = absl::Now();
iteration_start_time_ = start_time_;
Init(options, parameters, solver_summary);
RETURN_IF_ERROR_AND_LOG(IterationZero());
// Create the TrustRegionStepEvaluator. The construction needs to be
// delayed to this point because we need the cost for the starting
// point to initialize the step evaluator.
step_evaluator_ = std::make_unique<TrustRegionStepEvaluator>(
x_cost_,
options_.use_nonmonotonic_steps
? options_.max_consecutive_nonmonotonic_steps
: 0);
bool atleast_one_successful_step = false;
while (FinalizeIterationAndCheckIfMinimizerCanContinue()) {
iteration_start_time_ = absl::Now();
const double previous_gradient_norm = iteration_summary_.gradient_norm;
const double previous_gradient_max_norm =
iteration_summary_.gradient_max_norm;
iteration_summary_ = IterationSummary();
iteration_summary_.iteration =
solver_summary->iterations.back().iteration + 1;
RETURN_IF_ERROR_AND_LOG(ComputeTrustRegionStep());
if (!iteration_summary_.step_is_valid) {
RETURN_IF_ERROR_AND_LOG(HandleInvalidStep());
continue;
}
if (options_.is_constrained &&
options_.max_num_line_search_step_size_iterations > 0) {
// Use a projected line search to enforce the bounds constraints
// and improve the quality of the step.
DoLineSearch(x_, gradient_, x_cost_, &delta_);
}
ComputeCandidatePointAndEvaluateCost();
DoInnerIterationsIfNeeded();
if (atleast_one_successful_step && ParameterToleranceReached()) {
return;
}
if (FunctionToleranceReached()) {
return;
}
if (IsStepSuccessful()) {
atleast_one_successful_step = true;
RETURN_IF_ERROR_AND_LOG(HandleSuccessfulStep());
} else {
// Declare the step unsuccessful and inform the trust region strategy.
iteration_summary_.step_is_successful = false;
iteration_summary_.cost = candidate_cost_ + solver_summary_->fixed_cost;
// When the step is unsuccessful, we do not compute the gradient
// (or update x), so we preserve its value from the last
// successful iteration.
iteration_summary_.gradient_norm = previous_gradient_norm;
iteration_summary_.gradient_max_norm = previous_gradient_max_norm;
strategy_->StepRejected(iteration_summary_.relative_decrease);
}
}
}
// Initialize the minimizer, allocate working space and set some of
// the fields in the solver_summary.
void TrustRegionMinimizer::Init(const Minimizer::Options& options,
double* parameters,
Solver::Summary* solver_summary) {
options_ = options;
std::sort(options_.trust_region_minimizer_iterations_to_dump.begin(),
options_.trust_region_minimizer_iterations_to_dump.end());
parameters_ = parameters;
solver_summary_ = solver_summary;
solver_summary_->termination_type = NO_CONVERGENCE;
solver_summary_->num_successful_steps = 0;
solver_summary_->num_unsuccessful_steps = 0;
solver_summary_->is_constrained = options.is_constrained;
CHECK(options_.evaluator != nullptr);
CHECK(options_.jacobian != nullptr);
CHECK(options_.trust_region_strategy != nullptr);
evaluator_ = options_.evaluator.get();
jacobian_ = options_.jacobian.get();
strategy_ = options_.trust_region_strategy.get();
is_not_silent_ = !options.is_silent;
inner_iterations_are_enabled_ =
options.inner_iteration_minimizer.get() != nullptr;
inner_iterations_were_useful_ = false;
num_parameters_ = evaluator_->NumParameters();
num_effective_parameters_ = evaluator_->NumEffectiveParameters();
num_residuals_ = evaluator_->NumResiduals();
num_consecutive_invalid_steps_ = 0;
x_ = ConstVectorRef(parameters_, num_parameters_);
residuals_.resize(num_residuals_);
trust_region_step_.resize(num_effective_parameters_);
delta_.resize(num_effective_parameters_);
candidate_x_.resize(num_parameters_);
gradient_.resize(num_effective_parameters_);
model_residuals_.resize(num_residuals_);
negative_gradient_.resize(num_effective_parameters_);
projected_gradient_step_.resize(num_parameters_);
// By default scaling is one, if the user requests Jacobi scaling of
// the Jacobian, we will compute and overwrite this vector.
jacobian_scaling_ = Vector::Ones(num_effective_parameters_);
x_cost_ = std::numeric_limits<double>::max();
minimum_cost_ = x_cost_;
model_cost_change_ = 0.0;
}
// 1. Project the initial solution onto the feasible set if needed.
// 2. Compute the initial cost, jacobian & gradient.
//
// Return true if all computations can be performed successfully.
bool TrustRegionMinimizer::IterationZero() {
iteration_summary_ = IterationSummary();
iteration_summary_.iteration = 0;
iteration_summary_.step_is_valid = false;
iteration_summary_.step_is_successful = false;
iteration_summary_.cost_change = 0.0;
iteration_summary_.gradient_max_norm = 0.0;
iteration_summary_.gradient_norm = 0.0;
iteration_summary_.step_norm = 0.0;
iteration_summary_.relative_decrease = 0.0;
iteration_summary_.eta = options_.eta;
iteration_summary_.linear_solver_iterations = 0;
iteration_summary_.step_solver_time_in_seconds = 0;
if (options_.is_constrained) {
delta_.setZero();
if (!evaluator_->Plus(x_.data(), delta_.data(), candidate_x_.data())) {
solver_summary_->message =
"Unable to project initial point onto the feasible set.";
solver_summary_->termination_type = FAILURE;
return false;
}
x_ = candidate_x_;
}
if (!EvaluateGradientAndJacobian(/*new_evaluation_point=*/true)) {
solver_summary_->message =
"Initial residual and Jacobian evaluation failed.";
return false;
}
solver_summary_->initial_cost = x_cost_ + solver_summary_->fixed_cost;
iteration_summary_.step_is_valid = true;
iteration_summary_.step_is_successful = true;
return true;
}
// For the current x_, compute
//
// 1. Cost
// 2. Jacobian
// 3. Gradient
// 4. Scale the Jacobian if needed (and compute the scaling if we are
// in iteration zero).
// 5. Compute the 2 and max norm of the gradient.
//
// Returns true if all computations could be performed
// successfully. Any failures are considered fatal and the
// Solver::Summary is updated to indicate this.
bool TrustRegionMinimizer::EvaluateGradientAndJacobian(
bool new_evaluation_point) {
Evaluator::EvaluateOptions evaluate_options;
evaluate_options.new_evaluation_point = new_evaluation_point;
if (!evaluator_->Evaluate(evaluate_options,
x_.data(),
&x_cost_,
residuals_.data(),
gradient_.data(),
jacobian_)) {
solver_summary_->message = "Residual and Jacobian evaluation failed.";
solver_summary_->termination_type = FAILURE;
return false;
}
iteration_summary_.cost = x_cost_ + solver_summary_->fixed_cost;
if (options_.jacobi_scaling) {
if (iteration_summary_.iteration == 0) {
// Compute a scaling vector that is used to improve the
// conditioning of the Jacobian.
//
// jacobian_scaling_ = diag(J'J)^{-1}
jacobian_->SquaredColumnNorm(jacobian_scaling_.data());
for (int i = 0; i < jacobian_->num_cols(); ++i) {
// Add one to the denominator to prevent division by zero.
jacobian_scaling_[i] = 1.0 / (1.0 + sqrt(jacobian_scaling_[i]));
}
}
// jacobian = jacobian * diag(J'J) ^{-1}
jacobian_->ScaleColumns(
jacobian_scaling_.data(), options_.context, options_.num_threads);
}
// The gradient exists in the local tangent space. To account for
// the bounds constraints correctly, instead of just computing the
// norm of the gradient vector, we compute
//
// |Plus(x, -gradient) - x|
//
// Where the Plus operator lifts the negative gradient to the
// ambient space, adds it to x and projects it on the hypercube
// defined by the bounds.
negative_gradient_ = -gradient_;
if (!evaluator_->Plus(x_.data(),
negative_gradient_.data(),
projected_gradient_step_.data())) {
solver_summary_->message =
"projected_gradient_step = Plus(x, -gradient) failed.";
solver_summary_->termination_type = FAILURE;
return false;
}
iteration_summary_.gradient_max_norm =
(x_ - projected_gradient_step_).lpNorm<Eigen::Infinity>();
iteration_summary_.gradient_norm = (x_ - projected_gradient_step_).norm();
return true;
}
// 1. Add the final timing information to the iteration summary.
// 2. Run the callbacks
// 3. Check for termination based on
// a. Run time
// b. Iteration count
// c. Max norm of the gradient
// d. Size of the trust region radius.
//
// Returns true if user did not terminate the solver and none of these
// termination criterion are met.
bool TrustRegionMinimizer::FinalizeIterationAndCheckIfMinimizerCanContinue() {
if (iteration_summary_.step_is_successful) {
++solver_summary_->num_successful_steps;
if (x_cost_ < minimum_cost_) {
minimum_cost_ = x_cost_;
VectorRef(parameters_, num_parameters_) = x_;
iteration_summary_.step_is_nonmonotonic = false;
} else {
iteration_summary_.step_is_nonmonotonic = true;
}
} else {
++solver_summary_->num_unsuccessful_steps;
}
iteration_summary_.trust_region_radius = strategy_->Radius();
const absl::Time now = absl::Now();
iteration_summary_.iteration_time_in_seconds =
absl::ToDoubleSeconds(now - iteration_start_time_);
iteration_summary_.cumulative_time_in_seconds =
absl::ToDoubleSeconds(now - start_time_) +
solver_summary_->preprocessor_time_in_seconds;
solver_summary_->iterations.push_back(iteration_summary_);
if (!RunCallbacks(options_, iteration_summary_, solver_summary_)) {
return false;
}
if (MaxSolverTimeReached()) {
return false;
}
if (MaxSolverIterationsReached()) {
return false;
}
if (GradientToleranceReached()) {
return false;
}
if (MinTrustRegionRadiusReached()) {
return false;
}
return true;
}
// Compute the trust region step using the TrustRegionStrategy chosen
// by the user.
//
// If the strategy returns with LinearSolverTerminationType::FATAL_ERROR, which
// indicates an unrecoverable error, return false. This is the only
// condition that returns false.
//
// If the strategy returns with LinearSolverTerminationType::FAILURE, which
// indicates a numerical failure that could be recovered from by retrying (e.g.
// by increasing the strength of the regularization), we set
// iteration_summary_.step_is_valid to false and return true.
//
// In all other cases, we compute the decrease in the trust region
// model problem. In exact arithmetic, this should always be
// positive, but due to numerical problems in the TrustRegionStrategy
// or round off error when computing the decrease it may be
// negative. In which case again, we set
// iteration_summary_.step_is_valid to false.
bool TrustRegionMinimizer::ComputeTrustRegionStep() {
const absl::Time strategy_start_time = absl::Now();
iteration_summary_.step_is_valid = false;
TrustRegionStrategy::PerSolveOptions per_solve_options;
per_solve_options.eta = options_.eta;
if (std::find(options_.trust_region_minimizer_iterations_to_dump.begin(),
options_.trust_region_minimizer_iterations_to_dump.end(),
iteration_summary_.iteration) !=
options_.trust_region_minimizer_iterations_to_dump.end()) {
per_solve_options.dump_format_type =
options_.trust_region_problem_dump_format_type;
per_solve_options.dump_filename_base =
JoinPath(options_.trust_region_problem_dump_directory,
absl::StrFormat("ceres_solver_iteration_%03d",
iteration_summary_.iteration));
}
TrustRegionStrategy::Summary strategy_summary =
strategy_->ComputeStep(per_solve_options,
jacobian_,
residuals_.data(),
trust_region_step_.data());
if (strategy_summary.termination_type ==
LinearSolverTerminationType::FATAL_ERROR) {
solver_summary_->message =
"Linear solver failed due to unrecoverable "
"non-numeric causes. Please see the error log for clues. ";
solver_summary_->termination_type = FAILURE;
return false;
}
iteration_summary_.step_solver_time_in_seconds =
absl::ToDoubleSeconds(absl::Now() - strategy_start_time);
iteration_summary_.linear_solver_iterations = strategy_summary.num_iterations;
if (strategy_summary.termination_type ==
LinearSolverTerminationType::FAILURE) {
return true;
}
// new_model_cost
// = 1/2 [f + J * step]^2
// = 1/2 [ f'f + 2f'J * step + step' * J' * J * step ]
// model_cost_change
// = cost - new_model_cost
// = f'f/2 - 1/2 [ f'f + 2f'J * step + step' * J' * J * step]
// = -f'J * step - step' * J' * J * step / 2
// = -(J * step)'(f + J * step / 2)
ParallelSetZero(options_.context, options_.num_threads, model_residuals_);
jacobian_->RightMultiplyAndAccumulate(trust_region_step_.data(),
model_residuals_.data(),
options_.context,
options_.num_threads);
model_cost_change_ = -Dot(model_residuals_,
residuals_ + model_residuals_ / 2.0,
options_.context,
options_.num_threads);
// TODO(sameeragarwal)
//
// 1. What happens if model_cost_change_ = 0
// 2. What happens if -epsilon <= model_cost_change_ < 0 for some
// small epsilon due to round off error.
iteration_summary_.step_is_valid = (model_cost_change_ > 0.0);
if (iteration_summary_.step_is_valid) {
// Undo the Jacobian column scaling.
ParallelAssign(options_.context,
options_.num_threads,
delta_,
(trust_region_step_.array() * jacobian_scaling_.array()));
num_consecutive_invalid_steps_ = 0;
}
if (is_not_silent_ && !iteration_summary_.step_is_valid) {
VLOG(1) << "Invalid step: current_cost: " << x_cost_
<< " absolute model cost change: " << model_cost_change_
<< " relative model cost change: "
<< (model_cost_change_ / x_cost_);
}
return true;
}
// Invalid steps can happen due to a number of reasons, and we allow a
// limited number of consecutive failures, and return false if this
// limit is exceeded.
bool TrustRegionMinimizer::HandleInvalidStep() {
// TODO(sameeragarwal): Should we be returning FAILURE or
// NO_CONVERGENCE? The solution value is still usable in many cases,
// it is not clear if we should declare the solver a failure
// entirely. For example the case where model_cost_change ~ 0.0, but
// just slightly negative.
if (++num_consecutive_invalid_steps_ >=
options_.max_num_consecutive_invalid_steps) {
solver_summary_->message = absl::StrFormat(
"Number of consecutive invalid steps more "
"than Solver::Options::max_num_consecutive_invalid_steps: %d",
options_.max_num_consecutive_invalid_steps);
solver_summary_->termination_type = FAILURE;
return false;
}
strategy_->StepIsInvalid();
// We are going to try and reduce the trust region radius and
// solve again. To do this, we are going to treat this iteration
// as an unsuccessful iteration. Since the various callbacks are
// still executed, we are going to fill the iteration summary
// with data that assumes a step of length zero and no progress.
iteration_summary_.cost = x_cost_ + solver_summary_->fixed_cost;
iteration_summary_.cost_change = 0.0;
iteration_summary_.gradient_max_norm =
solver_summary_->iterations.back().gradient_max_norm;
iteration_summary_.gradient_norm =
solver_summary_->iterations.back().gradient_norm;
iteration_summary_.step_norm = 0.0;
iteration_summary_.relative_decrease = 0.0;
iteration_summary_.eta = options_.eta;
return true;
}
// Use the supplied coordinate descent minimizer to perform inner
// iterations and compute the improvement due to it. Returns the cost
// after performing the inner iterations.
//
// The optimization is performed with candidate_x_ as the starting
// point, and if the optimization is successful, candidate_x_ will be
// updated with the optimized parameters.
void TrustRegionMinimizer::DoInnerIterationsIfNeeded() {
inner_iterations_were_useful_ = false;
if (!inner_iterations_are_enabled_ ||
candidate_cost_ >= std::numeric_limits<double>::max()) {
return;
}
const absl::Time inner_iteration_start_time = absl::Now();
++solver_summary_->num_inner_iteration_steps;
inner_iteration_x_ = candidate_x_;
Solver::Summary inner_iteration_summary;
options_.inner_iteration_minimizer->Minimize(
options_, inner_iteration_x_.data(), &inner_iteration_summary);
double inner_iteration_cost;
if (!evaluator_->Evaluate(inner_iteration_x_.data(),
&inner_iteration_cost,
nullptr,
nullptr,
nullptr)) {
if (is_not_silent_) {
VLOG(2) << "Inner iteration failed.";
}
return;
}
if (is_not_silent_) {
VLOG(2) << "Inner iteration succeeded; Current cost: " << x_cost_
<< " Trust region step cost: " << candidate_cost_
<< " Inner iteration cost: " << inner_iteration_cost;
}
candidate_x_ = inner_iteration_x_;
// Normally, the quality of a trust region step is measured by
// the ratio
//
// cost_change
// r = -----------------
// model_cost_change
//
// All the change in the nonlinear objective is due to the trust
// region step so this ratio is a good measure of the quality of
// the trust region radius. However, when inner iterations are
// being used, cost_change includes the contribution of the
// inner iterations and it's not fair to credit it all to the
// trust region algorithm. So we change the ratio to be
//
// cost_change
// r = ------------------------------------------------
// (model_cost_change + inner_iteration_cost_change)
//
// Practically we do this by increasing model_cost_change by
// inner_iteration_cost_change.
const double inner_iteration_cost_change =
candidate_cost_ - inner_iteration_cost;
model_cost_change_ += inner_iteration_cost_change;
inner_iterations_were_useful_ = inner_iteration_cost < x_cost_;
const double inner_iteration_relative_progress =
1.0 - inner_iteration_cost / candidate_cost_;
// Disable inner iterations once the relative improvement
// drops below tolerance.
inner_iterations_are_enabled_ =
(inner_iteration_relative_progress > options_.inner_iteration_tolerance);
if (is_not_silent_ && !inner_iterations_are_enabled_) {
VLOG(2) << "Disabling inner iterations. Progress : "
<< inner_iteration_relative_progress;
}
candidate_cost_ = inner_iteration_cost;
solver_summary_->inner_iteration_time_in_seconds +=
absl::ToDoubleSeconds(absl::Now() - inner_iteration_start_time);
}
// Perform a projected line search to improve the objective function
// value along delta.
//
// TODO(sameeragarwal): The current implementation does not do
// anything illegal but is incorrect and not terribly effective.
//
// https://github.com/ceres-solver/ceres-solver/issues/187
void TrustRegionMinimizer::DoLineSearch(const Vector& x,
const Vector& gradient,
const double cost,
Vector* delta) {
LineSearchFunction line_search_function(evaluator_);
LineSearch::Options line_search_options;
line_search_options.is_silent = true;
line_search_options.interpolation_type =
options_.line_search_interpolation_type;
line_search_options.min_step_size = options_.min_line_search_step_size;
line_search_options.sufficient_decrease =
options_.line_search_sufficient_function_decrease;
line_search_options.max_step_contraction =
options_.max_line_search_step_contraction;
line_search_options.min_step_contraction =
options_.min_line_search_step_contraction;
line_search_options.max_num_iterations =
options_.max_num_line_search_step_size_iterations;
line_search_options.sufficient_curvature_decrease =
options_.line_search_sufficient_curvature_decrease;
line_search_options.max_step_expansion =
options_.max_line_search_step_expansion;
line_search_options.function = &line_search_function;
std::string message;
std::unique_ptr<LineSearch> line_search(
LineSearch::Create(ceres::ARMIJO, line_search_options, &message));
LineSearch::Summary line_search_summary;
line_search_function.Init(x, *delta);
line_search->Search(1.0, cost, gradient.dot(*delta), &line_search_summary);
solver_summary_->num_line_search_steps += line_search_summary.num_iterations;
solver_summary_->line_search_cost_evaluation_time_in_seconds +=
absl::ToDoubleSeconds(line_search_summary.cost_evaluation_time);
solver_summary_->line_search_gradient_evaluation_time_in_seconds +=
absl::ToDoubleSeconds(line_search_summary.gradient_evaluation_time);
solver_summary_->line_search_polynomial_minimization_time_in_seconds +=
absl::ToDoubleSeconds(line_search_summary.polynomial_minimization_time);
solver_summary_->line_search_total_time_in_seconds +=
absl::ToDoubleSeconds(line_search_summary.total_time);
if (line_search_summary.success) {
*delta *= line_search_summary.optimal_point.x;
}
}
// Check if the maximum amount of time allowed by the user for the
// solver has been exceeded, and if so return false after updating
// Solver::Summary::message.
bool TrustRegionMinimizer::MaxSolverTimeReached() {
const double total_solver_time =
absl::ToDoubleSeconds(absl::Now() - start_time_) +
solver_summary_->preprocessor_time_in_seconds;
if (total_solver_time < options_.max_solver_time_in_seconds) {
return false;
}
solver_summary_->message = absl::StrFormat(
"Maximum solver time reached. "
"Total solver time: %e >= %e.",
total_solver_time,
options_.max_solver_time_in_seconds);
solver_summary_->termination_type = NO_CONVERGENCE;
if (is_not_silent_) {
VLOG(1) << "Terminating: " << solver_summary_->message;
}
return true;
}
// Check if the maximum number of iterations allowed by the user for
// the solver has been exceeded, and if so return false after updating
// Solver::Summary::message.
bool TrustRegionMinimizer::MaxSolverIterationsReached() {
if (iteration_summary_.iteration < options_.max_num_iterations) {
return false;
}
solver_summary_->message = absl::StrFormat(
"Maximum number of iterations reached. "
"Number of iterations: %d.",
iteration_summary_.iteration);
solver_summary_->termination_type = NO_CONVERGENCE;
if (is_not_silent_) {
VLOG(1) << "Terminating: " << solver_summary_->message;
}
return true;
}
// Check convergence based on the max norm of the gradient (only for
// iterations where the step was declared successful).
bool TrustRegionMinimizer::GradientToleranceReached() {
if (!iteration_summary_.step_is_successful ||
iteration_summary_.gradient_max_norm > options_.gradient_tolerance) {
return false;
}
solver_summary_->message = absl::StrFormat(
"Gradient tolerance reached. "
"Gradient max norm: %e <= %e",
iteration_summary_.gradient_max_norm,
options_.gradient_tolerance);
solver_summary_->termination_type = CONVERGENCE;
if (is_not_silent_) {
VLOG(1) << "Terminating: " << solver_summary_->message;
}
return true;
}
// Check convergence based the size of the trust region radius.
bool TrustRegionMinimizer::MinTrustRegionRadiusReached() {
if (iteration_summary_.trust_region_radius >
options_.min_trust_region_radius) {
return false;
}
solver_summary_->message = absl::StrFormat(
"Minimum trust region radius reached. "
"Trust region radius: %e <= %e",
iteration_summary_.trust_region_radius,
options_.min_trust_region_radius);
solver_summary_->termination_type = CONVERGENCE;
if (is_not_silent_) {
VLOG(1) << "Terminating: " << solver_summary_->message;
}
return true;
}
// Solver::Options::parameter_tolerance based convergence check.
bool TrustRegionMinimizer::ParameterToleranceReached() {
const double x_norm = x_.norm();
// Compute the norm of the step in the ambient space.
iteration_summary_.step_norm = (x_ - candidate_x_).norm();
const double step_size_tolerance =
options_.parameter_tolerance * (x_norm + options_.parameter_tolerance);
if (iteration_summary_.step_norm > step_size_tolerance) {
return false;
}
solver_summary_->message = absl::StrFormat(
"Parameter tolerance reached. "
"Relative step_norm: %e <= %e.",
(iteration_summary_.step_norm / (x_norm + options_.parameter_tolerance)),
options_.parameter_tolerance);
solver_summary_->termination_type = CONVERGENCE;
if (is_not_silent_) {
VLOG(1) << "Terminating: " << solver_summary_->message;
}
return true;
}
// Solver::Options::function_tolerance based convergence check.
bool TrustRegionMinimizer::FunctionToleranceReached() {
iteration_summary_.cost_change = x_cost_ - candidate_cost_;
const double absolute_function_tolerance =
options_.function_tolerance * x_cost_;
if (fabs(iteration_summary_.cost_change) > absolute_function_tolerance) {
return false;
}
solver_summary_->message = absl::StrFormat(
"Function tolerance reached. "
"|cost_change|/cost: %e <= %e",
fabs(iteration_summary_.cost_change) / x_cost_,
options_.function_tolerance);
solver_summary_->termination_type = CONVERGENCE;
if (is_not_silent_) {
VLOG(1) << "Terminating: " << solver_summary_->message;
}
return true;
}
// Compute candidate_x_ = Plus(x_, delta_)
// Evaluate the cost of candidate_x_ as candidate_cost_.
//
// Failure to compute the step or the cost mean that candidate_cost_ is set to
// std::numeric_limits<double>::max(). Unlike EvaluateGradientAndJacobian,
// failure in this function is not fatal as we are only computing and evaluating
// a candidate point, and if for some reason we are unable to evaluate it, we
// consider it to be a point with very high cost. This allows the user to deal
// with edge cases/constraints as part of the Manifold and CostFunction objects.
void TrustRegionMinimizer::ComputeCandidatePointAndEvaluateCost() {
if (!evaluator_->Plus(x_.data(), delta_.data(), candidate_x_.data())) {
if (is_not_silent_) {
LOG(WARNING) << "x_plus_delta = Plus(x, delta) failed. "
<< "Treating it as a step with infinite cost";
}
candidate_cost_ = std::numeric_limits<double>::max();
return;
}
if (!evaluator_->Evaluate(
candidate_x_.data(), &candidate_cost_, nullptr, nullptr, nullptr)) {
if (is_not_silent_) {
LOG(WARNING) << "Step failed to evaluate. "
<< "Treating it as a step with infinite cost";
}
candidate_cost_ = std::numeric_limits<double>::max();
}
}
bool TrustRegionMinimizer::IsStepSuccessful() {
iteration_summary_.relative_decrease =
step_evaluator_->StepQuality(candidate_cost_, model_cost_change_);
// In most cases, boosting the model_cost_change by the
// improvement caused by the inner iterations is fine, but it can
// be the case that the original trust region step was so bad that
// the resulting improvement in the cost was negative, and the
// change caused by the inner iterations was large enough to
// improve the step, but also to make relative decrease quite
// small.
//
// This can cause the trust region loop to reject this step. To
// get around this, we explicitly check if the inner iterations
// led to a net decrease in the objective function value. If
// they did, we accept the step even if the trust region ratio
// is small.
//
// Notice that we do not just check that cost_change is positive
// which is a weaker condition and would render the
// min_relative_decrease threshold useless. Instead, we keep
// track of inner_iterations_were_useful, which is true only
// when inner iterations lead to a net decrease in the cost.
return (inner_iterations_were_useful_ ||
iteration_summary_.relative_decrease >
options_.min_relative_decrease);
}
// Declare the step successful, move to candidate_x, update the
// derivatives and let the trust region strategy and the step
// evaluator know that the step has been accepted.
bool TrustRegionMinimizer::HandleSuccessfulStep() {
x_ = candidate_x_;
// Since the step was successful, this point has already had the residual
// evaluated (but not the jacobian). So indicate that to the evaluator.
if (!EvaluateGradientAndJacobian(/*new_evaluation_point=*/false)) {
return false;
}
iteration_summary_.step_is_successful = true;
strategy_->StepAccepted(iteration_summary_.relative_decrease);
step_evaluator_->StepAccepted(candidate_cost_, model_cost_change_);
return true;
}
} // namespace ceres::internal