// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 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)
//
// Implementation of a simple LM algorithm which uses the step sizing
// rule of "Methods for Nonlinear Least Squares" by K. Madsen,
// H.B. Nielsen and O. Tingleff. Available to download from
//
// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf
//
// The basic algorithm described in this note is an exact step
// algorithm that depends on the Newton(LM) step being solved exactly
// in each iteration. When a suitable iterative solver is available to
// solve the Newton(LM) step, the algorithm will automatically switch
// to an inexact step solution method. This trades some slowdown in
// convergence for significant savings in solve time and memory
// usage. Our implementation of the Truncated Newton algorithm follows
// the discussion and recommendataions in "Stephen G. Nash, A Survey
// of Truncated Newton Methods, Journal of Computational and Applied
// Mathematics, 124(1-2), 45-59, 2000.

#include "ceres/levenberg_marquardt.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/file.h"
#include "ceres/linear_least_squares_problems.h"
#include "ceres/linear_solver.h"
#include "ceres/matrix_proto.h"
#include "ceres/sparse_matrix.h"
#include "ceres/stringprintf.h"
#include "ceres/internal/eigen.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/types.h"

namespace ceres {
namespace internal {
namespace {

// Numbers for clamping the size of the LM diagonal. The size of these
// numbers is heuristic. We will probably be adjusting them in the
// future based on more numerical experience. With jacobi scaling
// enabled, these numbers should be all but redundant.
const double kMinLevenbergMarquardtDiagonal = 1e-6;
const double kMaxLevenbergMarquardtDiagonal = 1e32;

// Small constant for various floating point issues.
const double kEpsilon = 1e-12;

// Number of times the linear solver should be retried in case of
// numerical failure. The retries are done by exponentially scaling up
// mu at each retry. This leads to stronger and stronger
// regularization making the linear least squares problem better
// conditioned at each retry.
const int kMaxLinearSolverRetries = 5;

// D = 1/sqrt(diag(J^T * J))
void EstimateScale(const SparseMatrix& jacobian, double* D) {
  CHECK_NOTNULL(D);
  jacobian.SquaredColumnNorm(D);
  for (int i = 0; i < jacobian.num_cols(); ++i) {
    D[i] = 1.0 / (kEpsilon + sqrt(D[i]));
  }
}

// D = diag(J^T * J)
void LevenbergMarquardtDiagonal(const SparseMatrix& jacobian,
                                double* D) {
  CHECK_NOTNULL(D);
  jacobian.SquaredColumnNorm(D);
  for (int i = 0; i < jacobian.num_cols(); ++i) {
    D[i] = min(max(D[i], kMinLevenbergMarquardtDiagonal),
               kMaxLevenbergMarquardtDiagonal);
  }
}

bool RunCallback(IterationCallback* callback,
                 const IterationSummary& iteration_summary,
                 Solver::Summary* summary) {
  const CallbackReturnType status = (*callback)(iteration_summary);
  switch (status) {
    case SOLVER_TERMINATE_SUCCESSFULLY:
      summary->termination_type = USER_SUCCESS;
      VLOG(1) << "Terminating on USER_SUCCESS.";
      return false;
    case SOLVER_ABORT:
      summary->termination_type = USER_ABORT;
      VLOG(1) << "Terminating on USER_ABORT.";
      return false;
    case SOLVER_CONTINUE:
      return true;
    default:
      LOG(FATAL) << "Unknown status returned by callback: "
                 << status;
  }
}

}  // namespace

LevenbergMarquardt::~LevenbergMarquardt() {}

void LevenbergMarquardt::Minimize(const Minimizer::Options& options,
                                  Evaluator* evaluator,
                                  LinearSolver* linear_solver,
                                  const double* initial_parameters,
                                  double* final_parameters,
                                  Solver::Summary* summary) {
  time_t start_time = time(NULL);
  const int num_parameters = evaluator->NumParameters();
  const int num_effective_parameters = evaluator->NumEffectiveParameters();
  const int num_residuals = evaluator->NumResiduals();

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

  // Allocate the various vectors needed by the algorithm.
  memcpy(final_parameters, initial_parameters,
         num_parameters * sizeof(*initial_parameters));

  VectorRef x(final_parameters, num_parameters);
  Vector x_new(num_parameters);

  Vector lm_step(num_effective_parameters);
  Vector gradient(num_effective_parameters);
  Vector scaled_gradient(num_effective_parameters);
  // Jacobi scaling vector
  Vector scale(num_effective_parameters);

  Vector f_model(num_residuals);
  Vector f(num_residuals);
  Vector f_new(num_residuals);
  Vector D(num_parameters);
  Vector muD(num_parameters);

  // Ask the Evaluator to create the jacobian matrix. The sparsity
  // pattern of this matrix is going to remain constant, so we only do
  // this once and then re-use this matrix for all subsequent Jacobian
  // computations.
  scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());

