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


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:10