33 typedef std::shared_ptr<ConjugateGradientParameters>
shared_ptr;
47 : minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1
e-3),
48 epsilon_abs_(1
e-3), blas_kernel_(GTSAM) {}
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) {}
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) {}
62 inline size_t reset()
const {
return reset_; }
63 inline double epsilon()
const {
return epsilon_rel_; }
69 inline size_t getReset()
const {
return reset_; }
70 inline double getEpsilon()
const {
return epsilon_rel_; }
83 void print(std::ostream &
os)
const override;
85 static std::string blasTranslator(
const BLASKernel k) ;
86 static BLASKernel blasTranslator(
const std::string &
s) ;
99 template<
class S,
class V>
103 V estimate, residual, direction, q1, q2;
104 estimate = residual = direction = q1 = q2 =
initial;
106 system.residual(estimate, q1);
107 system.leftPrecondition(q1, residual);
108 system.rightPrecondition(residual, direction);
110 double currentGamma = system.dot(residual, residual), prevGamma,
alpha,
beta;
122 <<
", ||r0||^2 = " << currentGamma
123 <<
", threshold = " << threshold << std::endl;
126 for ( k = 1 ; k <= iMaxIterations && (currentGamma > threshold || k <= iMinIterations) ; k++ ) {
128 if ( k % iReset == 0 ) {
129 system.residual(estimate, q1);
130 system.leftPrecondition(q1, residual);
131 system.rightPrecondition(residual, direction);
132 currentGamma = system.dot(residual, residual);
134 system.multiply(direction, q1);
135 alpha = currentGamma / system.dot(direction, q1);
136 system.axpy(
alpha, direction, estimate);
137 system.leftPrecondition(q1, q2);
138 system.axpy(-
alpha, q2, residual);
139 prevGamma = currentGamma;
140 currentGamma = system.dot(residual, residual);
141 beta = currentGamma / prevGamma;
142 system.rightPrecondition(residual, q1);
143 system.scal(
beta, direction);
144 system.axpy(1.0, q1, direction);
147 std::cout <<
"[PCG] k = " << k
148 <<
", alpha = " <<
alpha
149 <<
", beta = " <<
beta
150 <<
", ||r||^2 = " << currentGamma
156 std::cout <<
"[PCG] iterations = " << k
157 <<
", ||r||^2 = " << currentGamma