12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'conjugate-gradient' into conjugate-gradient-system

release/4.3a0
Varun Agrawal 2024-10-16 10:19:14 -04:00
parent 7dd6d977fe f5f7590e78
commit 3fef6aa256
14 changed files with 132 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -94,11 +94,10 @@ int main(int argc, char* argv[]) {
parameters.maxIterations = 500; parameters.maxIterations = 500;
PCGSolverParameters::shared_ptr pcg = PCGSolverParameters::shared_ptr pcg =
std::make_shared<PCGSolverParameters>(); std::make_shared<PCGSolverParameters>();
pcg->preconditioner_ = pcg->preconditioner = std::make_shared<BlockJacobiPreconditionerParameters>();
std::make_shared<BlockJacobiPreconditionerParameters>();
// Following is crucial: // Following is crucial:
pcg->setEpsilon_abs(1e-10); pcg->epsilon_abs = 1e-10;
pcg->setEpsilon_rel(1e-10); pcg->epsilon_rel = 1e-10;
parameters.iterativeParams = pcg; parameters.iterativeParams = pcg;
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);

10
gtsam/linear/ConjugateGradientSolver.cpp
View File

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

100
gtsam/linear/ConjugateGradientSolver.h
View File

@ -26,59 +26,64 @@ namespace gtsam {
/** /**
* Parameters for the Conjugate Gradient method * Parameters for the Conjugate Gradient method
*/ */
class GTSAM_EXPORT ConjugateGradientParameters : public IterativeOptimizationParameters { struct GTSAM_EXPORT ConjugateGradientParameters
: public IterativeOptimizationParameters {
public:
typedef IterativeOptimizationParameters Base; typedef IterativeOptimizationParameters Base;
typedef std::shared_ptr<ConjugateGradientParameters> shared_ptr; typedef std::shared_ptr<ConjugateGradientParameters> shared_ptr;
protected: size_t minIterations; ///< minimum number of cg iterations
size_t minIterations_; ///< minimum number of cg iterations size_t maxIterations; ///< maximum number of cg iterations
size_t maxIterations_; ///< maximum number of cg iterations size_t reset; ///< number of iterations before reset
size_t reset_; ///< number of iterations before reset double epsilon_rel; ///< threshold for relative error decrease
double epsilon_rel_; ///< threshold for relative error decrease double epsilon_abs; ///< threshold for absolute error decrease
double epsilon_abs_; ///< threshold for absolute error decrease
public:
/* Matrix Operation Kernel */ /* Matrix Operation Kernel */
enum BLASKernel { enum BLASKernel {
GTSAM = 0, ///< Jacobian Factor Graph of GTSAM GTSAM = 0, ///< Jacobian Factor Graph of GTSAM
} blas_kernel_ ; } blas_kernel;
ConjugateGradientParameters() ConjugateGradientParameters()
: minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3), : minIterations(1),
epsilon_abs_(1e-3), blas_kernel_(GTSAM) {} 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, ConjugateGradientParameters(size_t minIterations, size_t maxIterations,
double epsilon_rel, double epsilon_abs, BLASKernel blas) size_t reset, double epsilon_rel,
: minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset), double epsilon_abs, BLASKernel blas)
epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs), blas_kernel_(blas) {} : minIterations(minIterations),
maxIterations(maxIterations),
reset(reset),
epsilon_rel(epsilon_rel),
epsilon_abs(epsilon_abs),
blas_kernel(blas) {}
ConjugateGradientParameters(const ConjugateGradientParameters &p) ConjugateGradientParameters(const ConjugateGradientParameters &p)
: Base(p), minIterations_(p.minIterations_), maxIterations_(p.maxIterations_), reset_(p.reset_), : Base(p),
epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_), blas_kernel_(GTSAM) {} minIterations(p.minIterations),
maxIterations(p.maxIterations),
reset(p.reset),
epsilon_rel(p.epsilon_rel),
epsilon_abs(p.epsilon_abs),
blas_kernel(GTSAM) {}
/* general interface */ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
inline size_t minIterations() const { return minIterations_; } inline size_t getMinIterations() const { return minIterations; }
inline size_t maxIterations() const { return maxIterations_; } inline size_t getMaxIterations() const { return maxIterations; }
inline size_t reset() const { return reset_; } inline size_t getReset() const { return reset; }
inline double epsilon() const { return epsilon_rel_; } inline double getEpsilon() const { return epsilon_rel; }
inline double epsilon_rel() const { return epsilon_rel_; } inline double getEpsilon_rel() const { return epsilon_rel; }
inline double epsilon_abs() const { return epsilon_abs_; } inline double getEpsilon_abs() const { return epsilon_abs; }
inline size_t getMinIterations() const { return minIterations_; } inline void setMinIterations(size_t value) { minIterations = value; }
inline size_t getMaxIterations() const { return maxIterations_; } inline void setMaxIterations(size_t value) { maxIterations = value; }
inline size_t getReset() const { return reset_; } inline void setReset(size_t value) { reset = value; }
inline double getEpsilon() const { return epsilon_rel_; } inline void setEpsilon(double value) { epsilon_rel = value; }
inline double getEpsilon_rel() const { return epsilon_rel_; } inline void setEpsilon_rel(double value) { epsilon_rel = value; }
inline double getEpsilon_abs() const { return epsilon_abs_; } inline void setEpsilon_abs(double value) { epsilon_abs = value; }
#endif
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; }
void print() const { Base::print(); } void print() const { Base::print(); }
@ -111,16 +116,17 @@ V preconditionedConjugateGradient(const S &system, const V &initial,
double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta; double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta;
const size_t iMaxIterations = parameters.maxIterations(), const size_t iMaxIterations = parameters.maxIterations,
iMinIterations = parameters.minIterations(), iMinIterations = parameters.minIterations,
iReset = parameters.reset() ; iReset = parameters.reset;
const double threshold = std::max(parameters.epsilon_abs(), const double threshold =
parameters.epsilon() * parameters.epsilon() * currentGamma); std::max(parameters.epsilon_abs,
parameters.epsilon_rel * parameters.epsilon_rel * currentGamma);
if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY) if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY)
std::cout << "[PCG] epsilon = " << parameters.epsilon() std::cout << "[PCG] epsilon = " << parameters.epsilon_rel
<< ", max = " << parameters.maxIterations() << ", max = " << parameters.maxIterations
<< ", reset = " << parameters.reset() << ", reset = " << parameters.reset
<< ", ||r0||^2 = " << currentGamma << ", ||r0||^2 = " << currentGamma
<< ", threshold = " << threshold << std::endl; << ", threshold = " << threshold << std::endl;

