| // 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)); } |
| |
| } // 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(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 |