gtsam  4.1.0
gtsam
iterative-inl.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #pragma once
20 
21 #include <gtsam/linear/iterative.h>
23 #include <boost/shared_ptr.hpp>
24 
25 namespace gtsam {
26 
27  /* ************************************************************************* */
28  // state for CG method
29  template<class S, class V, class E>
30  struct CGState {
31 
33  const Parameters &parameters_;
34 
35  int k;
36  bool steepest;
37  V g, d;
38  double gamma, threshold;
39  E Ad;
40 
41  /* ************************************************************************* */
42  // Constructor
43  CGState(const S& Ab, const V& x, const Parameters &parameters, bool steep):
44  parameters_(parameters),k(0),steepest(steep) {
45 
46  // Start with g0 = A'*(A*x0-b), d0 = - g0
47  // i.e., first step is in direction of negative gradient
48  g = Ab.gradient(x);
49  d = g; // instead of negating gradient, alpha will be negated
50 
51  // init gamma and calculate threshold
52  gamma = dot(g,g);
53  threshold = std::max(parameters_.epsilon_abs(), parameters_.epsilon() * parameters_.epsilon() * gamma);
54 
55  // Allocate and calculate A*d for first iteration
56  if (gamma > parameters_.epsilon_abs()) Ad = Ab * d;
57  }
58 
59  /* ************************************************************************* */
60  // print
61  void print(const V& x) {
62  std::cout << "iteration = " << k << std::endl;
63  gtsam::print(x,"x");
64  gtsam::print(g, "g");
65  std::cout << "dotg = " << gamma << std::endl;
66  gtsam::print(d, "d");
67  gtsam::print(Ad, "Ad");
68  }
69 
70  /* ************************************************************************* */
71  // step the solution
72  double takeOptimalStep(V& x) {
73  // TODO: can we use gamma instead of dot(d,g) ????? Answer not trivial
74  double alpha = -dot(d, g) / dot(Ad, Ad); // calculate optimal step-size
75  axpy(alpha, d, x); // // do step in new search direction, x += alpha*d
76  return alpha;
77  }
78 
79  /* ************************************************************************* */
80  // take a step, return true if converged
81  bool step(const S& Ab, V& x) {
82 
83  if ((++k) >= ((int)parameters_.maxIterations())) return true;
84 
85  //---------------------------------->
86  double alpha = takeOptimalStep(x);
87 
88  // update gradient (or re-calculate at reset time)
89  if (k % parameters_.reset() == 0) g = Ab.gradient(x);
90  // axpy(alpha, Ab ^ Ad, g); // g += alpha*(Ab^Ad)
91  else Ab.transposeMultiplyAdd(alpha, Ad, g);
92 
93  // check for convergence
94  double new_gamma = dot(g, g);
95 
96  if (parameters_.verbosity() != ConjugateGradientParameters::SILENT)
97  std::cout << "iteration " << k << ": alpha = " << alpha
98  << ", dotg = " << new_gamma
99  << std::endl;
100 
101  if (new_gamma < threshold) return true;
102 
103  // calculate new search direction
104  if (steepest) d = g;
105  else {
106  double beta = new_gamma / gamma;
107  // d = g + d*beta;
108  d *= beta;
109  axpy(1.0, g, d);
110  }
111 
112  gamma = new_gamma;
113 
114  // In-place recalculation Ad <- A*d to avoid re-allocating Ad
115  Ab.multiplyInPlace(d, Ad);
116  return false;
117  }
118 
119  }; // CGState Class
120 
121  /* ************************************************************************* */
122  // conjugate gradient method.
123  // S: linear system, V: step vector, E: errors
124  template<class S, class V, class E>
125  V conjugateGradients(const S& Ab, V x, const ConjugateGradientParameters &parameters, bool steepest) {
126 
127  CGState<S, V, E> state(Ab, x, parameters, steepest);
128 
129  if (parameters.verbosity() != ConjugateGradientParameters::SILENT)
130  std::cout << "CG: epsilon = " << parameters.epsilon()
131  << ", maxIterations = " << parameters.maxIterations()
132  << ", ||g0||^2 = " << state.gamma
133  << ", threshold = " << state.threshold
134  << std::endl;
135 
136  if ( state.gamma < state.threshold ) {
137  if (parameters.verbosity() != ConjugateGradientParameters::SILENT)
138  std::cout << "||g0||^2 < threshold, exiting immediately !" << std::endl;
139 
140  return x;
141  }
142 
143  // loop maxIterations times
144  while (!state.step(Ab, x)) {}
145  return x;
146  }
147 /* ************************************************************************* */
148 
149 } // namespace gtsam
gtsam::ConjugateGradientParameters
parameters for the conjugate gradient method
Definition: ConjugateGradientSolver.h:29
gtsam::axpy
void axpy(double alpha, const V1 &x, V2 &y)
BLAS Level 1 axpy: y <- alpha*x + y.
Definition: Vector.h:217
iterative.h
Iterative methods, implementation.
ConjugateGradientSolver.h
Implementation of Conjugate Gradient solver for a linear system.
gtsam::CGState::k
int k
iteration
Definition: iterative-inl.h:35
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::CGState
Definition: iterative-inl.h:30
gtsam::CGState::steepest
bool steepest
flag to indicate we are doing steepest descent
Definition: iterative-inl.h:36
gtsam::CGState::d
V d
gradient g and search direction d for CG
Definition: iterative-inl.h:37
gtsam::conjugateGradients
V conjugateGradients(const S &Ab, V x, const ConjugateGradientParameters &parameters, bool steepest)
Method of conjugate gradients (CG) template "System" class S needs gradient(S,v), e=S*v,...
Definition: iterative-inl.h:125
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:194
gtsam::CGState::threshold
double threshold
gamma (squared L2 norm of g) and convergence threshold
Definition: iterative-inl.h:38