// Ceres Solver - A fast non-linear least squares minimizer

// Copyright 2015 Google Inc. All rights reserved.

// http://ceres-solver.org/

//

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

//

// A preconditioned conjugate gradients solver

// (ConjugateGradientsSolver) for positive semidefinite linear

// systems.

//

// We have also augmented the termination criterion used by this

// solver to support not just residual based termination but also

// termination based on decrease in the value of the quadratic model

// that CG optimizes.

#include "ceres/conjugate_gradients_solver.h"

#include <cmath>

#include <cstddef>

#include "ceres/fpclassify.h"

#include "ceres/internal/eigen.h"

#include "ceres/linear_operator.h"

#include "ceres/stringprintf.h"

#include "ceres/types.h"

#include "glog/logging.h"

namespace ceres {

namespace internal {

namespace {

bool IsZeroOrInfinity(double x) {

  return ((x == 0.0) || (IsInfinite(x)));

}

}  // namespace

ConjugateGradientsSolver::ConjugateGradientsSolver(

    const LinearSolver::Options& options)

    : options_(options) {

}

LinearSolver::Summary ConjugateGradientsSolver::Solve(

    LinearOperator* A,

    const double* b,

    const LinearSolver::PerSolveOptions& per_solve_options,

    double* x) {

  CHECK_NOTNULL(A);

  CHECK_NOTNULL(x);

  CHECK_NOTNULL(b);

  CHECK_EQ(A->num_rows(), A->num_cols());

  LinearSolver::Summary summary;

  summary.termination_type = LINEAR_SOLVER_NO_CONVERGENCE;

  summary.message = "Maximum number of iterations reached.";

  summary.num_iterations = 0;

  const int num_cols = A->num_cols();

  VectorRef xref(x, num_cols);

  ConstVectorRef bref(b, num_cols);

  const double norm_b = bref.norm();

  if (norm_b == 0.0) {

    xref.setZero();

    summary.termination_type = LINEAR_SOLVER_SUCCESS;

    summary.message = "Convergence. |b| = 0.";

    return summary;

  }

  Vector r(num_cols);

  Vector p(num_cols);

  Vector z(num_cols);

  Vector tmp(num_cols);

  const double tol_r = per_solve_options.r_tolerance * norm_b;

  tmp.setZero();

  A->RightMultiply(x, tmp.data());

  r = bref - tmp;

  double norm_r = r.norm();

  if (options_.min_num_iterations == 0 && norm_r <= tol_r) {

    summary.termination_type = LINEAR_SOLVER_SUCCESS;

    summary.message =

        StringPrintf("Convergence. |r| = %e <= %e.", norm_r, tol_r);

    return summary;

  }

  double rho = 1.0;

  // Initial value of the quadratic model Q = x'Ax - 2 * b'x.

  double Q0 = -1.0 * xref.dot(bref + r);

  for (summary.num_iterations = 1;; ++summary.num_iterations) {

    // Apply preconditioner

    if (per_solve_options.preconditioner != NULL) {

      z.setZero();

      per_solve_options.preconditioner->RightMultiply(r.data(), z.data());

    } else {

      z = r;

    }

    double last_rho = rho;

    rho = r.dot(z);

    if (IsZeroOrInfinity(rho)) {

      summary.termination_type = LINEAR_SOLVER_FAILURE;

      summary.message = StringPrintf("Numerical failure. rho = r'z = %e.", rho);

      break;

    }

    if (summary.num_iterations == 1) {

      p = z;

    } else {

      double beta = rho / last_rho;

      if (IsZeroOrInfinity(beta)) {

        summary.termination_type = LINEAR_SOLVER_FAILURE;

        summary.message = StringPrintf(

            "Numerical failure. beta = rho_n / rho_{n-1} = %e, "

            "rho_n = %e, rho_{n-1} = %e", beta, rho, last_rho);

        break;

      }

      p = z + beta * p;

    }

    Vector& q = z;

    q.setZero();

    A->RightMultiply(p.data(), q.data());

    const double pq = p.dot(q);

    if ((pq <= 0) || IsInfinite(pq))  {

      summary.termination_type = LINEAR_SOLVER_NO_CONVERGENCE;

      summary.message = StringPrintf(

          "Matrix is indefinite, no more progress can be made. "

          "p'q = %e. |p| = %e, |q| = %e",

          pq, p.norm(), q.norm());

      break;

    }

    const double alpha = rho / pq;

    if (IsInfinite(alpha)) {

      summary.termination_type = LINEAR_SOLVER_FAILURE;

      summary.message =

          StringPrintf("Numerical failure. alpha = rho / pq = %e, "

                       "rho = %e, pq = %e.", alpha, rho, pq);

      break;

    }

    xref = xref + alpha * p;

    // Ideally we would just use the update r = r - alpha*q to keep

    // track of the residual vector. However this estimate tends to

    // drift over time due to round off errors. Thus every

    // residual_reset_period iterations, we calculate the residual as

    // r = b - Ax. We do not do this every iteration because this

    // requires an additional matrix vector multiply which would

    // double the complexity of the CG algorithm.

    if (summary.num_iterations % options_.residual_reset_period == 0) {

      tmp.setZero();

      A->RightMultiply(x, tmp.data());

      r = bref - tmp;

    } else {

      r = r - alpha * q;

    }

    // Quadratic model based termination.

    //   Q1 = x'Ax - 2 * b' x.

    const double Q1 = -1.0 * xref.dot(bref + r);

    // For PSD matrices A, let

    //

    //   Q(x) = x'Ax - 2b'x

    //

    // be the cost of the quadratic function defined by A and b. Then,

    // the solver terminates at iteration i if

    //

    //   i * (Q(x_i) - Q(x_i-1)) / Q(x_i) < q_tolerance.

    //

    // This termination criterion is more useful when using CG to

    // solve the Newton step. This particular convergence test comes

    // from Stephen Nash's work on truncated Newton

    // methods. References:

    //

    //   1. Stephen G. Nash & Ariela Sofer, Assessing A Search

    //   Direction Within A Truncated Newton Method, Operation

    //   Research Letters 9(1990) 219-221.

    //

    //   2. Stephen G. Nash, A Survey of Truncated Newton Methods,

    //   Journal of Computational and Applied Mathematics,

    //   124(1-2), 45-59, 2000.

    //

    const double zeta = summary.num_iterations * (Q1 - Q0) / Q1;

    if (zeta < per_solve_options.q_tolerance &&

        summary.num_iterations >= options_.min_num_iterations) {

      summary.termination_type = LINEAR_SOLVER_SUCCESS;

      summary.message =

          StringPrintf("Iteration: %d Convergence: zeta = %e < %e. |r| = %e",

                       summary.num_iterations,

                       zeta,

                       per_solve_options.q_tolerance,

                       r.norm());

      break;

    }

    Q0 = Q1;

    // Residual based termination.

    norm_r = r. norm();

    if (norm_r <= tol_r &&

        summary.num_iterations >= options_.min_num_iterations) {

      summary.termination_type = LINEAR_SOLVER_SUCCESS;

      summary.message =

          StringPrintf("Iteration: %d Convergence. |r| = %e <= %e.",

                       summary.num_iterations,

                       norm_r,

                       tol_r);

      break;

    }

    if (summary.num_iterations >= options_.max_num_iterations) {

      break;

    }

  }

  return summary;

}

}  // namespace internal

}  // namespace ceres