Skip to content

Commit

Permalink
Merge pull request #1876 from borglab/conjugate-gradient
Browse files Browse the repository at this point in the history
  • Loading branch information
@varunagrawal
varunagrawal authored Oct 18, 2024
2 parents 77422d4 + 5786073 commit dc24255
Show file tree
Hide file tree
Showing 17 changed files with 250 additions and 261 deletions.
7 changes: 3 additions & 4 deletions examples/SFMExample_SmartFactorPCG.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,11 +94,10 @@ int main(int argc, char* argv[]) {
parameters.maxIterations = 500;
PCGSolverParameters::shared_ptr pcg =
std::make_shared<PCGSolverParameters>();
pcg->preconditioner_ =
std::make_shared<BlockJacobiPreconditionerParameters>();
pcg->preconditioner = std::make_shared<BlockJacobiPreconditionerParameters>();
// Following is crucial:
pcg->setEpsilon_abs(1e-10);
pcg->setEpsilon_rel(1e-10);
pcg->epsilon_abs = 1e-10;
pcg->epsilon_rel = 1e-10;
parameters.iterativeParams = pcg;

LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
Expand Down
14 changes: 7 additions & 7 deletions gtsam/linear/ConjugateGradientSolver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,13 +26,13 @@ namespace gtsam {

/*****************************************************************************/
void ConjugateGradientParameters::print(ostream &os) const {
Base::print(os);
cout << "ConjugateGradientParameters" << endl
<< "minIter: " << minIterations_ << endl
<< "maxIter: " << maxIterations_ << endl
<< "resetIter: " << reset_ << endl
<< "eps_rel: " << epsilon_rel_ << endl
<< "eps_abs: " << epsilon_abs_ << endl;
Base::print(os);
cout << "ConjugateGradientParameters" << endl
<< "minIter: " << minIterations << endl
<< "maxIter: " << maxIterations << endl
<< "resetIter: " << reset << endl
<< "eps_rel: " << epsilon_rel << endl
<< "eps_abs: " << epsilon_abs << endl;
}

/*****************************************************************************/
Expand Down
116 changes: 62 additions & 54 deletions gtsam/linear/ConjugateGradientSolver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,59 +24,66 @@
namespace gtsam {

/**
* parameters for the conjugate gradient method
* Parameters for the Conjugate Gradient method
*/
class GTSAM_EXPORT ConjugateGradientParameters : public IterativeOptimizationParameters {

public:
struct GTSAM_EXPORT ConjugateGradientParameters
: public IterativeOptimizationParameters {
typedef IterativeOptimizationParameters Base;
typedef std::shared_ptr<ConjugateGradientParameters> shared_ptr;

size_t minIterations_; ///< minimum number of cg iterations
size_t maxIterations_; ///< maximum number of cg iterations
size_t reset_; ///< number of iterations before reset
double epsilon_rel_; ///< threshold for relative error decrease
double epsilon_abs_; ///< threshold for absolute error decrease
size_t minIterations; ///< minimum number of cg iterations
size_t maxIterations; ///< maximum number of cg iterations
size_t reset; ///< number of iterations before reset
double epsilon_rel; ///< threshold for relative error decrease
double epsilon_abs; ///< threshold for absolute error decrease

/* Matrix Operation Kernel */
enum BLASKernel {
GTSAM = 0, ///< Jacobian Factor Graph of GTSAM
} blas_kernel_ ;
GTSAM = 0, ///< Jacobian Factor Graph of GTSAM
} blas_kernel;

ConjugateGradientParameters()
: minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3),
epsilon_abs_(1e-3), blas_kernel_(GTSAM) {}

ConjugateGradientParameters(size_t minIterations, size_t maxIterations, size_t reset,
double epsilon_rel, double epsilon_abs, BLASKernel blas)
: minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset),
epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs), blas_kernel_(blas) {}
: minIterations(1),
maxIterations(500),
reset(501),
epsilon_rel(1e-3),
epsilon_abs(1e-3),
blas_kernel(GTSAM) {}

ConjugateGradientParameters(size_t minIterations, size_t maxIterations,
size_t reset, double epsilon_rel,
double epsilon_abs, BLASKernel blas)
: minIterations(minIterations),
maxIterations(maxIterations),
reset(reset),
epsilon_rel(epsilon_rel),
epsilon_abs(epsilon_abs),
blas_kernel(blas) {}

ConjugateGradientParameters(const ConjugateGradientParameters &p)
: Base(p), minIterations_(p.minIterations_), maxIterations_(p.maxIterations_), reset_(p.reset_),
epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_), blas_kernel_(GTSAM) {}

/* general interface */
inline size_t minIterations() const { return minIterations_; }
inline size_t maxIterations() const { return maxIterations_; }
inline size_t reset() const { return reset_; }
inline double epsilon() const { return epsilon_rel_; }
inline double epsilon_rel() const { return epsilon_rel_; }
inline double epsilon_abs() const { return epsilon_abs_; }

