New Trust region loop.
1. New TrustRegionMinimizer and basic tests for it.
2. New TrustRegionStrategy interface.
3. New LevenbergMarquardtStrategy and tests for it.
4. Updates to SolverImpl to reflect this.
5. Changes to Solver::Options and IterationSummary related to this.
6. Deleted levenberg_marquardt.cc/h/_test.cc
Change-Id: I6c1d1a7c774f014856f9f26263a830aa886e1400
diff --git a/internal/ceres/trust_region_minimizer.cc b/internal/ceres/trust_region_minimizer.cc
new file mode 100644
index 0000000..bb6c4a1
--- /dev/null
+++ b/internal/ceres/trust_region_minimizer.cc
@@ -0,0 +1,445 @@
+// 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 <string>
+#include <vector>
+#include <glog/logging.h>
+
+#include "Eigen/Core"
+#include "ceres/array_utils.h"
+#include "ceres/evaluator.h"
+#include "ceres/linear_least_squares_problems.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/internal/scoped_ptr.h"
+#include "ceres/sparse_matrix.h"
+#include "ceres/trust_region_strategy.h"
+#include "ceres/types.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 Minimizer::Options& options_,
+ 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 / (kEpsilon + 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.
+ //
+ // Doing this right requires either an API change to the
+ // TrustRegionStrategy and/or how LinearLeastSquares problems are
+ // stored on disk.
+ //
+ // For now, we will just not dump the regularizer.
+ return (!binary_search(options_.lsqp_iterations_to_dump.begin(),
+ options_.lsqp_iterations_to_dump.end(),
+ iteration) ||
+ DumpLinearLeastSquaresProblem(options_.lsqp_dump_directory,
+ iteration,
+ options_.lsqp_dump_format_type,
+ jacobian,
+ NULL,
+ residuals,
+ step,
+ options_.num_eliminate_blocks));
+}
+
+void TrustRegionMinimizer::Minimize(const Minimizer::Options& options,
+ double* parameters,
+ Solver::Summary* summary) {
+ time_t start_time = time(NULL);
+ time_t 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(parameters, num_parameters);
+ 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 = summary->initial_cost;
+ 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_sec = 0;
+
+ // Do initial cost and Jacobian evaluation.
+ double cost = 0.0;
+ if (!evaluator->Evaluate(x.data(), &cost, residuals.data(), jacobian)) {
+ LOG(WARNING) << "Terminating: Residual and Jacobian evaluation failed.";
+ summary->termination_type = NUMERICAL_FAILURE;
+ return;
+ }
+
+ // Compute the fixed part of the cost.
+ //
+ // This is a poor way to do this computation. Even if fixed_cost is
+ // zero, because we are subtracting two possibly large numbers, we
+ // are depending on exact cancellation to give us a zero here. But
+ // initial_cost and cost have been computed by two different
+ // evaluators. One which runs on the whole problem (in
+ // solver_impl.cc) in single threaded mode and another which runs
+ // here on the reduced problem, so fixed_cost can (and does) contain
+ // some numerical garbage with a relative magnitude of 1e-14.
+ //
+ // The right way to do this, would be to compute the fixed cost on
+ // just the set of residual blocks which are held constant and were
+ // removed from the original problem when the reduced problem was
+ // constructed.
+ summary->fixed_cost = summary->initial_cost - cost;
+
+ 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();
+ }
+
+ const double absolute_gradient_tolerance =
+ options_.gradient_tolerance *
+ max(iteration_summary.gradient_max_norm, kEpsilon);
+ 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 /
+ summary->iterations[0].gradient_max_norm
+ << " <= " << options_.gradient_tolerance;
+ return;
+ }
+
+ // Call the various callbacks.
+ iteration_summary.iteration_time_sec = time(NULL) - iteration_start_time;
+ summary->iterations.push_back(iteration_summary);
+ switch (RunCallbacks(options_, 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 = time(NULL);
+ if (iteration_summary.iteration >= options_.max_num_iterations) {
+ summary->termination_type = NO_CONVERGENCE;
+ VLOG(1) << "Terminating: Maximum number of iterations reached.";
+ break;
+ }
+
+ if ((start_time - iteration_start_time) >= options_.max_solver_time_sec) {
+ 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 time_t strategy_start_time = time(NULL);
+ TrustRegionStrategy::PerSolveOptions per_solve_options;
+ per_solve_options.eta = options_.eta;
+ LinearSolver::Summary strategy_summary =
+ strategy->ComputeStep(per_solve_options,
+ jacobian,
+ residuals.data(),
+ trust_region_step.data());
+
+ iteration_summary.step_solver_time_sec = time(NULL) - 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 new_model_cost = 0.0;
+ if (strategy_summary.termination_type != FAILURE) {
+ // new_model_cost = 1/2 |J * step - f|^2
+ model_residuals = -residuals;
+ jacobian->RightMultiply(trust_region_step.data(), model_residuals.data());
+ new_model_cost = model_residuals.squaredNorm() / 2.0;
+
+ // In exact arithmetic, this would never be the case. But poorly
+ // conditioned matrices can give rise to situations where the
+ // new_model_cost can actually be larger than half the squared
+ // norm of the residual vector. We allow for small tolerance
+ // around cost and beyond that declare the step to be invalid.
+ if (cost < (new_model_cost - kEpsilon)) {
+ VLOG(1) << "Invalid step: current_cost: " << cost
+ << " new_model_cost " << new_model_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;
+ LOG(WARNING) << "Terminating. Number of successive invalid steps more "
+ << "than "
+ << "Solver::Options::max_num_consecutive_invalid_steps: "
+ << options_.max_num_consecutive_invalid_steps;
+ 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;
+ 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;
+
+ // We allow some slop around 0, and clamp the model_cost_change
+ // at kEpsilon from below.
+ //
+ // There is probably a better way to do this, as it is going to
+ // create problems for problems where the objective function is
+ // kEpsilon close to zero.
+ const double model_cost_change = max(kEpsilon, cost - new_model_cost);
+
+ // Undo the Jacobian column scaling.
+ delta = -(trust_region_step.array() * scale.array()).matrix();
+ iteration_summary.step_norm = 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;
+ }
+
+ if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) {
+ summary->termination_type = NUMERICAL_FAILURE;
+ LOG(WARNING) << "Terminating. Failed to compute "
+ << "Plus(x, delta, x_plus_delta).";
+ return;
+ }
+
+ // Try this step.
+ double new_cost;
+ if (!evaluator->Evaluate(x_plus_delta.data(), &new_cost, NULL, NULL)) {
+ summary->termination_type = NUMERICAL_FAILURE;
+ LOG(WARNING) << "Terminating: Cost evaluation failed.";
+ 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;
+ }
+
+ iteration_summary.relative_decrease =
+ iteration_summary.cost_change / model_cost_change;
+ iteration_summary.step_is_successful =
+ iteration_summary.relative_decrease > options_.min_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(), jacobian)) {
+ summary->termination_type = NUMERICAL_FAILURE;
+ LOG(WARNING) << "Terminating: Residual and Jacobian evaluation failed.";
+ 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 /
+ summary->iterations[0].gradient_max_norm
+ << " <= " << options_.gradient_tolerance;
+ return;
+ }
+
+ if (options_.jacobi_scaling) {
+ jacobian->ScaleColumns(scale.data());
+ }
+ } else {
+ ++summary->num_unsuccessful_steps;
+ strategy->StepRejected(iteration_summary.relative_decrease);
+ }
+
+ 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_sec = time(NULL) - iteration_start_time;
+ summary->iterations.push_back(iteration_summary);
+ switch (RunCallbacks(options_, 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
