gtsam 4.1.1 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
23#include <boost/shared_ptr.hpp>
24
25namespace 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 x += alpha * d; // 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 d += 1.0 * g;
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
Iterative methods, implementation.
Implementation of Conjugate Gradient solver for a linear system.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
Method of conjugate gradients (CG) template "System" class S needs gradient(S,v), e=S*v,...
Definition: iterative-inl.h:125
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:194
parameters for the conjugate gradient method