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
50 namespace ceres {
51 namespace internal {
52 namespace {
53
IsZeroOrInfinity(double x)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
ConjugateGradientsSolver(const LinearSolver::Options & options)63 ConjugateGradientsSolver::ConjugateGradientsSolver(
64 const LinearSolver::Options& options)
65 : options_(options) {
66 }
67
Solve(LinearOperator * A,const double * b,const LinearSolver::PerSolveOptions & per_solve_options,double * x)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