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 (at) 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 50 namespace ceres { 51 namespace internal { 52 namespace { 53 54 bool IsZeroOrInfinity(double x) { 55 return ((x == 0.0) || (IsInfinite(x))); 56 } 57 58 // Constant used in the MATLAB implementation ~ 2 * eps. 59 const double kEpsilon = 2.2204e-16; 60 61 } // namespace 62 63 ConjugateGradientsSolver::ConjugateGradientsSolver( 64 const LinearSolver::Options& options) 65 : options_(options) { 66 } 67 68 LinearSolver::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
