← Back to branch summary

~ubuntu-branches/ubuntu/trusty/blender/trusty

« back to all changes in this revision

Viewing changes to extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc

  • Committer: Package Import Robot
  • Author(s): Jeremy Bicha
  • Date: 2013-03-06 12:08:47 UTC
  • mfrom: (1.5.1) (14.1.8 experimental)
  • Revision ID: package-import@ubuntu.com-20130306120847-frjfaryb2zrotwcg
Tags: 2.66a-1ubuntu1
https://launchpad.net/bugs/1076930
https://launchpad.net/bugs/1089256
https://launchpad.net/bugs/1052743
https://launchpad.net/bugs/999024
* Resynchronize with Debian (LP: #1076930, #1089256, #1052743, #999024,
  #1122888, #1147084)
* debian/control:
  - Lower build-depends on libavcodec-dev since we're not
    doing the libav9 transition in Ubuntu yet

Show diffs side-by-side

added added

removed removed

Lines of Context:
extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc
 
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
 
 
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