gtsam  4.1.0
gtsam
ConjugateGradientSolver.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 
20 #pragma once
21 
23 
24 namespace gtsam {
25 
30 
31 public:
33  typedef boost::shared_ptr<ConjugateGradientParameters> shared_ptr;
34 
35  size_t minIterations_;
36  size_t maxIterations_;
37  size_t reset_;
38  double epsilon_rel_;
39  double epsilon_abs_;
40 
41  /* Matrix Operation Kernel */
42  enum BLASKernel {
43  GTSAM = 0,
44  } blas_kernel_ ;
45 
47  : minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3),
48  epsilon_abs_(1e-3), blas_kernel_(GTSAM) {}
49 
50  ConjugateGradientParameters(size_t minIterations, size_t maxIterations, size_t reset,
51  double epsilon_rel, double epsilon_abs, BLASKernel blas)
52  : minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset),
53  epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs), blas_kernel_(blas) {}
54 
55  ConjugateGradientParameters(const ConjugateGradientParameters &p)
56  : Base(p), minIterations_(p.minIterations_), maxIterations_(p.maxIterations_), reset_(p.reset_),
57  epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_), blas_kernel_(GTSAM) {}
58 
59  /* general interface */
60  inline size_t minIterations() const { return minIterations_; }
61  inline size_t maxIterations() const { return maxIterations_; }
62  inline size_t reset() const { return reset_; }
63  inline double epsilon() const { return epsilon_rel_; }
64  inline double epsilon_rel() const { return epsilon_rel_; }
65  inline double epsilon_abs() const { return epsilon_abs_; }
66 
67  inline size_t getMinIterations() const { return minIterations_; }
68  inline size_t getMaxIterations() const { return maxIterations_; }
69  inline size_t getReset() const { return reset_; }
70  inline double getEpsilon() const { return epsilon_rel_; }
71  inline double getEpsilon_rel() const { return epsilon_rel_; }
72  inline double getEpsilon_abs() const { return epsilon_abs_; }
73 
74  inline void setMinIterations(size_t value) { minIterations_ = value; }
75  inline void setMaxIterations(size_t value) { maxIterations_ = value; }
76  inline void setReset(size_t value) { reset_ = value; }
77  inline void setEpsilon(double value) { epsilon_rel_ = value; }
78  inline void setEpsilon_rel(double value) { epsilon_rel_ = value; }
79  inline void setEpsilon_abs(double value) { epsilon_abs_ = value; }
80 
81 
82  void print() const { Base::print(); }
83  void print(std::ostream &os) const override;
84 
85  static std::string blasTranslator(const BLASKernel k) ;
86  static BLASKernel blasTranslator(const std::string &s) ;
87 };
88 
89 /*
90  * A template for the linear preconditioned conjugate gradient method.
91  * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y)
92  * leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T
93  * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book.
94  *
95  ** REFERENCES:
96  * [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems,
97  * 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281.
98  */
99 template<class S, class V>
100 V preconditionedConjugateGradient(const S &system, const V &initial,
101  const ConjugateGradientParameters &parameters) {
102 
103  V estimate, residual, direction, q1, q2;
104  estimate = residual = direction = q1 = q2 = initial;
105 
106  system.residual(estimate, q1); /* q1 = b-Ax */
107  system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */
108  system.rightPrecondition(residual, direction);/* p = L^{-T} r */
109 
110  double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta;
111 
112  const size_t iMaxIterations = parameters.maxIterations(),
113  iMinIterations = parameters.minIterations(),
114  iReset = parameters.reset() ;
115  const double threshold = std::max(parameters.epsilon_abs(),
116  parameters.epsilon() * parameters.epsilon() * currentGamma);
117 
118  if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY )
119  std::cout << "[PCG] epsilon = " << parameters.epsilon()
120  << ", max = " << parameters.maxIterations()
121  << ", reset = " << parameters.reset()
122  << ", ||r0||^2 = " << currentGamma
123  << ", threshold = " << threshold << std::endl;
124 
125  size_t k;
126  for ( k = 1 ; k <= iMaxIterations && (currentGamma > threshold || k <= iMinIterations) ; k++ ) {
127 
128  if ( k % iReset == 0 ) {
129  system.residual(estimate, q1); /* q1 = b-Ax */
130  system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */
131  system.rightPrecondition(residual, direction); /* p = L^{-T} r */
132  currentGamma = system.dot(residual, residual);
133  }
134  system.multiply(direction, q1); /* q1 = A p */
135  alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (p' A p) */
136  system.axpy(alpha, direction, estimate); /* estimate += alpha * p */
137  system.leftPrecondition(q1, q2); /* q2 = L^{-1} * q1 */
138  system.axpy(-alpha, q2, residual); /* r -= alpha * q2 */
139  prevGamma = currentGamma;
140  currentGamma = system.dot(residual, residual); /* gamma = |r|^2 */
141  beta = currentGamma / prevGamma;
142  system.rightPrecondition(residual, q1); /* q1 = L^{-T} r */
143  system.scal(beta, direction);
144  system.axpy(1.0, q1, direction); /* p = q1 + beta * p */
145 
146  if (parameters.verbosity() >= ConjugateGradientParameters::ERROR )
147  std::cout << "[PCG] k = " << k
148  << ", alpha = " << alpha
149  << ", beta = " << beta
150  << ", ||r||^2 = " << currentGamma
151 // << "\nx =\n" << estimate
152 // << "\nr =\n" << residual
153  << std::endl;
154  }
155  if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY )
156  std::cout << "[PCG] iterations = " << k
157  << ", ||r||^2 = " << currentGamma
158  << std::endl;
159 
160  return estimate;
161 }
162 
163 
164 }
gtsam::ConjugateGradientParameters::epsilon_rel_
double epsilon_rel_
threshold for relative error decrease
Definition: ConjugateGradientSolver.h:38
gtsam::ConjugateGradientParameters
parameters for the conjugate gradient method
Definition: ConjugateGradientSolver.h:29
gtsam::IterativeOptimizationParameters
parameters for iterative linear solvers
Definition: IterativeSolver.h:44
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::ConjugateGradientParameters::maxIterations_
size_t maxIterations_
maximum number of cg iterations
Definition: ConjugateGradientSolver.h:36
gtsam::ConjugateGradientParameters::BLASKernel
BLASKernel
Definition: ConjugateGradientSolver.h:42
gtsam::ConjugateGradientParameters::reset_
size_t reset_
number of iterations before reset
Definition: ConjugateGradientSolver.h:37
IterativeSolver.h
Some support classes for iterative solvers.
gtsam::ConjugateGradientParameters::minIterations_
size_t minIterations_
minimum number of cg iterations
Definition: ConjugateGradientSolver.h:35
gtsam::ConjugateGradientParameters::epsilon_abs_
double epsilon_abs_
threshold for absolute error decrease
Definition: ConjugateGradientSolver.h:39