| 123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247 | // 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/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) || std::isinf(x));}}  // namespaceConjugateGradientsSolver::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(A != nullptr);  CHECK(x != nullptr);  CHECK(b != nullptr);  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) || std::isinf(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 (std::isinf(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
 |