  double x_norm  = x.norm();

  double cost = 0.0;
  D.setOnes();
  f.setZero();

  // Do initial cost and Jacobian evaluation.
  if (!evaluator->Evaluate(x.data(), &cost, f.data(), jacobian.get())) {
    LOG(WARNING) << "Failed to compute residuals and Jacobian. "
                 << "Terminating.";
    summary->termination_type = NUMERICAL_FAILURE;
    return;
  }

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

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

  double model_cost = f.squaredNorm() / 2.0;
  double total_cost = summary->fixed_cost + cost;

  scaled_gradient.setZero();
  jacobian->LeftMultiply(f.data(), scaled_gradient.data());
  gradient = scaled_gradient.array() / scale.array();

  double gradient_max_norm = gradient.lpNorm<Eigen::Infinity>();
  // We need the max here to guard againt the gradient being zero.
  const double gradient_max_norm_0 = max(gradient_max_norm, kEpsilon);
  double gradient_tolerance = options.gradient_tolerance * gradient_max_norm_0;

  double mu = options.tau;
  double nu = 2.0;
  int iteration = 0;
  double actual_cost_change = 0.0;
  double step_norm = 0.0;
  double relative_decrease = 0.0;

  // Insane steps are steps which are not sane, i.e. there is some
  // numerical kookiness going on with them. There are various reasons
  // for this kookiness, some easier to diagnose then others. From the
  // point of view of the non-linear solver, they are steps which
  // cannot be used. We return with NUMERICAL_FAILURE after
  // kMaxLinearSolverRetries consecutive insane steps.
  bool step_is_sane = false;
  int num_consecutive_insane_steps = 0;

  // Whether the step resulted in a sufficient decrease in the
  // objective function when compared to the decrease in the value of
  // the lineariztion.
  bool step_is_successful = false;

  // Parse the iterations for which to dump the linear problem.
  vector<int> iterations_to_dump = options.lsqp_iterations_to_dump;
  sort(iterations_to_dump.begin(), iterations_to_dump.end());

  IterationSummary iteration_summary;
  iteration_summary.iteration = iteration;
  iteration_summary.step_is_successful = false;
  iteration_summary.cost = total_cost;
  iteration_summary.cost_change = actual_cost_change;
  iteration_summary.gradient_max_norm = gradient_max_norm;
  iteration_summary.step_norm = step_norm;
  iteration_summary.relative_decrease = relative_decrease;
  iteration_summary.mu = mu;
  iteration_summary.eta = options.eta;
  iteration_summary.linear_solver_iterations = 0;
  iteration_summary.linear_solver_time_sec = 0.0;
  iteration_summary.iteration_time_sec = (time(NULL) - start_time);
  if (options.logging_type >= PER_MINIMIZER_ITERATION) {
    summary->iterations.push_back(iteration_summary);
  }

