1// Ceres Solver - A fast non-linear least squares minimizer 2// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3// http://code.google.com/p/ceres-solver/ 4// 5// Redistribution and use in source and binary forms, with or without 6// modification, are permitted provided that the following conditions are met: 7// 8// * Redistributions of source code must retain the above copyright notice, 9// this list of conditions and the following disclaimer. 10// * Redistributions in binary form must reproduce the above copyright notice, 11// this list of conditions and the following disclaimer in the documentation 12// and/or other materials provided with the distribution. 13// * Neither the name of Google Inc. nor the names of its contributors may be 14// used to endorse or promote products derived from this software without 15// specific prior written permission. 16// 17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27// POSSIBILITY OF SUCH DAMAGE. 28// 29// Author: sameeragarwal@google.com (Sameer Agarwal) 30// 31// A preconditioned conjugate gradients solver 32// (ConjugateGradientsSolver) for positive semidefinite linear 33// systems. 34// 35// We have also augmented the termination criterion used by this 36// solver to support not just residual based termination but also 37// termination based on decrease in the value of the quadratic model 38// that CG optimizes. 39 40#include "ceres/conjugate_gradients_solver.h" 41 42#include <cmath> 43#include <cstddef> 44#include "ceres/fpclassify.h" 45#include "ceres/internal/eigen.h" 46#include "ceres/linear_operator.h" 47#include "ceres/types.h" 48#include "glog/logging.h" 49 50namespace ceres { 51namespace internal { 52namespace { 53 54bool IsZeroOrInfinity(double x) { 55 return ((x == 0.0) || (IsInfinite(x))); 56} 57 58// Constant used in the MATLAB implementation ~ 2 * eps. 59const double kEpsilon = 2.2204e-16; 60 61} // namespace 62 63ConjugateGradientsSolver::ConjugateGradientsSolver( 64 const LinearSolver::Options& options) 65 : options_(options) { 66} 67 68LinearSolver::Summary ConjugateGradientsSolver::Solve( 69 LinearOperator* A, 70 const double* b, 71 const LinearSolver::PerSolveOptions& per_solve_options, 72 double* x) { 73 CHECK_NOTNULL(A); 74 CHECK_NOTNULL(x); 75 CHECK_NOTNULL(b); 76 CHECK_EQ(A->num_rows(), A->num_cols()); 77 78 LinearSolver::Summary summary; 79 summary.termination_type = MAX_ITERATIONS; 80 summary.num_iterations = 0; 81 82 int num_cols = A->num_cols(); 83 VectorRef xref(x, num_cols); 84 ConstVectorRef bref(b, num_cols); 85 86 double norm_b = bref.norm(); 87 if (norm_b == 0.0) { 88 xref.setZero(); 89 summary.termination_type = TOLERANCE; 90 return summary; 91 } 92 93 Vector r(num_cols); 94 Vector p(num_cols); 95 Vector z(num_cols); 96 Vector tmp(num_cols); 97 98 double tol_r = per_solve_options.r_tolerance * norm_b; 99 100 tmp.setZero(); 101 A->RightMultiply(x, tmp.data()); 102 r = bref - tmp; 103 double norm_r = r.norm(); 104 105 if (norm_r <= tol_r) { 106 summary.termination_type = TOLERANCE; 107 return summary; 108 } 109 110 double rho = 1.0; 111 112 // Initial value of the quadratic model Q = x'Ax - 2 * b'x. 113 double Q0 = -1.0 * xref.dot(bref + r); 114 115 for (summary.num_iterations = 1; 116 summary.num_iterations < options_.max_num_iterations; 117 ++summary.num_iterations) { 118 VLOG(3) << "cg iteration " << summary.num_iterations; 119 120 // Apply preconditioner 121 if (per_solve_options.preconditioner != NULL) { 122 z.setZero(); 123 per_solve_options.preconditioner->RightMultiply(r.data(), z.data()); 124 } else { 125 z = r; 126 } 127 128 double last_rho = rho; 129 rho = r.dot(z); 130 131 if (IsZeroOrInfinity(rho)) { 132 LOG(ERROR) << "Numerical failure. rho = " << rho; 133 summary.termination_type = FAILURE; 134 break; 135 }; 136 137 if (summary.num_iterations == 1) { 138 p = z; 139 } else { 140 double beta = rho / last_rho; 141 if (IsZeroOrInfinity(beta)) { 142 LOG(ERROR) << "Numerical failure. beta = " << beta; 143 summary.termination_type = FAILURE; 144 break; 145 } 146 p = z + beta * p; 147 } 148 149 Vector& q = z; 150 q.setZero(); 151 A->RightMultiply(p.data(), q.data()); 152 double pq = p.dot(q); 153 154 if ((pq <= 0) || IsInfinite(pq)) { 155 LOG(ERROR) << "Numerical failure. pq = " << pq; 156 summary.termination_type = FAILURE; 157 break; 158 } 159 160 double alpha = rho / pq; 161 if (IsInfinite(alpha)) { 162 LOG(ERROR) << "Numerical failure. alpha " << alpha; 163 summary.termination_type = FAILURE; 164 break; 165 } 166 167 xref = xref + alpha * p; 168 169 // Ideally we would just use the update r = r - alpha*q to keep 170 // track of the residual vector. However this estimate tends to 171 // drift over time due to round off errors. Thus every 172 // residual_reset_period iterations, we calculate the residual as 173 // r = b - Ax. We do not do this every iteration because this 174 // requires an additional matrix vector multiply which would 175 // double the complexity of the CG algorithm. 176 if (summary.num_iterations % options_.residual_reset_period == 0) { 177 tmp.setZero(); 178 A->RightMultiply(x, tmp.data()); 179 r = bref - tmp; 180 } else { 181 r = r - alpha * q; 182 } 183 184 // Quadratic model based termination. 185 // Q1 = x'Ax - 2 * b' x. 186 double Q1 = -1.0 * xref.dot(bref + r); 187 188 // For PSD matrices A, let 189 // 190 // Q(x) = x'Ax - 2b'x 191 // 192 // be the cost of the quadratic function defined by A and b. Then, 193 // the solver terminates at iteration i if 194 // 195 // i * (Q(x_i) - Q(x_i-1)) / Q(x_i) < q_tolerance. 196 // 197 // This termination criterion is more useful when using CG to 198 // solve the Newton step. This particular convergence test comes 199 // from Stephen Nash's work on truncated Newton 200 // methods. References: 201 // 202 // 1. Stephen G. Nash & Ariela Sofer, Assessing A Search 203 // Direction Within A Truncated Newton Method, Operation 204 // Research Letters 9(1990) 219-221. 205 // 206 // 2. Stephen G. Nash, A Survey of Truncated Newton Methods, 207 // Journal of Computational and Applied Mathematics, 208 // 124(1-2), 45-59, 2000. 209 // 210 double zeta = summary.num_iterations * (Q1 - Q0) / Q1; 211 VLOG(3) << "Q termination: zeta " << zeta 212 << " " << per_solve_options.q_tolerance; 213 if (zeta < per_solve_options.q_tolerance) { 214 summary.termination_type = TOLERANCE; 215 break; 216 } 217 Q0 = Q1; 218 219 // Residual based termination. 220 norm_r = r. norm(); 221 VLOG(3) << "R termination: norm_r " << norm_r 222 << " " << tol_r; 223 if (norm_r <= tol_r) { 224 summary.termination_type = TOLERANCE; 225 break; 226 } 227 } 228 229 return summary; 230}; 231 232} // namespace internal 233} // namespace ceres 234