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 <string>
 #include <vector>
 #include <glog/logging.h>

 #include "Eigen/Core"
 #include "ceres/array_utils.h"
 #include "ceres/evaluator.h"
 #include "ceres/linear_least_squares_problems.h"
 #include "ceres/internal/eigen.h"
 #include "ceres/internal/scoped_ptr.h"
 #include "ceres/sparse_matrix.h"
 #include "ceres/trust_region_strategy.h"
 #include "ceres/types.h"

 namespace ceres {
 namespace internal {
 namespace {
 // Small constant for various floating point issues.
 const double kEpsilon = 1e-12;
 }  // namespace

 // Execute the list of IterationCallbacks sequentially. If any one of
 // the callbacks does not return SOLVER_CONTINUE, then stop and return
 // its status.
 CallbackReturnType TrustRegionMinimizer::RunCallbacks(
     const Minimizer::Options& options_,
     const IterationSummary& iteration_summary) {
   for (int i = 0; i < options_.callbacks.size(); ++i) {
     const CallbackReturnType status =
         (*options_.callbacks[i])(iteration_summary);
     if (status != SOLVER_CONTINUE) {
       return status;
     }
   }
   return SOLVER_CONTINUE;
 }

 // Compute a scaling vector that is used to improve the conditioning
 // of the Jacobian.
 void TrustRegionMinimizer::EstimateScale(const SparseMatrix& jacobian,
                                          double* scale) const {
   jacobian.SquaredColumnNorm(scale);
   for (int i = 0; i < jacobian.num_cols(); ++i) {
     scale[i] = 1.0 / (kEpsilon + sqrt(scale[i]));
   }
 }

 void TrustRegionMinimizer::Init(const Minimizer::Options& options) {
   options_ = options;
   sort(options_.lsqp_iterations_to_dump.begin(),
        options_.lsqp_iterations_to_dump.end());
 }

 bool TrustRegionMinimizer::MaybeDumpLinearLeastSquaresProblem(
     const int iteration,
     const SparseMatrix* jacobian,
     const double* residuals,
     const double* step) const  {
   // TODO(sameeragarwal): Since the use of trust_region_radius has
   // moved inside TrustRegionStrategy, its not clear how we dump the
   // regularization vector/matrix anymore.
   //
   // Doing this right requires either an API change to the
   // TrustRegionStrategy and/or how LinearLeastSquares problems are
   // stored on disk.
   //
   // For now, we will just not dump the regularizer.
   return (!binary_search(options_.lsqp_iterations_to_dump.begin(),
                          options_.lsqp_iterations_to_dump.end(),
                          iteration) ||
           DumpLinearLeastSquaresProblem(options_.lsqp_dump_directory,
                                         iteration,
                                         options_.lsqp_dump_format_type,
                                         jacobian,
                                         NULL,
                                         residuals,
                                         step,
                                         options_.num_eliminate_blocks));
 }

 void TrustRegionMinimizer::Minimize(const Minimizer::Options& options,
                                     double* parameters,
                                     Solver::Summary* summary) {
   time_t start_time = time(NULL);
   time_t iteration_start_time =  start_time;
   Init(options);

   summary->termination_type = NO_CONVERGENCE;
   summary->num_successful_steps = 0;
   summary->num_unsuccessful_steps = 0;

   Evaluator* evaluator = CHECK_NOTNULL(options_.evaluator);
   SparseMatrix* jacobian = CHECK_NOTNULL(options_.jacobian);
   TrustRegionStrategy* strategy = CHECK_NOTNULL(options_.trust_region_strategy);

   const int num_parameters = evaluator->NumParameters();
   const int num_effective_parameters = evaluator->NumEffectiveParameters();
   const int num_residuals = evaluator->NumResiduals();

   VectorRef x(parameters, num_parameters);
   double x_norm = x.norm();

   Vector residuals(num_residuals);
   Vector trust_region_step(num_effective_parameters);
   Vector delta(num_effective_parameters);
   Vector x_plus_delta(num_parameters);
   Vector gradient(num_effective_parameters);
   Vector model_residuals(num_residuals);
   Vector scale(num_effective_parameters);

   IterationSummary iteration_summary;
   iteration_summary.iteration = 0;
   iteration_summary.step_is_valid=false;
   iteration_summary.step_is_successful=false;
   iteration_summary.cost = summary->initial_cost;
   iteration_summary.cost_change = 0.0;
   iteration_summary.gradient_max_norm = 0.0;
   iteration_summary.step_norm = 0.0;
   iteration_summary.relative_decrease = 0.0;
   iteration_summary.trust_region_radius = strategy->Radius();
   // TODO(sameeragarwal): Rename eta to linear_solver_accuracy or
   // something similar across the board.
   iteration_summary.eta = options_.eta;
   iteration_summary.linear_solver_iterations = 0;
   iteration_summary.step_solver_time_sec = 0;

   // Do initial cost and Jacobian evaluation.
   double cost = 0.0;
   if (!evaluator->Evaluate(x.data(), &cost, residuals.data(), jacobian)) {
     LOG(WARNING) << "Terminating: Residual and Jacobian evaluation failed.";
     summary->termination_type = NUMERICAL_FAILURE;
     return;
   }

   // Compute the fixed part of the cost.
   //
   // This is a poor way to do this computation. Even if fixed_cost is
   // zero, because we are subtracting two possibly large numbers, we
   // are depending on exact cancellation to give us a zero here. But
   // initial_cost and cost have been computed by two different
   // evaluators. One which runs on the whole problem (in
   // solver_impl.cc) in single threaded mode and another which runs
   // here on the reduced problem, so fixed_cost can (and does) contain
   // some numerical garbage with a relative magnitude of 1e-14.
   //
   // The right way to do this, would be to compute the fixed cost on
   // just the set of residual blocks which are held constant and were
   // removed from the original problem when the reduced problem was
   // constructed.
   summary->fixed_cost = summary->initial_cost - cost;

   gradient.setZero();
   jacobian->LeftMultiply(residuals.data(), gradient.data());
   iteration_summary.gradient_max_norm = gradient.lpNorm<Eigen::Infinity>();

   if (options_.jacobi_scaling) {
     EstimateScale(*jacobian, scale.data());
     jacobian->ScaleColumns(scale.data());
   } else {
     scale.setOnes();
   }

   const double absolute_gradient_tolerance =
       options_.gradient_tolerance *
       max(iteration_summary.gradient_max_norm, kEpsilon);
   if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) {
     summary->termination_type = GRADIENT_TOLERANCE;
     VLOG(1) << "Terminating: Gradient tolerance reached."
             << "Relative gradient max norm: "
             << iteration_summary.gradient_max_norm /
         summary->iterations[0].gradient_max_norm
             << " <= " << options_.gradient_tolerance;
     return;
   }

   // Call the various callbacks.
   iteration_summary.iteration_time_sec = time(NULL) - iteration_start_time;
   summary->iterations.push_back(iteration_summary);
   switch (RunCallbacks(options_, iteration_summary)) {
     case SOLVER_TERMINATE_SUCCESSFULLY:
       summary->termination_type = USER_SUCCESS;
       VLOG(1) << "Terminating: User callback returned USER_SUCCESS.";
       return;
     case SOLVER_ABORT:
       summary->termination_type = USER_ABORT;
       VLOG(1) << "Terminating: User callback returned  USER_ABORT.";
       return;
     case SOLVER_CONTINUE:
       break;
     default:
       LOG(FATAL) << "Unknown type of user callback status";
   }

   int num_consecutive_invalid_steps = 0;
   while (true) {
     iteration_start_time = time(NULL);
     if (iteration_summary.iteration >= options_.max_num_iterations) {
       summary->termination_type = NO_CONVERGENCE;
       VLOG(1) << "Terminating: Maximum number of iterations reached.";
       break;
     }

     if ((start_time - iteration_start_time) >= options_.max_solver_time_sec) {
       summary->termination_type = NO_CONVERGENCE;
       VLOG(1) << "Terminating: Maximum solver time reached.";
       break;
     }

     iteration_summary = IterationSummary();
     iteration_summary = summary->iterations.back();
     iteration_summary.iteration = summary->iterations.back().iteration + 1;
     iteration_summary.step_is_valid = false;
     iteration_summary.step_is_successful = false;

     const time_t strategy_start_time = time(NULL);
     TrustRegionStrategy::PerSolveOptions per_solve_options;
     per_solve_options.eta = options_.eta;
     LinearSolver::Summary strategy_summary =
         strategy->ComputeStep(per_solve_options,
                               jacobian,
                               residuals.data(),
                               trust_region_step.data());

     iteration_summary.step_solver_time_sec = time(NULL) - strategy_start_time;
     iteration_summary.linear_solver_iterations =
         strategy_summary.num_iterations;

     if (!MaybeDumpLinearLeastSquaresProblem(iteration_summary.iteration,
                                             jacobian,
                                             residuals.data(),
                                             trust_region_step.data())) {
       LOG(FATAL) << "Tried writing linear least squares problem: "
                  << options.lsqp_dump_directory << "but failed.";
     }

     double new_model_cost = 0.0;
     if (strategy_summary.termination_type != FAILURE) {
       // new_model_cost = 1/2 |J * step - f|^2
       model_residuals = -residuals;
       jacobian->RightMultiply(trust_region_step.data(), model_residuals.data());
       new_model_cost = model_residuals.squaredNorm() / 2.0;

       // In exact arithmetic, this would never be the case. But poorly
       // conditioned matrices can give rise to situations where the
       // new_model_cost can actually be larger than half the squared
       // norm of the residual vector. We allow for small tolerance
       // around cost and beyond that declare the step to be invalid.
       if (cost < (new_model_cost - kEpsilon)) {
         VLOG(1) << "Invalid step: current_cost: " << cost
                 << " new_model_cost " << new_model_cost;
       } else {
         iteration_summary.step_is_valid = true;
       }
     }

     if (!iteration_summary.step_is_valid) {
       // Invalid steps can happen due to a number of reasons, and we
       // allow a limited number of successive failures, and return with
       // NUMERICAL_FAILURE if this limit is exceeded.
       if (++num_consecutive_invalid_steps >=
           options_.max_num_consecutive_invalid_steps) {
         summary->termination_type = NUMERICAL_FAILURE;
         LOG(WARNING) << "Terminating. Number of successive invalid steps more "
                      << "than "
                      << "Solver::Options::max_num_consecutive_invalid_steps: "
                      << options_.max_num_consecutive_invalid_steps;
         return;
       }

       // We are going to try and reduce the trust region radius and
       // solve again. To do this, we are going to treat this iteration
       // as an unsuccessful iteration. Since the various callbacks are
       // still executed, we are going to fill the iteration summary
       // with data that assumes a step of length zero and no progress.
       iteration_summary.cost = cost;
       iteration_summary.cost_change = 0.0;
       iteration_summary.gradient_max_norm =
           summary->iterations.back().gradient_max_norm;
       iteration_summary.step_norm = 0.0;
       iteration_summary.relative_decrease = 0.0;
       iteration_summary.eta = options_.eta;
     } else {
       // The step is numerically valid, so now we can judge its quality.
       num_consecutive_invalid_steps = 0;

       // We allow some slop around 0, and clamp the model_cost_change
       // at kEpsilon from below.
       //
       // There is probably a better way to do this, as it is going to
       // create problems for problems where the objective function is
       // kEpsilon close to zero.
       const double model_cost_change = max(kEpsilon, cost - new_model_cost);

       // Undo the Jacobian column scaling.
       delta =  -(trust_region_step.array() * scale.array()).matrix();
       iteration_summary.step_norm = delta.norm();

       // Convergence based on parameter_tolerance.
       const double step_size_tolerance =  options_.parameter_tolerance *
           (x_norm + options_.parameter_tolerance);
       if (iteration_summary.step_norm <= step_size_tolerance) {
         VLOG(1) << "Terminating. Parameter tolerance reached. "
                 << "relative step_norm: "
                 << iteration_summary.step_norm /
             (x_norm + options_.parameter_tolerance)
                 << " <= " << options_.parameter_tolerance;
         summary->termination_type = PARAMETER_TOLERANCE;
         return;
       }

       if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) {
         summary->termination_type = NUMERICAL_FAILURE;
         LOG(WARNING) << "Terminating. Failed to compute "
                      << "Plus(x, delta, x_plus_delta).";
         return;
       }

       // Try this step.
       double new_cost;
       if (!evaluator->Evaluate(x_plus_delta.data(), &new_cost, NULL, NULL)) {
         summary->termination_type = NUMERICAL_FAILURE;
         LOG(WARNING) << "Terminating: Cost evaluation failed.";
         return;
       }

       VLOG(2) << "old cost: " << cost << " new cost: " << new_cost;
       iteration_summary.cost_change =  cost - new_cost;
       const double absolute_function_tolerance =
           options_.function_tolerance * cost;
       if (fabs(iteration_summary.cost_change) < absolute_function_tolerance) {
         VLOG(1) << "Terminating. Function tolerance reached. "
                 << "|cost_change|/cost: " << fabs(iteration_summary.cost_change) / cost
                 << " <= " << options_.function_tolerance;
         summary->termination_type = FUNCTION_TOLERANCE;
         return;
       }

       iteration_summary.relative_decrease =
           iteration_summary.cost_change / model_cost_change;
       iteration_summary.step_is_successful =
           iteration_summary.relative_decrease > options_.min_relative_decrease;
     }

     if (iteration_summary.step_is_successful) {
       ++summary->num_successful_steps;
       strategy->StepAccepted(iteration_summary.relative_decrease);
       x = x_plus_delta;
       x_norm = x.norm();
       // Step looks good, evaluate the residuals and Jacobian at this
       // point.
       if (!evaluator->Evaluate(x.data(), &cost, residuals.data(), jacobian)) {
         summary->termination_type = NUMERICAL_FAILURE;
         LOG(WARNING) << "Terminating: Residual and Jacobian evaluation failed.";
         return;
       }

       gradient.setZero();
       jacobian->LeftMultiply(residuals.data(), gradient.data());
       iteration_summary.gradient_max_norm = gradient.lpNorm<Eigen::Infinity>();

       if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) {
         summary->termination_type = GRADIENT_TOLERANCE;
         VLOG(1) << "Terminating: Gradient tolerance reached."
                 << "Relative gradient max norm: "
                 << iteration_summary.gradient_max_norm /
             summary->iterations[0].gradient_max_norm
                 << " <= " << options_.gradient_tolerance;
         return;
       }

       if (options_.jacobi_scaling) {
         jacobian->ScaleColumns(scale.data());
       }
     } else {
       ++summary->num_unsuccessful_steps;
       strategy->StepRejected(iteration_summary.relative_decrease);
     }

     iteration_summary.cost = cost + summary->fixed_cost;
     iteration_summary.trust_region_radius = strategy->Radius();
     if (iteration_summary.trust_region_radius <
         options_.min_trust_region_radius) {
       summary->termination_type = PARAMETER_TOLERANCE;
       VLOG(1) << "Termination. Minimum trust region radius reached.";
       return;
     }

     iteration_summary.iteration_time_sec = time(NULL) - iteration_start_time;
     summary->iterations.push_back(iteration_summary);
     switch (RunCallbacks(options_, iteration_summary)) {
       case SOLVER_TERMINATE_SUCCESSFULLY:
         summary->termination_type = USER_SUCCESS;
         VLOG(1) << "Terminating: User callback returned USER_SUCCESS.";
         return;
       case SOLVER_ABORT:
         summary->termination_type = USER_ABORT;
         VLOG(1) << "Terminating: User callback returned  USER_ABORT.";
         return;
       case SOLVER_CONTINUE:
         break;
       default:
         LOG(FATAL) << "Unknown type of user callback status";
     }
   }
 }


 }  // namespace internal
 }  // namespace ceres
	// Ceres Solver - A fast non-linear least squares minimizer
	// Copyright 2012 Google Inc. All rights reserved.
	// http://code.google.com/p/ceres-solver/
	//
	// Redistribution and use in source and binary forms, with or without
	// modification, are permitted provided that the following conditions are met:
	//
	// * Redistributions of source code must retain the above copyright notice,
	// this list of conditions and the following disclaimer.
	// * Redistributions in binary form must reproduce the above copyright notice,
	// this list of conditions and the following disclaimer in the documentation
	// and/or other materials provided with the distribution.
	// * Neither the name of Google Inc. nor the names of its contributors may be
	// used to endorse or promote products derived from this software without
	// specific prior written permission.
	//
	// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
	// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
	// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
	// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
	// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
	// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
	// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
	// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
	// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
	// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
	// POSSIBILITY OF SUCH DAMAGE.
	//
	// Author: sameeragarwal@google.com (Sameer Agarwal)

	#include "ceres/trust_region_minimizer.h"

	#include <algorithm>
	#include <cstdlib>
	#include <cmath>
	#include <cstring>
	#include <string>
	#include <vector>
	#include <glog/logging.h>

	#include "Eigen/Core"
	#include "ceres/array_utils.h"
	#include "ceres/evaluator.h"
	#include "ceres/linear_least_squares_problems.h"
	#include "ceres/internal/eigen.h"
	#include "ceres/internal/scoped_ptr.h"
	#include "ceres/sparse_matrix.h"
	#include "ceres/trust_region_strategy.h"
	#include "ceres/types.h"

	namespace ceres {
	namespace internal {
	namespace {
	// Small constant for various floating point issues.
	const double kEpsilon = 1e-12;
	} // namespace

	// Execute the list of IterationCallbacks sequentially. If any one of
	// the callbacks does not return SOLVER_CONTINUE, then stop and return
	// its status.
	CallbackReturnType TrustRegionMinimizer::RunCallbacks(
	const Minimizer::Options& options_,
	const IterationSummary& iteration_summary) {
	for (int i = 0; i < options_.callbacks.size(); ++i) {
	const CallbackReturnType status =
	(*options_.callbacks[i])(iteration_summary);
	if (status != SOLVER_CONTINUE) {
	return status;
	}
	}
	return SOLVER_CONTINUE;
	}

	// Compute a scaling vector that is used to improve the conditioning
	// of the Jacobian.
	void TrustRegionMinimizer::EstimateScale(const SparseMatrix& jacobian,
	double* scale) const {
	jacobian.SquaredColumnNorm(scale);
	for (int i = 0; i < jacobian.num_cols(); ++i) {
	scale[i] = 1.0 / (kEpsilon + sqrt(scale[i]));
	}
	}

	void TrustRegionMinimizer::Init(const Minimizer::Options& options) {
	options_ = options;
	sort(options_.lsqp_iterations_to_dump.begin(),
	options_.lsqp_iterations_to_dump.end());
	}

	bool TrustRegionMinimizer::MaybeDumpLinearLeastSquaresProblem(
	const int iteration,
	const SparseMatrix* jacobian,
	const double* residuals,
	const double* step) const {
	// TODO(sameeragarwal): Since the use of trust_region_radius has
	// moved inside TrustRegionStrategy, its not clear how we dump the
	// regularization vector/matrix anymore.
	//
	// Doing this right requires either an API change to the
	// TrustRegionStrategy and/or how LinearLeastSquares problems are
	// stored on disk.
	//
	// For now, we will just not dump the regularizer.
	return (!binary_search(options_.lsqp_iterations_to_dump.begin(),
	options_.lsqp_iterations_to_dump.end(),
	iteration) \|\|
	DumpLinearLeastSquaresProblem(options_.lsqp_dump_directory,
	iteration,
	options_.lsqp_dump_format_type,
	jacobian,
	NULL,
	residuals,
	step,
	options_.num_eliminate_blocks));
	}

	void TrustRegionMinimizer::Minimize(const Minimizer::Options& options,
	double* parameters,
	Solver::Summary* summary) {
	time_t start_time = time(NULL);
	time_t iteration_start_time = start_time;
	Init(options);

	summary->termination_type = NO_CONVERGENCE;
	summary->num_successful_steps = 0;
	summary->num_unsuccessful_steps = 0;

	Evaluator* evaluator = CHECK_NOTNULL(options_.evaluator);
	SparseMatrix* jacobian = CHECK_NOTNULL(options_.jacobian);
	TrustRegionStrategy* strategy = CHECK_NOTNULL(options_.trust_region_strategy);

	const int num_parameters = evaluator->NumParameters();
	const int num_effective_parameters = evaluator->NumEffectiveParameters();
	const int num_residuals = evaluator->NumResiduals();

	VectorRef x(parameters, num_parameters);
	double x_norm = x.norm();

	Vector residuals(num_residuals);
	Vector trust_region_step(num_effective_parameters);
	Vector delta(num_effective_parameters);
	Vector x_plus_delta(num_parameters);
	Vector gradient(num_effective_parameters);
	Vector model_residuals(num_residuals);
	Vector scale(num_effective_parameters);

	IterationSummary iteration_summary;
	iteration_summary.iteration = 0;
	iteration_summary.step_is_valid=false;
	iteration_summary.step_is_successful=false;
	iteration_summary.cost = summary->initial_cost;
	iteration_summary.cost_change = 0.0;
	iteration_summary.gradient_max_norm = 0.0;
	iteration_summary.step_norm = 0.0;
	iteration_summary.relative_decrease = 0.0;
	iteration_summary.trust_region_radius = strategy->Radius();
	// TODO(sameeragarwal): Rename eta to linear_solver_accuracy or
	// something similar across the board.
	iteration_summary.eta = options_.eta;
	iteration_summary.linear_solver_iterations = 0;
	iteration_summary.step_solver_time_sec = 0;

	// Do initial cost and Jacobian evaluation.
	double cost = 0.0;
	if (!evaluator->Evaluate(x.data(), &cost, residuals.data(), jacobian)) {
	LOG(WARNING) << "Terminating: Residual and Jacobian evaluation failed.";
	summary->termination_type = NUMERICAL_FAILURE;
	return;
	}

	// Compute the fixed part of the cost.
	//
	// This is a poor way to do this computation. Even if fixed_cost is
	// zero, because we are subtracting two possibly large numbers, we
	// are depending on exact cancellation to give us a zero here. But
	// initial_cost and cost have been computed by two different
	// evaluators. One which runs on the whole problem (in
	// solver_impl.cc) in single threaded mode and another which runs
	// here on the reduced problem, so fixed_cost can (and does) contain
	// some numerical garbage with a relative magnitude of 1e-14.
	//
	// The right way to do this, would be to compute the fixed cost on
	// just the set of residual blocks which are held constant and were
	// removed from the original problem when the reduced problem was
	// constructed.
	summary->fixed_cost = summary->initial_cost - cost;

	gradient.setZero();
	jacobian->LeftMultiply(residuals.data(), gradient.data());
	iteration_summary.gradient_max_norm = gradient.lpNorm<Eigen::Infinity>();

	if (options_.jacobi_scaling) {
	EstimateScale(*jacobian, scale.data());
	jacobian->ScaleColumns(scale.data());
	} else {
	scale.setOnes();
	}

	const double absolute_gradient_tolerance =
	options_.gradient_tolerance *
	max(iteration_summary.gradient_max_norm, kEpsilon);
	if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) {
	summary->termination_type = GRADIENT_TOLERANCE;
	VLOG(1) << "Terminating: Gradient tolerance reached."
	<< "Relative gradient max norm: "
	<< iteration_summary.gradient_max_norm /
	summary->iterations[0].gradient_max_norm
	<< " <= " << options_.gradient_tolerance;
	return;
	}

	// Call the various callbacks.
	iteration_summary.iteration_time_sec = time(NULL) - iteration_start_time;
	summary->iterations.push_back(iteration_summary);
	switch (RunCallbacks(options_, iteration_summary)) {
	case SOLVER_TERMINATE_SUCCESSFULLY:
	summary->termination_type = USER_SUCCESS;
	VLOG(1) << "Terminating: User callback returned USER_SUCCESS.";
	return;
	case SOLVER_ABORT:
	summary->termination_type = USER_ABORT;
	VLOG(1) << "Terminating: User callback returned USER_ABORT.";
	return;
	case SOLVER_CONTINUE:
	break;
	default:
	LOG(FATAL) << "Unknown type of user callback status";
	}

	int num_consecutive_invalid_steps = 0;
	while (true) {
	iteration_start_time = time(NULL);
	if (iteration_summary.iteration >= options_.max_num_iterations) {
	summary->termination_type = NO_CONVERGENCE;
	VLOG(1) << "Terminating: Maximum number of iterations reached.";
	break;
	}

	if ((start_time - iteration_start_time) >= options_.max_solver_time_sec) {
	summary->termination_type = NO_CONVERGENCE;
	VLOG(1) << "Terminating: Maximum solver time reached.";
	break;
	}

	iteration_summary = IterationSummary();
	iteration_summary = summary->iterations.back();
	iteration_summary.iteration = summary->iterations.back().iteration + 1;
	iteration_summary.step_is_valid = false;
	iteration_summary.step_is_successful = false;

	const time_t strategy_start_time = time(NULL);
	TrustRegionStrategy::PerSolveOptions per_solve_options;
	per_solve_options.eta = options_.eta;
	LinearSolver::Summary strategy_summary =
	strategy->ComputeStep(per_solve_options,
	jacobian,
	residuals.data(),
	trust_region_step.data());

	iteration_summary.step_solver_time_sec = time(NULL) - strategy_start_time;
	iteration_summary.linear_solver_iterations =
	strategy_summary.num_iterations;

	if (!MaybeDumpLinearLeastSquaresProblem(iteration_summary.iteration,
	jacobian,
	residuals.data(),
	trust_region_step.data())) {
	LOG(FATAL) << "Tried writing linear least squares problem: "
	<< options.lsqp_dump_directory << "but failed.";
	}

	double new_model_cost = 0.0;
	if (strategy_summary.termination_type != FAILURE) {
	// new_model_cost = 1/2 \|J * step - f\|^2
	model_residuals = -residuals;
	jacobian->RightMultiply(trust_region_step.data(), model_residuals.data());
	new_model_cost = model_residuals.squaredNorm() / 2.0;

	// In exact arithmetic, this would never be the case. But poorly
	// conditioned matrices can give rise to situations where the
	// new_model_cost can actually be larger than half the squared
	// norm of the residual vector. We allow for small tolerance
	// around cost and beyond that declare the step to be invalid.
	if (cost < (new_model_cost - kEpsilon)) {
	VLOG(1) << "Invalid step: current_cost: " << cost
	<< " new_model_cost " << new_model_cost;
	} else {
	iteration_summary.step_is_valid = true;
	}
	}

	if (!iteration_summary.step_is_valid) {
	// Invalid steps can happen due to a number of reasons, and we
	// allow a limited number of successive failures, and return with
	// NUMERICAL_FAILURE if this limit is exceeded.
	if (++num_consecutive_invalid_steps >=
	options_.max_num_consecutive_invalid_steps) {
	summary->termination_type = NUMERICAL_FAILURE;
	LOG(WARNING) << "Terminating. Number of successive invalid steps more "
	<< "than "
	<< "Solver::Options::max_num_consecutive_invalid_steps: "
	<< options_.max_num_consecutive_invalid_steps;
	return;
	}

	// We are going to try and reduce the trust region radius and
	// solve again. To do this, we are going to treat this iteration
	// as an unsuccessful iteration. Since the various callbacks are
	// still executed, we are going to fill the iteration summary
	// with data that assumes a step of length zero and no progress.
	iteration_summary.cost = cost;
	iteration_summary.cost_change = 0.0;
	iteration_summary.gradient_max_norm =
	summary->iterations.back().gradient_max_norm;
	iteration_summary.step_norm = 0.0;
	iteration_summary.relative_decrease = 0.0;
	iteration_summary.eta = options_.eta;
	} else {
	// The step is numerically valid, so now we can judge its quality.
	num_consecutive_invalid_steps = 0;

	// We allow some slop around 0, and clamp the model_cost_change
	// at kEpsilon from below.
	//
	// There is probably a better way to do this, as it is going to
	// create problems for problems where the objective function is
	// kEpsilon close to zero.
	const double model_cost_change = max(kEpsilon, cost - new_model_cost);

	// Undo the Jacobian column scaling.
	delta = -(trust_region_step.array() * scale.array()).matrix();
	iteration_summary.step_norm = delta.norm();

	// Convergence based on parameter_tolerance.
	const double step_size_tolerance = options_.parameter_tolerance *
	(x_norm + options_.parameter_tolerance);
	if (iteration_summary.step_norm <= step_size_tolerance) {
	VLOG(1) << "Terminating. Parameter tolerance reached. "
	<< "relative step_norm: "
	<< iteration_summary.step_norm /
	(x_norm + options_.parameter_tolerance)
	<< " <= " << options_.parameter_tolerance;
	summary->termination_type = PARAMETER_TOLERANCE;
	return;
	}

	if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) {
	summary->termination_type = NUMERICAL_FAILURE;
	LOG(WARNING) << "Terminating. Failed to compute "
	<< "Plus(x, delta, x_plus_delta).";
	return;
	}

	// Try this step.
	double new_cost;
	if (!evaluator->Evaluate(x_plus_delta.data(), &new_cost, NULL, NULL)) {
	summary->termination_type = NUMERICAL_FAILURE;
	LOG(WARNING) << "Terminating: Cost evaluation failed.";
	return;
	}

	VLOG(2) << "old cost: " << cost << " new cost: " << new_cost;
	iteration_summary.cost_change = cost - new_cost;
	const double absolute_function_tolerance =
	options_.function_tolerance * cost;
	if (fabs(iteration_summary.cost_change) < absolute_function_tolerance) {
	VLOG(1) << "Terminating. Function tolerance reached. "
	<< "\|cost_change\|/cost: " << fabs(iteration_summary.cost_change) / cost
	<< " <= " << options_.function_tolerance;
	summary->termination_type = FUNCTION_TOLERANCE;
	return;
	}

	iteration_summary.relative_decrease =
	iteration_summary.cost_change / model_cost_change;
	iteration_summary.step_is_successful =
	iteration_summary.relative_decrease > options_.min_relative_decrease;
	}

	if (iteration_summary.step_is_successful) {
	++summary->num_successful_steps;
	strategy->StepAccepted(iteration_summary.relative_decrease);
	x = x_plus_delta;
	x_norm = x.norm();
	// Step looks good, evaluate the residuals and Jacobian at this
	// point.
	if (!evaluator->Evaluate(x.data(), &cost, residuals.data(), jacobian)) {
	summary->termination_type = NUMERICAL_FAILURE;
	LOG(WARNING) << "Terminating: Residual and Jacobian evaluation failed.";
	return;
	}

	gradient.setZero();
	jacobian->LeftMultiply(residuals.data(), gradient.data());
	iteration_summary.gradient_max_norm = gradient.lpNorm<Eigen::Infinity>();

	if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) {
	summary->termination_type = GRADIENT_TOLERANCE;
	VLOG(1) << "Terminating: Gradient tolerance reached."
	<< "Relative gradient max norm: "
	<< iteration_summary.gradient_max_norm /
	summary->iterations[0].gradient_max_norm
	<< " <= " << options_.gradient_tolerance;
	return;
	}

	if (options_.jacobi_scaling) {
	jacobian->ScaleColumns(scale.data());
	}
	} else {
	++summary->num_unsuccessful_steps;
	strategy->StepRejected(iteration_summary.relative_decrease);
	}

	iteration_summary.cost = cost + summary->fixed_cost;
	iteration_summary.trust_region_radius = strategy->Radius();
	if (iteration_summary.trust_region_radius <
	options_.min_trust_region_radius) {
	summary->termination_type = PARAMETER_TOLERANCE;
	VLOG(1) << "Termination. Minimum trust region radius reached.";
	return;
	}

	iteration_summary.iteration_time_sec = time(NULL) - iteration_start_time;
	summary->iterations.push_back(iteration_summary);
	switch (RunCallbacks(options_, iteration_summary)) {
	case SOLVER_TERMINATE_SUCCESSFULLY:
	summary->termination_type = USER_SUCCESS;
	VLOG(1) << "Terminating: User callback returned USER_SUCCESS.";
	return;
	case SOLVER_ABORT:
	summary->termination_type = USER_ABORT;
	VLOG(1) << "Terminating: User callback returned USER_ABORT.";
	return;
	case SOLVER_CONTINUE:
	break;
	default:
	LOG(FATAL) << "Unknown type of user callback status";
	}
	}
	}


	} // namespace internal
	} // namespace ceres