internal/ceres/trust_region_minimizer.cc - ceres-solver - Git at Google

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

 // 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 initial_gradient_max_norm =
       max(iteration_summary.gradient_max_norm, kEpsilon);
   const double absolute_gradient_tolerance =
       options_.gradient_tolerance * initial_gradient_max_norm;

   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 / initial_gradient_max_norm
             << " <= " << 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);

    int num_consecutive_invalid_steps = 0;
   while (true) {
     if (!RunCallbacks(options.callbacks, iteration_summary, summary)) {
       return;
     }

     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;
             // Boost 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 / initial_gradient_max_norm
                 << " <= " << 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);
   }
 }


 }  // namespace internal
 }  // namespace ceres
	// 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

	// 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 initial_gradient_max_norm =
	max(iteration_summary.gradient_max_norm, kEpsilon);
	const double absolute_gradient_tolerance =
	options_.gradient_tolerance * initial_gradient_max_norm;

	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 / initial_gradient_max_norm
	<< " <= " << 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);

	int num_consecutive_invalid_steps = 0;
	while (true) {
	if (!RunCallbacks(options.callbacks, iteration_summary, summary)) {
	return;
	}

	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;
	// Boost 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 / initial_gradient_max_norm
	<< " <= " << 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);
	}
	}


	} // namespace internal
	} // namespace ceres