internal/ceres/trust_region_minimizer.cc

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2016 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   4 //
   5 // Redistribution and use in source and binary forms, with or without
   6 // modification, are permitted provided that the following conditions are met:
   7 //
   8 // * Redistributions of source code must retain the above copyright notice,
   9 //   this list of conditions and the following disclaimer.
  10 // * Redistributions in binary form must reproduce the above copyright notice,
  11 //   this list of conditions and the following disclaimer in the documentation
  12 //   and/or other materials provided with the distribution.
  13 // * Neither the name of Google Inc. nor the names of its contributors may be
  14 //   used to endorse or promote products derived from this software without
  15 //   specific prior written permission.
  16 //
  17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  27 // POSSIBILITY OF SUCH DAMAGE.
  28 //
  29 // Author: sameeragarwal@google.com (Sameer Agarwal)
  30
  31 #include "ceres/trust_region_minimizer.h"
  32
  33 #include <algorithm>
  34 #include <cmath>
  35 #include <cstdlib>
  36 #include <cstring>
  37 #include <limits>
  38 #include <string>
  39 #include <vector>
  40
  41 #include "Eigen/Core"
  42 #include "ceres/array_utils.h"
  43 #include "ceres/coordinate_descent_minimizer.h"
  44 #include "ceres/evaluator.h"
  45 #include "ceres/file.h"
  46 #include "ceres/line_search.h"
  47 #include "ceres/stringprintf.h"
  48 #include "ceres/types.h"
  49 #include "ceres/wall_time.h"
  50 #include "glog/logging.h"
  51
  52 // Helper macro to simplify some of the control flow.
  53 #define RETURN_IF_ERROR_AND_LOG(expr)                            \
  54   do {                                                           \
  55     if (!(expr)) {                                               \
  56       LOG(ERROR) << "Terminating: " << solver_summary_->message; \
  57       return;                                                    \
  58     }                                                            \
  59   } while (0)
  60
  61 namespace ceres {
  62 namespace internal {
  63
  64 TrustRegionMinimizer::~TrustRegionMinimizer() {}
  65
  66 void TrustRegionMinimizer::Minimize(const Minimizer::Options& options,
  67                                     double* parameters,
  68                                     Solver::Summary* solver_summary) {
  69   start_time_in_secs_ = WallTimeInSeconds();
  70   iteration_start_time_in_secs_ = start_time_in_secs_;
  71   Init(options, parameters, solver_summary);
  72   RETURN_IF_ERROR_AND_LOG(IterationZero());
  73
  74   // Create the TrustRegionStepEvaluator. The construction needs to be
  75   // delayed to this point because we need the cost for the starting
  76   // point to initialize the step evaluator.
  77   step_evaluator_.reset(new TrustRegionStepEvaluator(
  78       x_cost_,
  79       options_.use_nonmonotonic_steps
  80           ? options_.max_consecutive_nonmonotonic_steps
  81           : 0));
  82
  83   while (FinalizeIterationAndCheckIfMinimizerCanContinue()) {
  84     iteration_start_time_in_secs_ = WallTimeInSeconds();
  85     iteration_summary_ = IterationSummary();
  86     iteration_summary_.iteration =
  87         solver_summary->iterations.back().iteration + 1;
  88
  89     RETURN_IF_ERROR_AND_LOG(ComputeTrustRegionStep());
  90     if (!iteration_summary_.step_is_valid) {
  91       RETURN_IF_ERROR_AND_LOG(HandleInvalidStep());
  92       continue;
  93     }
  94
  95     if (options_.is_constrained) {
  96       // Use a projected line search to enforce the bounds constraints
  97       // and improve the quality of the step.
  98       DoLineSearch(x_, gradient_, x_cost_, &delta_);
  99     }
 100
 101     ComputeCandidatePointAndEvaluateCost();
 102     DoInnerIterationsIfNeeded();
 103
 104     if (ParameterToleranceReached()) {
 105       return;
 106     }
 107
 108     if (FunctionToleranceReached()) {
 109       return;
 110     }
 111
 112     if (IsStepSuccessful()) {
 113       RETURN_IF_ERROR_AND_LOG(HandleSuccessfulStep());
 114       continue;
 115     }
 116
 117     HandleUnsuccessfulStep();
 118   }
 119 }
 120
 121 // Initialize the minimizer, allocate working space and set some of
 122 // the fields in the solver_summary.
 123 void TrustRegionMinimizer::Init(const Minimizer::Options& options,
 124                                 double* parameters,
 125                                 Solver::Summary* solver_summary) {
 126   options_ = options;
 127   sort(options_.trust_region_minimizer_iterations_to_dump.begin(),
 128        options_.trust_region_minimizer_iterations_to_dump.end());
 129
 130   parameters_ = parameters;
 131
 132   solver_summary_ = solver_summary;
 133   solver_summary_->termination_type = NO_CONVERGENCE;
 134   solver_summary_->num_successful_steps = 0;
 135   solver_summary_->num_unsuccessful_steps = 0;
 136   solver_summary_->is_constrained = options.is_constrained;
 137
 138   evaluator_ = CHECK_NOTNULL(options_.evaluator.get());
 139   jacobian_ = CHECK_NOTNULL(options_.jacobian.get());
 140   strategy_ = CHECK_NOTNULL(options_.trust_region_strategy.get());
 141
 142   is_not_silent_ = !options.is_silent;
 143   inner_iterations_are_enabled_ =
 144       options.inner_iteration_minimizer.get() != NULL;
 145   inner_iterations_were_useful_ = false;
 146
 147   num_parameters_ = evaluator_->NumParameters();
 148   num_effective_parameters_ = evaluator_->NumEffectiveParameters();
 149   num_residuals_ = evaluator_->NumResiduals();
 150   num_consecutive_invalid_steps_ = 0;
 151
 152   x_ = ConstVectorRef(parameters_, num_parameters_);
 153   x_norm_ = x_.norm();
 154   residuals_.resize(num_residuals_);
 155   trust_region_step_.resize(num_effective_parameters_);
 156   delta_.resize(num_effective_parameters_);
 157   candidate_x_.resize(num_parameters_);
 158   gradient_.resize(num_effective_parameters_);
 159   model_residuals_.resize(num_residuals_);
 160   negative_gradient_.resize(num_effective_parameters_);
 161   projected_gradient_step_.resize(num_parameters_);
 162
 163   // By default scaling is one, if the user requests Jacobi scaling of
 164   // the Jacobian, we will compute and overwrite this vector.
 165   jacobian_scaling_ = Vector::Ones(num_effective_parameters_);
 166
 167   x_norm_ = -1;  // Invalid value
 168   x_cost_ = std::numeric_limits<double>::max();
 169   minimum_cost_ = x_cost_;
 170   model_cost_change_ = 0.0;
 171 }
 172
 173 // 1. Project the initial solution onto the feasible set if needed.
 174 // 2. Compute the initial cost, jacobian & gradient.
 175 //
 176 // Return true if all computations can be performed successfully.
 177 bool TrustRegionMinimizer::IterationZero() {
 178   iteration_summary_ = IterationSummary();
 179   iteration_summary_.iteration = 0;
 180   iteration_summary_.step_is_valid = false;
 181   iteration_summary_.step_is_successful = false;
 182   iteration_summary_.cost_change = 0.0;
 183   iteration_summary_.gradient_max_norm = 0.0;
 184   iteration_summary_.gradient_norm = 0.0;
 185   iteration_summary_.step_norm = 0.0;
 186   iteration_summary_.relative_decrease = 0.0;
 187   iteration_summary_.eta = options_.eta;
 188   iteration_summary_.linear_solver_iterations = 0;
 189   iteration_summary_.step_solver_time_in_seconds = 0;
 190
 191   if (options_.is_constrained) {
 192     delta_.setZero();
 193     if (!evaluator_->Plus(x_.data(), delta_.data(), candidate_x_.data())) {
 194       solver_summary_->message =
 195           "Unable to project initial point onto the feasible set.";
 196       solver_summary_->termination_type = FAILURE;
 197       return false;
 198     }
 199
 200     x_ = candidate_x_;
 201     x_norm_ = x_.norm();
 202   }
 203
 204   if (!EvaluateGradientAndJacobian()) {
 205     return false;
 206   }
 207
 208   solver_summary_->initial_cost = x_cost_ + solver_summary_->fixed_cost;
 209   iteration_summary_.step_is_valid = true;
 210   iteration_summary_.step_is_successful = true;
 211   return true;
 212 }
 213
 214 // For the current x_, compute
 215 //
 216 //  1. Cost
 217 //  2. Jacobian
 218 //  3. Gradient
 219 //  4. Scale the Jacobian if needed (and compute the scaling if we are
 220 //     in iteration zero).
 221 //  5. Compute the 2 and max norm of the gradient.
 222 //
 223 // Returns true if all computations could be performed
 224 // successfully. Any failures are considered fatal and the
 225 // Solver::Summary is updated to indicate this.
 226 bool TrustRegionMinimizer::EvaluateGradientAndJacobian() {
 227   if (!evaluator_->Evaluate(x_.data(),
 228                             &x_cost_,
 229                             residuals_.data(),
 230                             gradient_.data(),
 231                             jacobian_)) {
 232     solver_summary_->message = "Residual and Jacobian evaluation failed.";
 233     solver_summary_->termination_type = FAILURE;
 234     return false;
 235   }
 236
 237   iteration_summary_.cost = x_cost_ + solver_summary_->fixed_cost;
 238
 239   if (options_.jacobi_scaling) {
 240     if (iteration_summary_.iteration == 0) {
 241       // Compute a scaling vector that is used to improve the
 242       // conditioning of the Jacobian.
 243       //
 244       // jacobian_scaling_ = diag(J'J)^{-1}
 245       jacobian_->SquaredColumnNorm(jacobian_scaling_.data());
 246       for (int i = 0; i < jacobian_->num_cols(); ++i) {
 247         // Add one to the denominator to prevent division by zero.
 248         jacobian_scaling_[i] = 1.0 / (1.0 + sqrt(jacobian_scaling_[i]));
 249       }
 250     }
 251
 252     // jacobian = jacobian * diag(J'J) ^{-1}
 253     jacobian_->ScaleColumns(jacobian_scaling_.data());
 254   }
 255
 256   // The gradient exists in the local tangent space. To account for
 257   // the bounds constraints correctly, instead of just computing the
 258   // norm of the gradient vector, we compute
 259   //
 260   // |Plus(x, -gradient) - x|
 261   //
 262   // Where the Plus operator lifts the negative gradient to the
 263   // ambient space, adds it to x and projects it on the hypercube
 264   // defined by the bounds.
 265   negative_gradient_ = -gradient_;
 266   if (!evaluator_->Plus(x_.data(),
 267                         negative_gradient_.data(),
 268                         projected_gradient_step_.data())) {
 269     solver_summary_->message =
 270         "projected_gradient_step = Plus(x, -gradient) failed.";
 271     solver_summary_->termination_type = FAILURE;
 272     return false;
 273   }
 274
 275   iteration_summary_.gradient_max_norm =
 276       (x_ - projected_gradient_step_).lpNorm<Eigen::Infinity>();
 277   iteration_summary_.gradient_norm = (x_ - projected_gradient_step_).norm();
 278   return true;
 279 }
 280
 281 // 1. Add the final timing information to the iteration summary.
 282 // 2. Run the callbacks
 283 // 3. Check for termination based on
 284 //    a. Run time
 285 //    b. Iteration count
 286 //    c. Max norm of the gradient
 287 //    d. Size of the trust region radius.
 288 //
 289 // Returns true if user did not terminate the solver and none of these
 290 // termination criterion are met.
 291 bool TrustRegionMinimizer::FinalizeIterationAndCheckIfMinimizerCanContinue() {
 292   if (iteration_summary_.step_is_successful) {
 293     ++solver_summary_->num_successful_steps;
 294     if (x_cost_ < minimum_cost_) {
 295       minimum_cost_ = x_cost_;
 296       VectorRef(parameters_, num_parameters_) = x_;
 297       iteration_summary_.step_is_nonmonotonic = false;
 298     } else {
 299       iteration_summary_.step_is_nonmonotonic = true;
 300     }
 301   } else {
 302     ++solver_summary_->num_unsuccessful_steps;
 303   }
 304
 305   iteration_summary_.trust_region_radius = strategy_->Radius();
 306   iteration_summary_.iteration_time_in_seconds =
 307       WallTimeInSeconds() - iteration_start_time_in_secs_;
 308   iteration_summary_.cumulative_time_in_seconds =
 309       WallTimeInSeconds() - start_time_in_secs_ +
 310       solver_summary_->preprocessor_time_in_seconds;
 311
 312   solver_summary_->iterations.push_back(iteration_summary_);
 313
 314   if (!RunCallbacks(options_, iteration_summary_, solver_summary_)) {
 315     return false;
 316   }
 317
 318   if (MaxSolverTimeReached()) {
 319     return false;
 320   }
 321
 322   if (MaxSolverIterationsReached()) {
 323     return false;
 324   }
 325
 326   if (GradientToleranceReached()) {
 327     return false;
 328   }
 329
 330   if (MinTrustRegionRadiusReached()) {
 331     return false;
 332   }
 333
 334   return true;
 335 }
 336
 337 // Compute the trust region step using the TrustRegionStrategy chosen
 338 // by the user.
 339 //
 340 // If the strategy returns with LINEAR_SOLVER_FATAL_ERROR, which
 341 // indicates an unrecoverable error, return false. This is the only
 342 // condition that returns false.
 343 //
 344 // If the strategy returns with LINEAR_SOLVER_FAILURE, which indicates
 345 // a numerical failure that could be recovered from by retrying
 346 // (e.g. by increasing the strength of the regularization), we set
 347 // iteration_summary_.step_is_valid to false and return true.
 348 //
 349 // In all other cases, we compute the decrease in the trust region
 350 // model problem. In exact arithmetic, this should always be
 351 // positive, but due to numerical problems in the TrustRegionStrategy
 352 // or round off error when computing the decrease it may be
 353 // negative. In which case again, we set
 354 // iteration_summary_.step_is_valid to false.
 355 bool TrustRegionMinimizer::ComputeTrustRegionStep() {
 356   const double strategy_start_time = WallTimeInSeconds();
 357   iteration_summary_.step_is_valid = false;
 358   TrustRegionStrategy::PerSolveOptions per_solve_options;
 359   per_solve_options.eta = options_.eta;
 360   if (find(options_.trust_region_minimizer_iterations_to_dump.begin(),
 361            options_.trust_region_minimizer_iterations_to_dump.end(),
 362            iteration_summary_.iteration) !=
 363       options_.trust_region_minimizer_iterations_to_dump.end()) {
 364     per_solve_options.dump_format_type =
 365         options_.trust_region_problem_dump_format_type;
 366     per_solve_options.dump_filename_base =
 367         JoinPath(options_.trust_region_problem_dump_directory,
 368                  StringPrintf("ceres_solver_iteration_%03d",
 369                               iteration_summary_.iteration));
 370   }
 371
 372   TrustRegionStrategy::Summary strategy_summary =
 373       strategy_->ComputeStep(per_solve_options,
 374                              jacobian_,
 375                              residuals_.data(),
 376                              trust_region_step_.data());
 377
 378   if (strategy_summary.termination_type == LINEAR_SOLVER_FATAL_ERROR) {
 379     solver_summary_->message =
 380         "Linear solver failed due to unrecoverable "
 381         "non-numeric causes. Please see the error log for clues. ";
 382     solver_summary_->termination_type = FAILURE;
 383     return false;
 384   }
 385
 386   iteration_summary_.step_solver_time_in_seconds =
 387       WallTimeInSeconds() - strategy_start_time;
 388   iteration_summary_.linear_solver_iterations = strategy_summary.num_iterations;
 389
 390   if (strategy_summary.termination_type == LINEAR_SOLVER_FAILURE) {
 391     return true;
 392   }
 393
 394   // new_model_cost
 395   //  = 1/2 [f + J * step]^2
 396   //  = 1/2 [ f'f + 2f'J * step + step' * J' * J * step ]
 397   // model_cost_change
 398   //  = cost - new_model_cost
 399   //  = f'f/2  - 1/2 [ f'f + 2f'J * step + step' * J' * J * step]
 400   //  = -f'J * step - step' * J' * J * step / 2
 401   //  = -(J * step)'(f + J * step / 2)
 402   model_residuals_.setZero();
 403   jacobian_->RightMultiply(trust_region_step_.data(), model_residuals_.data());
 404   model_cost_change_ =
 405       -model_residuals_.dot(residuals_ + model_residuals_ / 2.0);
 406
 407   // TODO(sameeragarwal)
 408   //
 409   //  1. What happens if model_cost_change_ = 0
 410   //  2. What happens if -epsilon <= model_cost_change_ < 0 for some
 411   //     small epsilon due to round off error.
 412   iteration_summary_.step_is_valid = (model_cost_change_ > 0.0);
 413   if (iteration_summary_.step_is_valid) {
 414     // Undo the Jacobian column scaling.
 415     delta_ = (trust_region_step_.array() * jacobian_scaling_.array()).matrix();
 416     num_consecutive_invalid_steps_ = 0;
 417   }
 418
 419   VLOG_IF(1, is_not_silent_ && !iteration_summary_.step_is_valid)
 420       << "Invalid step: current_cost: " << x_cost_
 421       << " absolute model cost change: " << model_cost_change_
 422       << " relative model cost change: " << (model_cost_change_ / x_cost_);
 423   return true;
 424 }
 425
 426 // Invalid steps can happen due to a number of reasons, and we allow a
 427 // limited number of consecutive failures, and return false if this
 428 // limit is exceeded.
 429 bool TrustRegionMinimizer::HandleInvalidStep() {
 430   // TODO(sameeragarwal): Should we be returning FAILURE or
 431   // NO_CONVERGENCE? The solution value is still usable in many cases,
 432   // it is not clear if we should declare the solver a failure
 433   // entirely. For example the case where model_cost_change ~ 0.0, but
 434   // just slightly negative.
 435   if (++num_consecutive_invalid_steps_ >=
 436       options_.max_num_consecutive_invalid_steps) {
 437     solver_summary_->message = StringPrintf(
 438         "Number of consecutive invalid steps more "
 439         "than Solver::Options::max_num_consecutive_invalid_steps: %d",
 440         options_.max_num_consecutive_invalid_steps);
 441     solver_summary_->termination_type = FAILURE;
 442     return false;
 443   }
 444
 445   strategy_->StepIsInvalid();
 446
 447   // We are going to try and reduce the trust region radius and
 448   // solve again. To do this, we are going to treat this iteration
 449   // as an unsuccessful iteration. Since the various callbacks are
 450   // still executed, we are going to fill the iteration summary
 451   // with data that assumes a step of length zero and no progress.
 452   iteration_summary_.cost = x_cost_ + solver_summary_->fixed_cost;
 453   iteration_summary_.cost_change = 0.0;
 454   iteration_summary_.gradient_max_norm =
 455       solver_summary_->iterations.back().gradient_max_norm;
 456   iteration_summary_.gradient_norm =
 457       solver_summary_->iterations.back().gradient_norm;
 458   iteration_summary_.step_norm = 0.0;
 459   iteration_summary_.relative_decrease = 0.0;
 460   iteration_summary_.eta = options_.eta;
 461   return true;
 462 }
 463
 464 // Use the supplied coordinate descent minimizer to perform inner
 465 // iterations and compute the improvement due to it. Returns the cost
 466 // after performing the inner iterations.
 467 //
 468 // The optimization is performed with candidate_x_ as the starting
 469 // point, and if the optimization is successful, candidate_x_ will be
 470 // updated with the optimized parameters.
 471 void TrustRegionMinimizer::DoInnerIterationsIfNeeded() {
 472   inner_iterations_were_useful_ = false;
 473   if (!inner_iterations_are_enabled_ ||
 474       candidate_cost_ >= std::numeric_limits<double>::max()) {
 475     return;
 476   }
 477
 478   double inner_iteration_start_time = WallTimeInSeconds();
 479   ++solver_summary_->num_inner_iteration_steps;
 480   inner_iteration_x_ = candidate_x_;
 481   Solver::Summary inner_iteration_summary;
 482   options_.inner_iteration_minimizer->Minimize(
 483       options_, inner_iteration_x_.data(), &inner_iteration_summary);
 484   double inner_iteration_cost;
 485   if (!evaluator_->Evaluate(
 486           inner_iteration_x_.data(), &inner_iteration_cost, NULL, NULL, NULL)) {
 487     VLOG_IF(2, is_not_silent_) << "Inner iteration failed.";
 488     return;
 489   }
 490
 491   VLOG_IF(2, is_not_silent_)
 492       << "Inner iteration succeeded; Current cost: " << x_cost_
 493       << " Trust region step cost: " << candidate_cost_
 494       << " Inner iteration cost: " << inner_iteration_cost;
 495
 496   candidate_x_ = inner_iteration_x_;
 497
 498   // Normally, the quality of a trust region step is measured by
 499   // the ratio
 500   //
 501   //              cost_change
 502   //    r =    -----------------
 503   //           model_cost_change
 504   //
 505   // All the change in the nonlinear objective is due to the trust
 506   // region step so this ratio is a good measure of the quality of
 507   // the trust region radius. However, when inner iterations are
 508   // being used, cost_change includes the contribution of the
 509   // inner iterations and its not fair to credit it all to the
 510   // trust region algorithm. So we change the ratio to be
 511   //
 512   //                              cost_change
 513   //    r =    ------------------------------------------------
 514   //           (model_cost_change + inner_iteration_cost_change)
 515   //
 516   // Practically we do this by increasing model_cost_change by
 517   // inner_iteration_cost_change.
 518
 519   const double inner_iteration_cost_change =
 520       candidate_cost_ - inner_iteration_cost;
 521   model_cost_change_ += inner_iteration_cost_change;
 522   inner_iterations_were_useful_ = inner_iteration_cost < x_cost_;
 523   const double inner_iteration_relative_progress =
 524       1.0 - inner_iteration_cost / candidate_cost_;
 525
 526   // Disable inner iterations once the relative improvement
 527   // drops below tolerance.
 528   inner_iterations_are_enabled_ =
 529       (inner_iteration_relative_progress > options_.inner_iteration_tolerance);
 530   VLOG_IF(2, is_not_silent_ && !inner_iterations_are_enabled_)
 531       << "Disabling inner iterations. Progress : "
 532       << inner_iteration_relative_progress;
 533   candidate_cost_ = inner_iteration_cost;
 534
 535   solver_summary_->inner_iteration_time_in_seconds +=
 536       WallTimeInSeconds() - inner_iteration_start_time;
 537 }
 538
 539 // Perform a projected line search to improve the objective function
 540 // value along delta.
 541 //
 542 // TODO(sameeragarwal): The current implementation does not do
 543 // anything illegal but is incorrect and not terribly effective.
 544 //
 545 // https://github.com/ceres-solver/ceres-solver/issues/187
 546 void TrustRegionMinimizer::DoLineSearch(const Vector& x,
 547                                         const Vector& gradient,
 548                                         const double cost,
 549                                         Vector* delta) {
 550   LineSearchFunction line_search_function(evaluator_);
 551
 552   LineSearch::Options line_search_options;
 553   line_search_options.is_silent = true;
 554   line_search_options.interpolation_type =
 555       options_.line_search_interpolation_type;
 556   line_search_options.min_step_size = options_.min_line_search_step_size;
 557   line_search_options.sufficient_decrease =
 558       options_.line_search_sufficient_function_decrease;
 559   line_search_options.max_step_contraction =
 560       options_.max_line_search_step_contraction;
 561   line_search_options.min_step_contraction =
 562       options_.min_line_search_step_contraction;
 563   line_search_options.max_num_iterations =
 564       options_.max_num_line_search_step_size_iterations;
 565   line_search_options.sufficient_curvature_decrease =
 566       options_.line_search_sufficient_curvature_decrease;
 567   line_search_options.max_step_expansion =
 568       options_.max_line_search_step_expansion;
 569   line_search_options.function = &line_search_function;
 570
 571   std::string message;
 572   scoped_ptr<LineSearch> line_search(CHECK_NOTNULL(
 573       LineSearch::Create(ceres::ARMIJO, line_search_options, &message)));
 574   LineSearch::Summary line_search_summary;
 575   line_search_function.Init(x, *delta);
 576   line_search->Search(1.0, cost, gradient.dot(*delta), &line_search_summary);
 577
 578   solver_summary_->num_line_search_steps += line_search_summary.num_iterations;
 579   solver_summary_->line_search_cost_evaluation_time_in_seconds +=
 580       line_search_summary.cost_evaluation_time_in_seconds;
 581   solver_summary_->line_search_gradient_evaluation_time_in_seconds +=
 582       line_search_summary.gradient_evaluation_time_in_seconds;
 583   solver_summary_->line_search_polynomial_minimization_time_in_seconds +=
 584       line_search_summary.polynomial_minimization_time_in_seconds;
 585   solver_summary_->line_search_total_time_in_seconds +=
 586       line_search_summary.total_time_in_seconds;
 587
 588   if (line_search_summary.success) {
 589     *delta *= line_search_summary.optimal_point.x;
 590   }
 591 }
 592
 593 // Check if the maximum amount of time allowed by the user for the
 594 // solver has been exceeded, and if so return false after updating
 595 // Solver::Summary::message.
 596 bool TrustRegionMinimizer::MaxSolverTimeReached() {
 597   const double total_solver_time =
 598       WallTimeInSeconds() - start_time_in_secs_ +
 599       solver_summary_->preprocessor_time_in_seconds;
 600   if (total_solver_time < options_.max_solver_time_in_seconds) {
 601     return false;
 602   }
 603
 604   solver_summary_->message = StringPrintf("Maximum solver time reached. "
 605                                           "Total solver time: %e >= %e.",
 606                                           total_solver_time,
 607                                           options_.max_solver_time_in_seconds);
 608   solver_summary_->termination_type = NO_CONVERGENCE;
 609   VLOG_IF(1, is_not_silent_) << "Terminating: " << solver_summary_->message;
 610   return true;
 611 }
 612
 613 // Check if the maximum number of iterations allowed by the user for
 614 // the solver has been exceeded, and if so return false after updating
 615 // Solver::Summary::message.
 616 bool TrustRegionMinimizer::MaxSolverIterationsReached() {
 617   if (iteration_summary_.iteration < options_.max_num_iterations) {
 618     return false;
 619   }
 620
 621   solver_summary_->message =
 622       StringPrintf("Maximum number of iterations reached. "
 623                    "Number of iterations: %d.",
 624                    iteration_summary_.iteration);
 625
 626   solver_summary_->termination_type = NO_CONVERGENCE;
 627   VLOG_IF(1, is_not_silent_) << "Terminating: " << solver_summary_->message;
 628   return true;
 629 }
 630
 631 // Check convergence based on the max norm of the gradient (only for
 632 // iterations where the step was declared successful).
 633 bool TrustRegionMinimizer::GradientToleranceReached() {
 634   if (!iteration_summary_.step_is_successful ||
 635       iteration_summary_.gradient_max_norm > options_.gradient_tolerance) {
 636     return false;
 637   }
 638
 639   solver_summary_->message = StringPrintf(
 640       "Gradient tolerance reached. "
 641       "Gradient max norm: %e <= %e",
 642       iteration_summary_.gradient_max_norm,
 643       options_.gradient_tolerance);
 644   solver_summary_->termination_type = CONVERGENCE;
 645   VLOG_IF(1, is_not_silent_) << "Terminating: " << solver_summary_->message;
 646   return true;
 647 }
 648
 649 // Check convergence based the size of the trust region radius.
 650 bool TrustRegionMinimizer::MinTrustRegionRadiusReached() {
 651   if (iteration_summary_.trust_region_radius >
 652       options_.min_trust_region_radius) {
 653     return false;
 654   }
 655
 656   solver_summary_->message =
 657       StringPrintf("Minimum trust region radius reached. "
 658                    "Trust region radius: %e <= %e",
 659                    iteration_summary_.trust_region_radius,
 660                    options_.min_trust_region_radius);
 661   solver_summary_->termination_type = CONVERGENCE;
 662   VLOG_IF(1, is_not_silent_) << "Terminating: " << solver_summary_->message;
 663   return true;
 664 }
 665
 666 // Solver::Options::parameter_tolerance based convergence check.
 667 bool TrustRegionMinimizer::ParameterToleranceReached() {
 668   // Compute the norm of the step in the ambient space.
 669   iteration_summary_.step_norm = (x_ - candidate_x_).norm();
 670   const double step_size_tolerance =
 671       options_.parameter_tolerance * (x_norm_ + options_.parameter_tolerance);
 672
 673   if (iteration_summary_.step_norm > step_size_tolerance) {
 674     return false;
 675   }
 676
 677   solver_summary_->message = StringPrintf(
 678       "Parameter tolerance reached. "
 679       "Relative step_norm: %e <= %e.",
 680       (iteration_summary_.step_norm / (x_norm_ + options_.parameter_tolerance)),
 681       options_.parameter_tolerance);
 682   solver_summary_->termination_type = CONVERGENCE;
 683   VLOG_IF(1, is_not_silent_) << "Terminating: " << solver_summary_->message;
 684   return true;
 685 }
 686
 687 // Solver::Options::function_tolerance based convergence check.
 688 bool TrustRegionMinimizer::FunctionToleranceReached() {
 689   iteration_summary_.cost_change = x_cost_ - candidate_cost_;
 690   const double absolute_function_tolerance =
 691       options_.function_tolerance * x_cost_;
 692
 693   if (fabs(iteration_summary_.cost_change) > absolute_function_tolerance) {
 694     return false;
 695   }
 696
 697   solver_summary_->message = StringPrintf(
 698       "Function tolerance reached. "
 699       "|cost_change|/cost: %e <= %e",
 700       fabs(iteration_summary_.cost_change) / x_cost_,
 701       options_.function_tolerance);
 702   solver_summary_->termination_type = CONVERGENCE;
 703   VLOG_IF(1, is_not_silent_) << "Terminating: " << solver_summary_->message;
 704   return true;
 705 }
 706
 707 // Compute candidate_x_ = Plus(x_, delta_)
 708 // Evaluate the cost of candidate_x_ as candidate_cost_.
 709 //
 710 // Failure to compute the step or the cost mean that candidate_cost_
 711 // is set to std::numeric_limits<double>::max(). Unlike
 712 // EvaluateGradientAndJacobian, failure in this function is not fatal
 713 // as we are only computing and evaluating a candidate point, and if
 714 // for some reason we are unable to evaluate it, we consider it to be
 715 // a point with very high cost. This allows the user to deal with edge
 716 // cases/constraints as part of the LocalParameterization and
 717 // CostFunction objects.
 718 void TrustRegionMinimizer::ComputeCandidatePointAndEvaluateCost() {
 719   if (!evaluator_->Plus(x_.data(), delta_.data(), candidate_x_.data())) {
 720     LOG_IF(WARNING, is_not_silent_)
 721         << "x_plus_delta = Plus(x, delta) failed. "
 722         << "Treating it as a step with infinite cost";
 723     candidate_cost_ = std::numeric_limits<double>::max();
 724     return;
 725   }
 726
 727   if (!evaluator_->Evaluate(
 728           candidate_x_.data(), &candidate_cost_, NULL, NULL, NULL)) {
 729     LOG_IF(WARNING, is_not_silent_)
 730         << "Step failed to evaluate. "
 731         << "Treating it as a step with infinite cost";
 732     candidate_cost_ = std::numeric_limits<double>::max();
 733   }
 734 }
 735
 736 bool TrustRegionMinimizer::IsStepSuccessful() {
 737   iteration_summary_.relative_decrease =
 738       step_evaluator_->StepQuality(candidate_cost_, model_cost_change_);
 739
 740   // In most cases, boosting the model_cost_change by the
 741   // improvement caused by the inner iterations is fine, but it can
 742   // be the case that the original trust region step was so bad that
 743   // the resulting improvement in the cost was negative, and the
 744   // change caused by the inner iterations was large enough to
 745   // improve the step, but also to make relative decrease quite
 746   // small.
 747   //
 748   // This can cause the trust region loop to reject this step. To
 749   // get around this, we expicitly check if the inner iterations
 750   // led to a net decrease in the objective function value. If
 751   // they did, we accept the step even if the trust region ratio
 752   // is small.
 753   //
 754   // Notice that we do not just check that cost_change is positive
 755   // which is a weaker condition and would render the
 756   // min_relative_decrease threshold useless. Instead, we keep
 757   // track of inner_iterations_were_useful, which is true only
 758   // when inner iterations lead to a net decrease in the cost.
 759   return (inner_iterations_were_useful_ ||
 760           iteration_summary_.relative_decrease >
 761               options_.min_relative_decrease);
 762 }
 763
 764 // Declare the step successful, move to candidate_x, update the
 765 // derivatives and let the trust region strategy and the step
 766 // evaluator know that the step has been accepted.
 767 bool TrustRegionMinimizer::HandleSuccessfulStep() {
 768   x_ = candidate_x_;
 769   x_norm_ = x_.norm();
 770
 771   if (!EvaluateGradientAndJacobian()) {
 772     return false;
 773   }
 774
 775   iteration_summary_.step_is_successful = true;
 776   strategy_->StepAccepted(iteration_summary_.relative_decrease);
 777   step_evaluator_->StepAccepted(candidate_cost_, model_cost_change_);
 778   return true;
 779 }
 780
 781 // Declare the step unsuccessful and inform the trust region strategy.
 782 void TrustRegionMinimizer::HandleUnsuccessfulStep() {
 783   iteration_summary_.step_is_successful = false;
 784   strategy_->StepRejected(iteration_summary_.relative_decrease);
 785   iteration_summary_.cost = candidate_cost_ + solver_summary_->fixed_cost;
 786 }
 787
 788 }  // namespace internal
 789 }  // namespace ceres