  // Check if the starting point is an optimum.
  VLOG(2) << "Gradient max norm: " << gradient_max_norm
          << " tolerance: " << gradient_tolerance
          << " ratio: " << gradient_max_norm / gradient_max_norm_0
          << " tolerance: " << options.gradient_tolerance;
  if (gradient_max_norm <= gradient_tolerance) {
    summary->termination_type = GRADIENT_TOLERANCE;
    VLOG(1) << "Terminating on GRADIENT_TOLERANCE. "
            << "Relative gradient max norm: "
            << gradient_max_norm / gradient_max_norm_0
            << " <= " << options.gradient_tolerance;
    return;
  }

  // Call the various callbacks.
  for (int i = 0; i < options.callbacks.size(); ++i) {
    if (!RunCallback(options.callbacks[i], iteration_summary, summary)) {
      return;
    }
  }

  // We only need the LM diagonal if we are actually going to do at
  // least one iteration of the optimization. So we wait to do it
  // until now.
  LevenbergMarquardtDiagonal(*jacobian, D.data());

  while ((iteration < options.max_num_iterations) &&
         (time(NULL) - start_time) <= options.max_solver_time_sec) {
    time_t iteration_start_time = time(NULL);
    step_is_sane = false;
    step_is_successful = false;

    IterationSummary iteration_summary;
    // The while loop here is just to provide an easily breakable
    // control structure. We are guaranteed to always exit this loop
    // at the end of one iteration or before.
    while (1) {
      muD = (mu * D).array().sqrt();
      LinearSolver::PerSolveOptions solve_options;
      solve_options.D = muD.data();
      solve_options.q_tolerance = options.eta;
      // Disable r_tolerance checking. Since we only care about
      // termination via the q_tolerance. As Nash and Sofer show,
      // r_tolerance based termination is essentially useless in
      // Truncated Newton methods.
      solve_options.r_tolerance = -1.0;

      // Invalidate the output array lm_step, so that we can detect if
      // the linear solver generated numerical garbage.  This is known
      // to happen for the DENSE_QR and then DENSE_SCHUR solver when
      // the Jacobin is severly rank deficient mu is too small.
      InvalidateArray(num_effective_parameters, lm_step.data());
      const time_t linear_solver_start_time = time(NULL);
      LinearSolver::Summary linear_solver_summary =
          linear_solver->Solve(jacobian.get(),
                               f.data(),
                               solve_options,
                               lm_step.data());
      iteration_summary.linear_solver_time_sec =
          (time(NULL) - linear_solver_start_time);
      iteration_summary.linear_solver_iterations =
          linear_solver_summary.num_iterations;

      if (binary_search(iterations_to_dump.begin(),
                        iterations_to_dump.end(),
                        iteration)) {
        CHECK(DumpLinearLeastSquaresProblem(options.lsqp_dump_directory,
                                            iteration,
                                            options.lsqp_dump_format_type,
                                            jacobian.get(),
                                            muD.data(),
                                            f.data(),
                                            lm_step.data(),
                                            options.num_eliminate_blocks))
            << "Tried writing linear least squares problem: " 
            << options.lsqp_dump_directory
            << " but failed.";
      }

      // We ignore the case where the linear solver did not converge,
      // since the partial solution computed by it still maybe of use,
      // and there is no reason to ignore it, especially since we
      // spent so much time computing it.
      if ((linear_solver_summary.termination_type != TOLERANCE) &&
          (linear_solver_summary.termination_type != MAX_ITERATIONS)) {
        VLOG(1) << "Linear solver failure: retrying with a higher mu";
        break;
      }

      if (!IsArrayValid(num_effective_parameters, lm_step.data())) {
        LOG(WARNING) << "Linear solver failure. Failed to compute a finite "
                     << "step. Terminating. Please report this to the Ceres "
                     << "Solver developers.";
        summary->termination_type = NUMERICAL_FAILURE;
        return;
      }

      step_norm = (lm_step.array() * scale.array()).matrix().norm();

      // Check step length based convergence. If the step length is
      // too small, then we are done.
      const double step_size_tolerance =  options.parameter_tolerance *
          (x_norm + options.parameter_tolerance);

      VLOG(2) << "Step size: " << step_norm
              << " tolerance: " <<  step_size_tolerance
              << " ratio: " << step_norm / step_size_tolerance
              << " tolerance: " << options.parameter_tolerance;
      if (step_norm <= options.parameter_tolerance *
          (x_norm + options.parameter_tolerance)) {
        summary->termination_type = PARAMETER_TOLERANCE;
        VLOG(1) << "Terminating on PARAMETER_TOLERANCE."
                << "Relative step size: " << step_norm / step_size_tolerance
            << " <= " << options.parameter_tolerance;
        return;
      }

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

      double cost_new = 0.0;
      if (!evaluator->Evaluate(x_new.data(), &cost_new, NULL, NULL)) {
        LOG(WARNING) << "Failed to compute the value of the objective "
                     << "function. Terminating.";
        summary->termination_type = NUMERICAL_FAILURE;
        return;
      }

      f_model.setZero();
      jacobian->RightMultiply(lm_step.data(), f_model.data());
      const double model_cost_new =
          (f.segment(0, num_residuals) - f_model).squaredNorm() / 2;

      actual_cost_change = cost - cost_new;
      double model_cost_change = model_cost - model_cost_new;

      VLOG(2) << "[Model cost] current: " << model_cost
              << " new : " << model_cost_new
              << " change: " << model_cost_change;

      VLOG(2) << "[Nonlinear cost] current: " << cost
              << " new : " << cost_new
              << " change: " << actual_cost_change
              << " relative change: " << fabs(actual_cost_change) / cost
              << " tolerance: " << options.function_tolerance;

      // In exact arithmetic model_cost_change should never be
      // negative. But due to numerical precision issues, we may end up
      // with a small negative number. model_cost_change which are
      // negative and large in absolute value are indicative of a
      // numerical failure in the solver.
      if (model_cost_change < -kEpsilon) {
        VLOG(1) << "Model cost change is negative.\n"
                << "Current : " << model_cost
                << " new : " << model_cost_new
                << " change: " << model_cost_change << "\n";
        break;
      }

      // If we have reached this far, then we are willing to trust the
      // numerical quality of the step.
      step_is_sane = true;
      num_consecutive_insane_steps = 0;

      // Check function value based convergence.
      if (fabs(actual_cost_change) < options.function_tolerance * cost) {
        VLOG(1) << "Termination on FUNCTION_TOLERANCE."
                << " Relative cost change: " << fabs(actual_cost_change) / cost
                << " tolerance: " << options.function_tolerance;
        summary->termination_type = FUNCTION_TOLERANCE;
        return;
      }

      // Clamp model_cost_change at kEpsilon from below.
      if (model_cost_change < kEpsilon) {
        VLOG(1) << "Clamping model cost change " << model_cost_change
                << " to " << kEpsilon;
        model_cost_change = kEpsilon;
      }

      relative_decrease = actual_cost_change / model_cost_change;
      VLOG(2) << "actual_cost_change / model_cost_change = "
              << relative_decrease;

      if (relative_decrease < options.min_relative_decrease) {
        VLOG(2) << "Unsuccessful step.";
        break;
      }

      VLOG(2) << "Successful step.";

      ++summary->num_successful_steps;
      x = x_new;
      x_norm = x.norm();

      if (!evaluator->Evaluate(x.data(), &cost, f.data(), jacobian.get())) {
        LOG(WARNING) << "Failed to compute residuals and jacobian. "
                     << "Terminating.";
        summary->termination_type = NUMERICAL_FAILURE;
        return;
      }

      if (options.jacobi_scaling) {
        jacobian->ScaleColumns(scale.data());
      }

      model_cost = f.squaredNorm() / 2.0;
      LevenbergMarquardtDiagonal(*jacobian, D.data());
      scaled_gradient.setZero();
      jacobian->LeftMultiply(f.data(), scaled_gradient.data());
      gradient = scaled_gradient.array() / scale.array();
      gradient_max_norm = gradient.lpNorm<Eigen::Infinity>();

      // Check gradient based convergence.
      VLOG(2) << "Gradient max norm: " << gradient_max_norm
              << " tolerance: " << gradient_tolerance
              << " ratio: " << gradient_max_norm / gradient_max_norm_0
              << " tolerance: " << options.gradient_tolerance;
      if (gradient_max_norm <= gradient_tolerance) {
        summary->termination_type = GRADIENT_TOLERANCE;
        VLOG(1) << "Terminating on GRADIENT_TOLERANCE. "
                << "Relative gradient max norm: "
                << gradient_max_norm / gradient_max_norm_0
                << " <= " << options.gradient_tolerance
                << " (tolerance).";
        return;
      }

      mu = mu * max(1.0 / 3.0, 1 - pow(2 * relative_decrease - 1, 3));
      nu = 2.0;
      step_is_successful = true;
      break;
    }

    if (!step_is_sane) {
      ++num_consecutive_insane_steps;
    }

    if (num_consecutive_insane_steps == kMaxLinearSolverRetries) {
      summary->termination_type = NUMERICAL_FAILURE;
      VLOG(1) << "Too many consecutive retries; ending with numerical fail.";

      if (!options.crash_and_dump_lsqp_on_failure) {
        return;
      }

      // Dump debugging information to disk.
      CHECK(options.lsqp_dump_format_type == TEXTFILE ||
            options.lsqp_dump_format_type == PROTOBUF)
          << "Dumping the linear least squares problem on crash "
          << "requires Solver::Options::lsqp_dump_format_type to be "
          << "PROTOBUF or TEXTFILE.";

      if (DumpLinearLeastSquaresProblem(options.lsqp_dump_directory,
                                        iteration,
                                        options.lsqp_dump_format_type,
                                        jacobian.get(),
                                        muD.data(),
                                        f.data(),
                                        lm_step.data(),
                                        options.num_eliminate_blocks)) {
        LOG(FATAL) << "Linear least squares problem saved to: " 
                   << options.lsqp_dump_directory
                   << ". Please provide this to the Ceres developers for "
                   << " debugging along with the v=2 log.";
      } else {
        LOG(FATAL) << "Tried writing linear least squares problem: " 
                   << options.lsqp_dump_directory
                   << " but failed.";
      }
    }

    if (!step_is_successful) {
      // Either the step did not lead to a decrease in cost or there
      // was numerical failure. In either case we will scale mu up and
      // retry. If it was a numerical failure, we hope that the
      // stronger regularization will make the linear system better
      // conditioned. If it was numerically sane, but there was no
      // decrease in cost, then increasing mu reduces the size of the
      // trust region and we look for a decrease closer to the
      // linearization point.
      ++summary->num_unsuccessful_steps;
      mu = mu * nu;
      nu = 2 * nu;
    }

    ++iteration;

    total_cost = summary->fixed_cost + cost;

    iteration_summary.iteration = iteration;
    iteration_summary.step_is_successful = step_is_successful;
    iteration_summary.cost = total_cost;
    iteration_summary.cost_change = actual_cost_change;
    iteration_summary.gradient_max_norm = gradient_max_norm;
    iteration_summary.step_norm = step_norm;
    iteration_summary.relative_decrease = relative_decrease;
    iteration_summary.mu = mu;
    iteration_summary.eta = options.eta;
    iteration_summary.iteration_time_sec = (time(NULL) - iteration_start_time);

    if (options.logging_type >= PER_MINIMIZER_ITERATION) {
      summary->iterations.push_back(iteration_summary);
    }

    // Call the various callbacks.
    for (int i = 0; i < options.callbacks.size(); ++i) {
      if (!RunCallback(options.callbacks[i], iteration_summary, summary)) {
        return;
      }
    }
  }
}

}  // namespace internal
}  // namespace ceres