inline size_t getMinIterations() const { return minIterations_; }
inline size_t getMaxIterations() const { return maxIterations_; }
inline size_t getReset() const { return reset_; }
inline double getEpsilon() const { return epsilon_rel_; }
inline double getEpsilon_rel() const { return epsilon_rel_; }
inline double getEpsilon_abs() const { return epsilon_abs_; }

inline void setMinIterations(size_t value) { minIterations_ = value; }
inline void setMaxIterations(size_t value) { maxIterations_ = value; }
inline void setReset(size_t value) { reset_ = value; }
inline void setEpsilon(double value) { epsilon_rel_ = value; }
inline void setEpsilon_rel(double value) { epsilon_rel_ = value; }
inline void setEpsilon_abs(double value) { epsilon_abs_ = value; }
: Base(p),
minIterations(p.minIterations),
maxIterations(p.maxIterations),
reset(p.reset),
epsilon_rel(p.epsilon_rel),
epsilon_abs(p.epsilon_abs),
blas_kernel(GTSAM) {}

#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
inline size_t getMinIterations() const { return minIterations; }
inline size_t getMaxIterations() const { return maxIterations; }
inline size_t getReset() const { return reset; }
inline double getEpsilon() const { return epsilon_rel; }
inline double getEpsilon_rel() const { return epsilon_rel; }
inline double getEpsilon_abs() const { return epsilon_abs; }

inline void setMinIterations(size_t value) { minIterations = value; }
inline void setMaxIterations(size_t value) { maxIterations = value; }
inline void setReset(size_t value) { reset = value; }
inline void setEpsilon(double value) { epsilon_rel = value; }
inline void setEpsilon_rel(double value) { epsilon_rel = value; }
inline void setEpsilon_abs(double value) { epsilon_abs = value; }
#endif


void print() const { Base::print(); }
Expand Down Expand Up @@ -109,18 +116,19 @@ V preconditionedConjugateGradient(const S &system, const V &initial,

double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta;

const size_t iMaxIterations = parameters.maxIterations(),
iMinIterations = parameters.minIterations(),
iReset = parameters.reset() ;
const double threshold = std::max(parameters.epsilon_abs(),
parameters.epsilon() * parameters.epsilon() * currentGamma);

if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY )
std::cout << "[PCG] epsilon = " << parameters.epsilon()
<< ", max = " << parameters.maxIterations()
<< ", reset = " << parameters.reset()
<< ", ||r0||^2 = " << currentGamma
<< ", threshold = " << threshold << std::endl;
const size_t iMaxIterations = parameters.maxIterations,
iMinIterations = parameters.minIterations,
iReset = parameters.reset;
const double threshold =
std::max(parameters.epsilon_abs,
parameters.epsilon_rel * parameters.epsilon_rel * currentGamma);

if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY)
std::cout << "[PCG] epsilon = " << parameters.epsilon_rel
<< ", max = " << parameters.maxIterations
<< ", reset = " << parameters.reset
<< ", ||r0||^2 = " << currentGamma
<< ", threshold = " << threshold << std::endl;

