diff options
Diffstat (limited to 'internal/ceres/trust_region_minimizer.cc')
| -rw-r--r-- | internal/ceres/trust_region_minimizer.cc | 557 |
1 files changed, 557 insertions, 0 deletions
diff --git a/internal/ceres/trust_region_minimizer.cc b/internal/ceres/trust_region_minimizer.cc new file mode 100644 index 0000000..db2641d --- /dev/null +++ b/internal/ceres/trust_region_minimizer.cc @@ -0,0 +1,557 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 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: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/trust_region_minimizer.h" + +#include <algorithm> +#include <cstdlib> +#include <cmath> +#include <cstring> +#include <limits> +#include <string> +#include <vector> + +#include "Eigen/Core" +#include "ceres/array_utils.h" +#include "ceres/evaluator.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/linear_least_squares_problems.h" +#include "ceres/sparse_matrix.h" +#include "ceres/stringprintf.h" +#include "ceres/trust_region_strategy.h" +#include "ceres/types.h" +#include "ceres/wall_time.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { +namespace { +// Small constant for various floating point issues. +const double kEpsilon = 1e-12; +} // namespace + +// Execute the list of IterationCallbacks sequentially. If any one of +// the callbacks does not return SOLVER_CONTINUE, then stop and return +// its status. +CallbackReturnType TrustRegionMinimizer::RunCallbacks( + const IterationSummary& iteration_summary) { + for (int i = 0; i < options_.callbacks.size(); ++i) { + const CallbackReturnType status = + (*options_.callbacks[i])(iteration_summary); + if (status != SOLVER_CONTINUE) { + return status; + } + } + return SOLVER_CONTINUE; +} + +// Compute a scaling vector that is used to improve the conditioning +// of the Jacobian. +void TrustRegionMinimizer::EstimateScale(const SparseMatrix& jacobian, + double* scale) const { + jacobian.SquaredColumnNorm(scale); + for (int i = 0; i < jacobian.num_cols(); ++i) { + scale[i] = 1.0 / (1.0 + sqrt(scale[i])); + } +} + +void TrustRegionMinimizer::Init(const Minimizer::Options& options) { + options_ = options; + sort(options_.lsqp_iterations_to_dump.begin(), + options_.lsqp_iterations_to_dump.end()); +} + +bool TrustRegionMinimizer::MaybeDumpLinearLeastSquaresProblem( + const int iteration, + const SparseMatrix* jacobian, + const double* residuals, + const double* step) const { + // TODO(sameeragarwal): Since the use of trust_region_radius has + // moved inside TrustRegionStrategy, its not clear how we dump the + // regularization vector/matrix anymore. + // + // Also num_eliminate_blocks is not visible to the trust region + // minimizer either. + // + // Both of these indicate that this is the wrong place for this + // code, and going forward this should needs fixing/refactoring. + return true; +} + +void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, + double* parameters, + Solver::Summary* summary) { + double start_time = WallTimeInSeconds(); + double iteration_start_time = start_time; + Init(options); + + summary->termination_type = NO_CONVERGENCE; + summary->num_successful_steps = 0; + summary->num_unsuccessful_steps = 0; + + Evaluator* evaluator = CHECK_NOTNULL(options_.evaluator); + SparseMatrix* jacobian = CHECK_NOTNULL(options_.jacobian); + TrustRegionStrategy* strategy = CHECK_NOTNULL(options_.trust_region_strategy); + + const int num_parameters = evaluator->NumParameters(); + const int num_effective_parameters = evaluator->NumEffectiveParameters(); + const int num_residuals = evaluator->NumResiduals(); + + VectorRef x_min(parameters, num_parameters); + Vector x = x_min; + double x_norm = x.norm(); + + Vector residuals(num_residuals); + Vector trust_region_step(num_effective_parameters); + Vector delta(num_effective_parameters); + Vector x_plus_delta(num_parameters); + Vector gradient(num_effective_parameters); + Vector model_residuals(num_residuals); + Vector scale(num_effective_parameters); + + IterationSummary iteration_summary; + 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.step_norm = 0.0; + iteration_summary.relative_decrease = 0.0; + iteration_summary.trust_region_radius = strategy->Radius(); + // TODO(sameeragarwal): Rename eta to linear_solver_accuracy or + // something similar across the board. + iteration_summary.eta = options_.eta; + iteration_summary.linear_solver_iterations = 0; + iteration_summary.step_solver_time_in_seconds = 0; + + // Do initial cost and Jacobian evaluation. + double cost = 0.0; + if (!evaluator->Evaluate(x.data(), &cost, residuals.data(), NULL, jacobian)) { + LOG(WARNING) << "Terminating: Residual and Jacobian evaluation failed."; + summary->termination_type = NUMERICAL_FAILURE; + return; + } + + iteration_summary.cost = cost + summary->fixed_cost; + + int num_consecutive_nonmonotonic_steps = 0; + double minimum_cost = cost; + double reference_cost = cost; + double accumulated_reference_model_cost_change = 0.0; + double candidate_cost = cost; + double accumulated_candidate_model_cost_change = 0.0; + + gradient.setZero(); + jacobian->LeftMultiply(residuals.data(), gradient.data()); + iteration_summary.gradient_max_norm = gradient.lpNorm<Eigen::Infinity>(); + + if (options_.jacobi_scaling) { + EstimateScale(*jacobian, scale.data()); + jacobian->ScaleColumns(scale.data()); + } else { + scale.setOnes(); + } + + // The initial gradient max_norm is bounded from below so that we do + // not divide by zero. + const double gradient_max_norm_0 = + max(iteration_summary.gradient_max_norm, kEpsilon); + const double absolute_gradient_tolerance = + options_.gradient_tolerance * gradient_max_norm_0; + + if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) { + summary->termination_type = GRADIENT_TOLERANCE; + VLOG(1) << "Terminating: Gradient tolerance reached." + << "Relative gradient max norm: " + << iteration_summary.gradient_max_norm / gradient_max_norm_0 + << " <= " << options_.gradient_tolerance; + return; + } + + iteration_summary.iteration_time_in_seconds = + WallTimeInSeconds() - iteration_start_time; + iteration_summary.cumulative_time_in_seconds = + WallTimeInSeconds() - start_time + + summary->preprocessor_time_in_seconds; + summary->iterations.push_back(iteration_summary); + + // Call the various callbacks. + switch (RunCallbacks(iteration_summary)) { + case SOLVER_TERMINATE_SUCCESSFULLY: + summary->termination_type = USER_SUCCESS; + VLOG(1) << "Terminating: User callback returned USER_SUCCESS."; + return; + case SOLVER_ABORT: + summary->termination_type = USER_ABORT; + VLOG(1) << "Terminating: User callback returned USER_ABORT."; + return; + case SOLVER_CONTINUE: + break; + default: + LOG(FATAL) << "Unknown type of user callback status"; + } + + int num_consecutive_invalid_steps = 0; + while (true) { + iteration_start_time = WallTimeInSeconds(); + if (iteration_summary.iteration >= options_.max_num_iterations) { + summary->termination_type = NO_CONVERGENCE; + VLOG(1) << "Terminating: Maximum number of iterations reached."; + break; + } + + const double total_solver_time = iteration_start_time - start_time + + summary->preprocessor_time_in_seconds; + if (total_solver_time >= options_.max_solver_time_in_seconds) { + summary->termination_type = NO_CONVERGENCE; + VLOG(1) << "Terminating: Maximum solver time reached."; + break; + } + + iteration_summary = IterationSummary(); + iteration_summary = summary->iterations.back(); + iteration_summary.iteration = summary->iterations.back().iteration + 1; + iteration_summary.step_is_valid = false; + iteration_summary.step_is_successful = false; + + const double strategy_start_time = WallTimeInSeconds(); + TrustRegionStrategy::PerSolveOptions per_solve_options; + per_solve_options.eta = options_.eta; + TrustRegionStrategy::Summary strategy_summary = + strategy->ComputeStep(per_solve_options, + jacobian, + residuals.data(), + trust_region_step.data()); + + iteration_summary.step_solver_time_in_seconds = + WallTimeInSeconds() - strategy_start_time; + iteration_summary.linear_solver_iterations = + strategy_summary.num_iterations; + + if (!MaybeDumpLinearLeastSquaresProblem(iteration_summary.iteration, + jacobian, + residuals.data(), + trust_region_step.data())) { + LOG(FATAL) << "Tried writing linear least squares problem: " + << options.lsqp_dump_directory << "but failed."; + } + + double model_cost_change = 0.0; + if (strategy_summary.termination_type != FAILURE) { + // 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 + model_residuals.setZero(); + jacobian->RightMultiply(trust_region_step.data(), model_residuals.data()); + model_cost_change = -(residuals.dot(model_residuals) + + model_residuals.squaredNorm() / 2.0); + + if (model_cost_change < 0.0) { + VLOG(1) << "Invalid step: current_cost: " << cost + << " absolute difference " << model_cost_change + << " relative difference " << (model_cost_change / cost); + } else { + iteration_summary.step_is_valid = true; + } + } + + if (!iteration_summary.step_is_valid) { + // Invalid steps can happen due to a number of reasons, and we + // allow a limited number of successive failures, and return with + // NUMERICAL_FAILURE if this limit is exceeded. + if (++num_consecutive_invalid_steps >= + options_.max_num_consecutive_invalid_steps) { + summary->termination_type = NUMERICAL_FAILURE; + summary->error = StringPrintf( + "Terminating. Number of successive invalid steps more " + "than Solver::Options::max_num_consecutive_invalid_steps: %d", + options_.max_num_consecutive_invalid_steps); + + LOG(WARNING) << summary->error; + return; + } + + // 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 = cost + summary->fixed_cost; + iteration_summary.cost_change = 0.0; + iteration_summary.gradient_max_norm = + summary->iterations.back().gradient_max_norm; + iteration_summary.step_norm = 0.0; + iteration_summary.relative_decrease = 0.0; + iteration_summary.eta = options_.eta; + } else { + // The step is numerically valid, so now we can judge its quality. + num_consecutive_invalid_steps = 0; + + // Undo the Jacobian column scaling. + delta = (trust_region_step.array() * scale.array()).matrix(); + if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) { + summary->termination_type = NUMERICAL_FAILURE; + summary->error = + "Terminating. Failed to compute Plus(x, delta, x_plus_delta)."; + + LOG(WARNING) << summary->error; + return; + } + + // Try this step. + double new_cost = numeric_limits<double>::max(); + if (!evaluator->Evaluate(x_plus_delta.data(), + &new_cost, + NULL, NULL, NULL)) { + // If the evaluation of the new cost fails, treat it as a step + // with high cost. + LOG(WARNING) << "Step failed to evaluate. " + << "Treating it as step with infinite cost"; + new_cost = numeric_limits<double>::max(); + } else { + // Check if performing an inner iteration will make it better. + if (options.inner_iteration_minimizer != NULL) { + const double x_plus_delta_cost = new_cost; + Vector inner_iteration_x = x_plus_delta; + Solver::Summary inner_iteration_summary; + options.inner_iteration_minimizer->Minimize(options, + inner_iteration_x.data(), + &inner_iteration_summary); + if(!evaluator->Evaluate(inner_iteration_x.data(), + &new_cost, + NULL, NULL, NULL)) { + VLOG(2) << "Inner iteration failed."; + new_cost = x_plus_delta_cost; + } else { + x_plus_delta = inner_iteration_x; + // Bost the model_cost_change, since the inner iteration + // improvements are not accounted for by the trust region. + model_cost_change += x_plus_delta_cost - new_cost; + VLOG(2) << "Inner iteration succeeded; current cost: " << cost + << " x_plus_delta_cost: " << x_plus_delta_cost + << " new_cost: " << new_cost; + } + } + } + + iteration_summary.step_norm = (x - x_plus_delta).norm(); + + // Convergence based on parameter_tolerance. + const double step_size_tolerance = options_.parameter_tolerance * + (x_norm + options_.parameter_tolerance); + if (iteration_summary.step_norm <= step_size_tolerance) { + VLOG(1) << "Terminating. Parameter tolerance reached. " + << "relative step_norm: " + << iteration_summary.step_norm / + (x_norm + options_.parameter_tolerance) + << " <= " << options_.parameter_tolerance; + summary->termination_type = PARAMETER_TOLERANCE; + return; + } + + VLOG(2) << "old cost: " << cost << " new cost: " << new_cost; + iteration_summary.cost_change = cost - new_cost; + const double absolute_function_tolerance = + options_.function_tolerance * cost; + if (fabs(iteration_summary.cost_change) < absolute_function_tolerance) { + VLOG(1) << "Terminating. Function tolerance reached. " + << "|cost_change|/cost: " + << fabs(iteration_summary.cost_change) / cost + << " <= " << options_.function_tolerance; + summary->termination_type = FUNCTION_TOLERANCE; + return; + } + + const double relative_decrease = + iteration_summary.cost_change / model_cost_change; + + const double historical_relative_decrease = + (reference_cost - new_cost) / + (accumulated_reference_model_cost_change + model_cost_change); + + // If monotonic steps are being used, then the relative_decrease + // is the usual ratio of the change in objective function value + // divided by the change in model cost. + // + // If non-monotonic steps are allowed, then we take the maximum + // of the relative_decrease and the + // historical_relative_decrease, which measures the increase + // from a reference iteration. The model cost change is + // estimated by accumulating the model cost changes since the + // reference iteration. The historical relative_decrease offers + // a boost to a step which is not too bad compared to the + // reference iteration, allowing for non-monotonic steps. + iteration_summary.relative_decrease = + options.use_nonmonotonic_steps + ? max(relative_decrease, historical_relative_decrease) + : relative_decrease; + + iteration_summary.step_is_successful = + iteration_summary.relative_decrease > options_.min_relative_decrease; + + if (iteration_summary.step_is_successful) { + accumulated_candidate_model_cost_change += model_cost_change; + accumulated_reference_model_cost_change += model_cost_change; + if (relative_decrease <= options_.min_relative_decrease) { + iteration_summary.step_is_nonmonotonic = true; + VLOG(2) << "Non-monotonic step! " + << " relative_decrease: " << relative_decrease + << " historical_relative_decrease: " + << historical_relative_decrease; + } + } + } + + if (iteration_summary.step_is_successful) { + ++summary->num_successful_steps; + strategy->StepAccepted(iteration_summary.relative_decrease); + x = x_plus_delta; + x_norm = x.norm(); + + // Step looks good, evaluate the residuals and Jacobian at this + // point. + if (!evaluator->Evaluate(x.data(), + &cost, + residuals.data(), + NULL, + jacobian)) { + summary->termination_type = NUMERICAL_FAILURE; + summary->error = "Terminating: Residual and Jacobian evaluation failed."; + LOG(WARNING) << summary->error; + return; + } + + gradient.setZero(); + jacobian->LeftMultiply(residuals.data(), gradient.data()); + iteration_summary.gradient_max_norm = gradient.lpNorm<Eigen::Infinity>(); + + if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) { + summary->termination_type = GRADIENT_TOLERANCE; + VLOG(1) << "Terminating: Gradient tolerance reached." + << "Relative gradient max norm: " + << iteration_summary.gradient_max_norm / gradient_max_norm_0 + << " <= " << options_.gradient_tolerance; + return; + } + + if (options_.jacobi_scaling) { + jacobian->ScaleColumns(scale.data()); + } + + // Update the best, reference and candidate iterates. + // + // Based on algorithm 10.1.2 (page 357) of "Trust Region + // Methods" by Conn Gould & Toint, or equations 33-40 of + // "Non-monotone trust-region algorithms for nonlinear + // optimization subject to convex constraints" by Phil Toint, + // Mathematical Programming, 77, 1997. + if (cost < minimum_cost) { + // A step that improves solution quality was found. + x_min = x; + minimum_cost = cost; + // Set the candidate iterate to the current point. + candidate_cost = cost; + num_consecutive_nonmonotonic_steps = 0; + accumulated_candidate_model_cost_change = 0.0; + } else { + ++num_consecutive_nonmonotonic_steps; + if (cost > candidate_cost) { + // The current iterate is has a higher cost than the + // candidate iterate. Set the candidate to this point. + VLOG(2) << "Updating the candidate iterate to the current point."; + candidate_cost = cost; + accumulated_candidate_model_cost_change = 0.0; + } + + // At this point we have made too many non-monotonic steps and + // we are going to reset the value of the reference iterate so + // as to force the algorithm to descend. + // + // This is the case because the candidate iterate has a value + // greater than minimum_cost but smaller than the reference + // iterate. + if (num_consecutive_nonmonotonic_steps == + options.max_consecutive_nonmonotonic_steps) { + VLOG(2) << "Resetting the reference point to the candidate point"; + reference_cost = candidate_cost; + accumulated_reference_model_cost_change = + accumulated_candidate_model_cost_change; + } + } + } else { + ++summary->num_unsuccessful_steps; + if (iteration_summary.step_is_valid) { + strategy->StepRejected(iteration_summary.relative_decrease); + } else { + strategy->StepIsInvalid(); + } + } + + iteration_summary.cost = cost + summary->fixed_cost; + iteration_summary.trust_region_radius = strategy->Radius(); + if (iteration_summary.trust_region_radius < + options_.min_trust_region_radius) { + summary->termination_type = PARAMETER_TOLERANCE; + VLOG(1) << "Termination. Minimum trust region radius reached."; + return; + } + + iteration_summary.iteration_time_in_seconds = + WallTimeInSeconds() - iteration_start_time; + iteration_summary.cumulative_time_in_seconds = + WallTimeInSeconds() - start_time + + summary->preprocessor_time_in_seconds; + summary->iterations.push_back(iteration_summary); + + switch (RunCallbacks(iteration_summary)) { + case SOLVER_TERMINATE_SUCCESSFULLY: + summary->termination_type = USER_SUCCESS; + VLOG(1) << "Terminating: User callback returned USER_SUCCESS."; + return; + case SOLVER_ABORT: + summary->termination_type = USER_ABORT; + VLOG(1) << "Terminating: User callback returned USER_ABORT."; + return; + case SOLVER_CONTINUE: + break; + default: + LOG(FATAL) << "Unknown type of user callback status"; + } + } +} + + +} // namespace internal +} // namespace ceres |