| // 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 |