8
gtsam/linear/PCGSolver.cpp
View File

@ -34,17 +34,13 @@ namespace gtsam {
void PCGSolverParameters::print(ostream &os) const { void PCGSolverParameters::print(ostream &os) const {
Base::print(os); Base::print(os);
os << "PCGSolverParameters:" << endl; os << "PCGSolverParameters:" << endl;
preconditioner_->print(os); preconditioner->print(os);
} }
/*****************************************************************************/ /*****************************************************************************/
PCGSolver::PCGSolver(const PCGSolverParameters &p) { PCGSolver::PCGSolver(const PCGSolverParameters &p) {
parameters_ = p; parameters_ = p;
preconditioner_ = createPreconditioner(p.preconditioner()); preconditioner_ = createPreconditioner(p.preconditioner);
}
void PCGSolverParameters::setPreconditionerParams(const std::shared_ptr<PreconditionerParameters> preconditioner) {
preconditioner_ = preconditioner;
} }
void PCGSolverParameters::print(const std::string &s) const { void PCGSolverParameters::print(const std::string &s) const {

15
gtsam/linear/PCGSolver.h
View File

@ -34,29 +34,18 @@ struct PreconditionerParameters;
* Parameters for Preconditioned Conjugate Gradient solver. * Parameters for Preconditioned Conjugate Gradient solver.
*/ */
struct GTSAM_EXPORT PCGSolverParameters : public ConjugateGradientParameters { struct GTSAM_EXPORT PCGSolverParameters : public ConjugateGradientParameters {
public:
typedef ConjugateGradientParameters Base; typedef ConjugateGradientParameters Base;
typedef std::shared_ptr<PCGSolverParameters> shared_ptr; typedef std::shared_ptr<PCGSolverParameters> shared_ptr;
protected: std::shared_ptr<PreconditionerParameters> preconditioner;
std::shared_ptr<PreconditionerParameters> preconditioner_;
public:
PCGSolverParameters() {} PCGSolverParameters() {}
PCGSolverParameters( PCGSolverParameters(
const std::shared_ptr<PreconditionerParameters> &preconditioner) const std::shared_ptr<PreconditionerParameters> &preconditioner)
: preconditioner_(preconditioner) {} : preconditioner(preconditioner) {}
void print(std::ostream &os) const override; void print(std::ostream &os) const override;
const std::shared_ptr<PreconditionerParameters> preconditioner() const {
return preconditioner_;
}
void setPreconditionerParams(
const std::shared_ptr<PreconditionerParameters> preconditioner);
void print(const std::string &s) const; void print(const std::string &s) const;
}; };

17
gtsam/linear/iterative-inl.h
View File

@ -49,10 +49,12 @@ namespace gtsam {
// init gamma and calculate threshold // init gamma and calculate threshold
gamma = dot(g,g); 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 // Allocate and calculate A*d for first iteration
if (gamma > parameters_.epsilon_abs()) Ad = Ab * d; if (gamma > parameters_.epsilon_abs) Ad = Ab * d;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -79,13 +81,13 @@ namespace gtsam {
// take a step, return true if converged // take a step, return true if converged
bool step(const S& Ab, V& x) { 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); double alpha = takeOptimalStep(x);
// update gradient (or re-calculate at reset time) // 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) // axpy(alpha, Ab ^ Ad, g); // g += alpha*(Ab^Ad)
else Ab.transposeMultiplyAdd(alpha, Ad, g); else Ab.transposeMultiplyAdd(alpha, Ad, g);
@ -126,11 +128,10 @@ namespace gtsam {
CGState<S, V, E> state(Ab, x, parameters, steepest); CGState<S, V, E> state(Ab, x, parameters, steepest);
if (parameters.verbosity() != ConjugateGradientParameters::SILENT) if (parameters.verbosity() != ConjugateGradientParameters::SILENT)
std::cout << "CG: epsilon = " << parameters.epsilon() std::cout << "CG: epsilon = " << parameters.epsilon_rel
<< ", maxIterations = " << parameters.maxIterations() << ", maxIterations = " << parameters.maxIterations
<< ", ||g0||^2 = " << state.gamma << ", ||g0||^2 = " << state.gamma
<< ", threshold = " << state.threshold << ", threshold = " << state.threshold << std::endl;
<< std::endl;
if ( state.gamma < state.threshold ) { if ( state.gamma < state.threshold ) {
if (parameters.verbosity() != ConjugateGradientParameters::SILENT) if (parameters.verbosity() != ConjugateGradientParameters::SILENT)

20
gtsam/linear/linear.i
View File

@ -710,17 +710,11 @@ virtual class IterativeOptimizationParameters {
#include <gtsam/linear/ConjugateGradientSolver.h> #include <gtsam/linear/ConjugateGradientSolver.h>
virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters {
ConjugateGradientParameters(); ConjugateGradientParameters();
int getMinIterations() const ; int minIterations;
int getMaxIterations() const ; int maxIterations;
int getReset() const; int reset;
double getEpsilon_rel() const; double epsilon_rel;
double getEpsilon_abs() const; double epsilon_abs;
void setMinIterations(int value);
void setMaxIterations(int value);
void setReset(int value);
void setEpsilon_rel(double value);
void setEpsilon_abs(double value);
}; };
#include <gtsam/linear/Preconditioner.h> #include <gtsam/linear/Preconditioner.h>
@ -739,8 +733,10 @@ virtual class BlockJacobiPreconditionerParameters : gtsam::PreconditionerParamet
#include <gtsam/linear/PCGSolver.h> #include <gtsam/linear/PCGSolver.h>
virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
PCGSolverParameters(); PCGSolverParameters();
PCGSolverParameters(gtsam::PreconditionerParameters* preconditioner);
void print(string s = ""); void print(string s = "");
void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner);
gtsam::PreconditionerParameters* preconditioner;
}; };
#include <gtsam/linear/SubgraphSolver.h> #include <gtsam/linear/SubgraphSolver.h>

18
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp
View File

@ -49,19 +49,23 @@ NonlinearConjugateGradientOptimizer::NonlinearConjugateGradientOptimizer(
params_(params) {} params_(params) {}
double NonlinearConjugateGradientOptimizer::System::error( double NonlinearConjugateGradientOptimizer::System::error(
const Values& state) const { const State& state) const {
return graph_.error(state); return graph_.error(state);
} }
VectorValues NonlinearConjugateGradientOptimizer::System::gradient( NonlinearConjugateGradientOptimizer::System::Gradient
const Values& state) const { NonlinearConjugateGradientOptimizer::System::gradient(
const State& state) const {
return gradientInPlace(graph_, state); return gradientInPlace(graph_, state);
} }
Values NonlinearConjugateGradientOptimizer::System::advance( NonlinearConjugateGradientOptimizer::System::State
const Values& current, const double alpha, NonlinearConjugateGradientOptimizer::System::advance(const State& current,
const VectorValues& gradient) const { const double alpha,
return current.retract(alpha * gradient); const Gradient& g) const {
Gradient step = g;
step *= alpha;
return current.retract(step);
} }
GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() { GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() {

14
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
View File

@ -29,6 +29,8 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer
/* a class for the nonlinearConjugateGradient template */ /* a class for the nonlinearConjugateGradient template */
class System { class System {
public: public:
typedef Values State;
typedef VectorValues Gradient;
typedef NonlinearOptimizerParams Parameters; typedef NonlinearOptimizerParams Parameters;
protected: protected:
@ -36,10 +38,10 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer
public: public:
System(const NonlinearFactorGraph &graph) : graph_(graph) {} System(const NonlinearFactorGraph &graph) : graph_(graph) {}
double error(const Values &state) const; double error(const State &state) const;
VectorValues gradient(const Values &state) const; Gradient gradient(const State &state) const;
Values advance(const Values &current, const double alpha, State advance(const State &current, const double alpha,
const VectorValues &g) const; const Gradient &g) const;
}; };
public: public:
@ -170,8 +172,8 @@ std::tuple<V, int> nonlinearConjugateGradient(
} }
V currentValues = initial; V currentValues = initial;
VectorValues currentGradient = system.gradient(currentValues), prevGradient, typename S::Gradient currentGradient = system.gradient(currentValues),
direction = currentGradient; prevGradient, direction = currentGradient;
/* do one step of gradient descent */ /* do one step of gradient descent */
V prevValues = currentValues; V prevValues = currentValues;

2
python/gtsam/tests/test_NonlinearOptimizer.py
View File

@ -69,7 +69,7 @@ class TestScenario(GtsamTestCase):
lmParams = LevenbergMarquardtParams.CeresDefaults() lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setLinearSolverType("ITERATIVE") lmParams.setLinearSolverType("ITERATIVE")
cgParams = PCGSolverParameters() cgParams = PCGSolverParameters()
cgParams.setPreconditionerParams(DummyPreconditionerParameters()) cgParams.preconditioner = DummyPreconditionerParameters()
lmParams.setIterativeParams(cgParams) lmParams.setIterativeParams(cgParams)
actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values, lmParams).optimize() actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values, lmParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual)) self.assertAlmostEqual(0, self.fg.error(actual))

12
tests/testIterative.cpp
View File

@ -95,9 +95,9 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint )
VectorValues zeros = config.zeroVectors(); VectorValues zeros = config.zeroVectors();
ConjugateGradientParameters parameters; ConjugateGradientParameters parameters;
parameters.setEpsilon_abs(1e-3); parameters.epsilon_abs = 1e-3;
parameters.setEpsilon_rel(1e-5); parameters.epsilon_rel = 1e-5;
parameters.setMaxIterations(100); parameters.maxIterations = 100;
VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters);
VectorValues expected; VectorValues expected;
@ -122,9 +122,9 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint )
VectorValues zeros = config.zeroVectors(); VectorValues zeros = config.zeroVectors();
ConjugateGradientParameters parameters; ConjugateGradientParameters parameters;
parameters.setEpsilon_abs(1e-3); parameters.epsilon_abs = 1e-3;
parameters.setEpsilon_rel(1e-5); parameters.epsilon_rel = 1e-5;
parameters.setMaxIterations(100); parameters.maxIterations = 100;
VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters);
VectorValues expected; VectorValues expected;

