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 
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>
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
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
#define max(a, b)
Definition: datatypes.h:20
Key E(std::uint64_t j)
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:194
Iterative methods, implementation.
V conjugateGradients(const S &Ab, V x, const ConjugateGradientParameters &parameters, bool steepest)
Implementation of Conjugate Gradient solver for a linear system.
int k
iteration
Definition: iterative-inl.h:35
bool steepest
flag to indicate we are doing steepest descent
Definition: iterative-inl.h:36
Key S(std::uint64_t j)
bool step(const S &Ab, V &x)
Definition: iterative-inl.h:81
CGState(const S &Ab, const V &x, const Parameters &parameters, bool steep)
Definition: iterative-inl.h:43
ConjugateGradientParameters Parameters
Definition: iterative-inl.h:32
RealScalar alpha
void axpy(double alpha, const V1 &x, V2 &y)
Definition: Vector.h:217
static ConjugateGradientParameters parameters
const Parameters & parameters_
Definition: iterative-inl.h:33
traits
Definition: chartTesting.h:28
double threshold
gamma (squared L2 norm of g) and convergence threshold
Definition: iterative-inl.h:38
V d
gradient g and search direction d for CG
Definition: iterative-inl.h:37
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
double takeOptimalStep(V &x)
Definition: iterative-inl.h:72
void print(const V &x)
Definition: iterative-inl.h:61


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:42:22