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