30
tests/testPreconditioner.cpp
View File

@ -54,23 +54,23 @@ TEST( PCGsolver, verySimpleLinearSystem) {
// Solve the system using Preconditioned Conjugate Gradient solver // Solve the system using Preconditioned Conjugate Gradient solver
// Common PCG parameters // Common PCG parameters
gtsam::PCGSolverParameters::shared_ptr pcg = std::make_shared<gtsam::PCGSolverParameters>(); gtsam::PCGSolverParameters::shared_ptr pcg = std::make_shared<gtsam::PCGSolverParameters>();
pcg->setMaxIterations(500); pcg->maxIterations = 500;
pcg->setEpsilon_abs(0.0); pcg->epsilon_abs = 0.0;
pcg->setEpsilon_rel(0.0); pcg->epsilon_rel = 0.0;
//pcg->setVerbosity("ERROR"); //pcg->setVerbosity("ERROR");
// With Dummy preconditioner // With Dummy preconditioner
pcg->setPreconditionerParams( pcg->preconditioner =
std::make_shared<gtsam::DummyPreconditionerParameters>()); std::make_shared<gtsam::DummyPreconditionerParameters>();
VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG);
EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7)); EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7));
//deltaPCGDummy.print("PCG Dummy"); //deltaPCGDummy.print("PCG Dummy");
// With Block-Jacobi preconditioner // With Block-Jacobi preconditioner
pcg->setPreconditionerParams( pcg->preconditioner =
std::make_shared<gtsam::BlockJacobiPreconditionerParameters>()); std::make_shared<gtsam::BlockJacobiPreconditionerParameters>();
// It takes more than 1000 iterations for this test // It takes more than 1000 iterations for this test
pcg->setMaxIterations(1500); pcg->maxIterations = 1500;
VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG); VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG);
EXPECT(assert_equal(exactSolution, deltaPCGJacobi, 1e-5)); EXPECT(assert_equal(exactSolution, deltaPCGJacobi, 1e-5));
@ -107,21 +107,21 @@ TEST(PCGSolver, simpleLinearSystem) {
// Solve the system using Preconditioned Conjugate Gradient solver // Solve the system using Preconditioned Conjugate Gradient solver
// Common PCG parameters // Common PCG parameters
gtsam::PCGSolverParameters::shared_ptr pcg = std::make_shared<gtsam::PCGSolverParameters>(); gtsam::PCGSolverParameters::shared_ptr pcg = std::make_shared<gtsam::PCGSolverParameters>();
pcg->setMaxIterations(500); pcg->maxIterations = 500;
pcg->setEpsilon_abs(0.0); pcg->epsilon_abs = 0.0;
pcg->setEpsilon_rel(0.0); pcg->epsilon_rel = 0.0;
//pcg->setVerbosity("ERROR"); //pcg->setVerbosity("ERROR");
// With Dummy preconditioner // With Dummy preconditioner
pcg->setPreconditionerParams( pcg->preconditioner =
std::make_shared<gtsam::DummyPreconditionerParameters>()); std::make_shared<gtsam::DummyPreconditionerParameters>();
VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG);
EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5)); EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5));
//deltaPCGDummy.print("PCG Dummy"); //deltaPCGDummy.print("PCG Dummy");
// With Block-Jacobi preconditioner // With Block-Jacobi preconditioner
pcg->setPreconditionerParams( pcg->preconditioner =
std::make_shared<gtsam::BlockJacobiPreconditionerParameters>()); std::make_shared<gtsam::BlockJacobiPreconditionerParameters>();
VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG); VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG);
EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5)); EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5));
//deltaPCGJacobi.print("PCG Jacobi"); //deltaPCGJacobi.print("PCG Jacobi");