size_t k;
for ( k = 1 ; k <= iMaxIterations && (currentGamma > threshold || k <= iMinIterations) ; k++ ) {
Expand Down
8 changes: 2 additions & 6 deletions gtsam/linear/PCGSolver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,17 +34,13 @@ namespace gtsam {
void PCGSolverParameters::print(ostream &os) const {
Base::print(os);
os << "PCGSolverParameters:" << endl;
preconditioner_->print(os);
preconditioner->print(os);
}

/*****************************************************************************/
PCGSolver::PCGSolver(const PCGSolverParameters &p) {
parameters_ = p;
preconditioner_ = createPreconditioner(p.preconditioner_);
}

void PCGSolverParameters::setPreconditionerParams(const std::shared_ptr<PreconditionerParameters> preconditioner) {
preconditioner_ = preconditioner;
preconditioner_ = createPreconditioner(p.preconditioner);
}

void PCGSolverParameters::print(const std::string &s) const {
Expand Down
39 changes: 16 additions & 23 deletions gtsam/linear/PCGSolver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,29 +31,22 @@ class VectorValues;
struct PreconditionerParameters;

/**
* Parameters for PCG
* Parameters for Preconditioned Conjugate Gradient solver.
*/
struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters {
public:
struct GTSAM_EXPORT PCGSolverParameters : public ConjugateGradientParameters {
typedef ConjugateGradientParameters Base;
typedef std::shared_ptr<PCGSolverParameters> shared_ptr;

PCGSolverParameters() {
}
std::shared_ptr<PreconditionerParameters> preconditioner;

void print(std::ostream &os) const override;
PCGSolverParameters() {}

/* interface to preconditioner parameters */
inline const PreconditionerParameters& preconditioner() const {
return *preconditioner_;
}
PCGSolverParameters(
const std::shared_ptr<PreconditionerParameters> &preconditioner)
: preconditioner(preconditioner) {}

// needed for python wrapper
void print(std::ostream &os) const override;
void print(const std::string &s) const;

std::shared_ptr<PreconditionerParameters> preconditioner_;

void setPreconditionerParams(const std::shared_ptr<PreconditionerParameters> preconditioner);
};

/**
Expand Down Expand Up @@ -87,16 +80,16 @@ class GTSAM_EXPORT PCGSolver: public IterativeSolver {
* System class needed for calling preconditionedConjugateGradient
*/
class GTSAM_EXPORT GaussianFactorGraphSystem {
public:

GaussianFactorGraphSystem(const GaussianFactorGraph &gfg,
const Preconditioner &preconditioner, const KeyInfo &info,
const std::map<Key, Vector> &lambda);

const GaussianFactorGraph &gfg_;
const Preconditioner &preconditioner_;
const KeyInfo &keyInfo_;
const std::map<Key, Vector> &lambda_;
KeyInfo keyInfo_;
std::map<Key, Vector> lambda_;

public:
GaussianFactorGraphSystem(const GaussianFactorGraph &gfg,
const Preconditioner &preconditioner,
const KeyInfo &info,
const std::map<Key, Vector> &lambda);

void residual(const Vector &x, Vector &r) const;
void multiply(const Vector &x, Vector& y) const;
Expand Down
17 changes: 9 additions & 8 deletions gtsam/linear/iterative-inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,10 +49,12 @@ namespace gtsam {

// init gamma and calculate threshold
gamma = dot(g,g);
threshold = std::max(parameters_.epsilon_abs(), parameters_.epsilon() * parameters_.epsilon() * gamma);
threshold =
std::max(parameters_.epsilon_abs,
parameters_.epsilon_rel * parameters_.epsilon_rel * gamma);

// Allocate and calculate A*d for first iteration
if (gamma > parameters_.epsilon_abs()) Ad = Ab * d;
if (gamma > parameters_.epsilon_abs) Ad = Ab * d;
}

/* ************************************************************************* */
Expand All @@ -79,13 +81,13 @@ namespace gtsam {
// take a step, return true if converged
bool step(const S& Ab, V& x) {

if ((++k) >= ((int)parameters_.maxIterations())) return true;
if ((++k) >= ((int)parameters_.maxIterations)) return true;

//---------------------------------->
double alpha = takeOptimalStep(x);

// update gradient (or re-calculate at reset time)
if (k % parameters_.reset() == 0) g = Ab.gradient(x);
if (k % parameters_.reset == 0) g = Ab.gradient(x);
// axpy(alpha, Ab ^ Ad, g); // g += alpha*(Ab^Ad)
else Ab.transposeMultiplyAdd(alpha, Ad, g);

Expand Down Expand Up @@ -126,11 +128,10 @@ namespace gtsam {
CGState<S, V, E> state(Ab, x, parameters, steepest);

if (parameters.verbosity() != ConjugateGradientParameters::SILENT)
std::cout << "CG: epsilon = " << parameters.epsilon()
<< ", maxIterations = " << parameters.maxIterations()
std::cout << "CG: epsilon = " << parameters.epsilon_rel
<< ", maxIterations = " << parameters.maxIterations
<< ", ||g0||^2 = " << state.gamma
<< ", threshold = " << state.threshold
<< std::endl;
<< ", threshold = " << state.threshold << std::endl;

if ( state.gamma < state.threshold ) {
if (parameters.verbosity() != ConjugateGradientParameters::SILENT)
Expand Down
20 changes: 8 additions & 12 deletions gtsam/linear/linear.i
View file
Original file line number Diff line number Diff line change
Expand Up @@ -710,17 +710,11 @@ virtual class IterativeOptimizationParameters {
#include <gtsam/linear/ConjugateGradientSolver.h>
virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters {
ConjugateGradientParameters();
int getMinIterations() const ;
int getMaxIterations() const ;
int getReset() const;
double getEpsilon_rel() const;
double getEpsilon_abs() const;

void setMinIterations(int value);
void setMaxIterations(int value);
void setReset(int value);
void setEpsilon_rel(double value);
void setEpsilon_abs(double value);
int minIterations;
int maxIterations;
int reset;
double epsilon_rel;
double epsilon_abs;
};

#include <gtsam/linear/Preconditioner.h>
Expand All @@ -739,8 +733,10 @@ virtual class BlockJacobiPreconditionerParameters : gtsam::PreconditionerParamet
#include <gtsam/linear/PCGSolver.h>
virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
PCGSolverParameters();
PCGSolverParameters(gtsam::PreconditionerParameters* preconditioner);
void print(string s = "");
void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner);

gtsam::PreconditionerParameters* preconditioner;
};

#include <gtsam/linear/SubgraphSolver.h>
Expand Down
Loading

0 comments on commit dc24255

Please sign in to comment.