diff --git a/internal/ceres/trust_region_minimizer.cc b/internal/ceres/trust_region_minimizer.cc index 7065977ad..196f9e623 100644 --- a/internal/ceres/trust_region_minimizer.cc +++ b/internal/ceres/trust_region_minimizer.cc @@ -83,6 +83,10 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, while (FinalizeIterationAndCheckIfMinimizerCanContinue()) { iteration_start_time_in_secs_ = WallTimeInSeconds(); + + const double previous_gradient_norm = iteration_summary_.gradient_norm; + const double previous_gradient_max_norm = iteration_summary_.gradient_max_norm; + iteration_summary_ = IterationSummary(); iteration_summary_.iteration = solver_summary->iterations.back().iteration + 1; @@ -111,12 +115,22 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, return; } + + if (IsStepSuccessful()) { RETURN_IF_ERROR_AND_LOG(HandleSuccessfulStep()); - continue; - } + } else { + // Declare the step unsuccessful and inform the trust region strategy. + iteration_summary_.step_is_successful = false; + iteration_summary_.cost = candidate_cost_ + solver_summary_->fixed_cost; - HandleUnsuccessfulStep(); + // When the step is unsuccessful, we do not compute the gradient + // (or update x), so we preserve its value from the last + // successful iteration. + iteration_summary_.gradient_norm = previous_gradient_norm; + iteration_summary_.gradient_max_norm = previous_gradient_max_norm; + strategy_->StepRejected(iteration_summary_.relative_decrease); + } } } @@ -793,12 +807,5 @@ bool TrustRegionMinimizer::HandleSuccessfulStep() { return true; } -// Declare the step unsuccessful and inform the trust region strategy. -void TrustRegionMinimizer::HandleUnsuccessfulStep() { - iteration_summary_.step_is_successful = false; - strategy_->StepRejected(iteration_summary_.relative_decrease); - iteration_summary_.cost = candidate_cost_ + solver_summary_->fixed_cost; -} - } // namespace internal } // namespace ceres diff --git a/internal/ceres/trust_region_minimizer.h b/internal/ceres/trust_region_minimizer.h index e18193c92..27a1b7848 100644 --- a/internal/ceres/trust_region_minimizer.h +++ b/internal/ceres/trust_region_minimizer.h @@ -81,7 +81,6 @@ class TrustRegionMinimizer : public Minimizer { bool MinTrustRegionRadiusReached(); bool IsStepSuccessful(); - void HandleUnsuccessfulStep(); bool HandleSuccessfulStep(); bool HandleInvalidStep();