2
tests/testSubgraphSolver.cpp
View File

@ -48,7 +48,7 @@ static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
TEST( SubgraphSolver, Parameters ) TEST( SubgraphSolver, Parameters )
{ {
LONGS_EQUAL(SubgraphSolverParameters::SILENT, kParameters.verbosity()); LONGS_EQUAL(SubgraphSolverParameters::SILENT, kParameters.verbosity());
LONGS_EQUAL(500, kParameters.maxIterations()); LONGS_EQUAL(500, kParameters.maxIterations);
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
timing/timeShonanFactor.cpp
View File

@ -83,7 +83,7 @@ int main(int argc, char* argv[]) {
// params.setVerbosityLM("SUMMARY"); // params.setVerbosityLM("SUMMARY");
// params.linearSolverType = LevenbergMarquardtParams::Iterative; // params.linearSolverType = LevenbergMarquardtParams::Iterative;
// auto pcg = std::make_shared<PCGSolverParameters>(); // auto pcg = std::make_shared<PCGSolverParameters>();
// pcg->preconditioner_ = // pcg->preconditioner =
// std::make_shared<SubgraphPreconditionerParameters>(); // std::make_shared<SubgraphPreconditionerParameters>();
// std::make_shared<BlockJacobiPreconditionerParameters>(); // std::make_shared<BlockJacobiPreconditionerParameters>();
// params.iterativeParams = pcg; // params.iterativeParams = pcg;