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
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
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;
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
47  // i.e., first step is in direction of negative gradient
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");
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);
92
93  // check for convergence
94  double new_gamma = dot(g, g);
95
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
116  return false;
117  }
118
119  }; // CGState Class
120
121  /* ************************************************************************* */
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
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 ) {
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
parameters for the conjugate gradient method
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.
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