Ceres: Update to the latest actual version
[blender.git] / extern / ceres / 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_step_size;
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