Merge branch 'conjugate-gradient' into conjugate-gradient-system
commit
3fef6aa256
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
|
|
|
@ -26,59 +26,64 @@ namespace gtsam {
|
|||
/**
|
||||
* 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;
|
||||
|
||||
protected:
|
||||
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
|
||||
|
||||
public:
|
||||
/* 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) {}
|
||||
: 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(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) {}
|
||||
: 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_; }
|
||||
#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 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; }
|
||||
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(); }
|
||||
|
@ -111,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);
|
||||
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()
|
||||
<< ", max = " << parameters.maxIterations()
|
||||
<< ", reset = " << parameters.reset()
|
||||
<< ", ||r0||^2 = " << currentGamma
|
||||
<< ", threshold = " << threshold << std::endl;
|
||||
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++ ) {
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -33,30 +33,19 @@ struct PreconditionerParameters;
|
|||
/**
|
||||
* 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;
|
||||
|
||||
protected:
|
||||
std::shared_ptr<PreconditionerParameters> preconditioner_;
|
||||
std::shared_ptr<PreconditionerParameters> preconditioner;
|
||||
|
||||
public:
|
||||
PCGSolverParameters() {}
|
||||
|
||||
PCGSolverParameters(
|
||||
const std::shared_ptr<PreconditionerParameters> &preconditioner)
|
||||
: preconditioner_(preconditioner) {}
|
||||
: preconditioner(preconditioner) {}
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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)
|
||||
|
|
|
@ -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>
|
||||
|
@ -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>
|
||||
|
|
|
@ -49,19 +49,23 @@ NonlinearConjugateGradientOptimizer::NonlinearConjugateGradientOptimizer(
|
|||
params_(params) {}
|
||||
|
||||
double NonlinearConjugateGradientOptimizer::System::error(
|
||||
const Values& state) const {
|
||||
const State& state) const {
|
||||
return graph_.error(state);
|
||||
}
|
||||
|
||||
VectorValues NonlinearConjugateGradientOptimizer::System::gradient(
|
||||
const Values& state) const {
|
||||
NonlinearConjugateGradientOptimizer::System::Gradient
|
||||
NonlinearConjugateGradientOptimizer::System::gradient(
|
||||
const State& state) const {
|
||||
return gradientInPlace(graph_, state);
|
||||
}
|
||||
|
||||
Values NonlinearConjugateGradientOptimizer::System::advance(
|
||||
const Values& current, const double alpha,
|
||||
const VectorValues& gradient) const {
|
||||
return current.retract(alpha * gradient);
|
||||
NonlinearConjugateGradientOptimizer::System::State
|
||||
NonlinearConjugateGradientOptimizer::System::advance(const State& current,
|
||||
const double alpha,
|
||||
const Gradient& g) const {
|
||||
Gradient step = g;
|
||||
step *= alpha;
|
||||
return current.retract(step);
|
||||
}
|
||||
|
||||
GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() {
|
||||
|
|
|
@ -29,6 +29,8 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer
|
|||
/* a class for the nonlinearConjugateGradient template */
|
||||
class System {
|
||||
public:
|
||||
typedef Values State;
|
||||
typedef VectorValues Gradient;
|
||||
typedef NonlinearOptimizerParams Parameters;
|
||||
|
||||
protected:
|
||||
|
@ -36,10 +38,10 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer
|
|||
|
||||
public:
|
||||
System(const NonlinearFactorGraph &graph) : graph_(graph) {}
|
||||
double error(const Values &state) const;
|
||||
VectorValues gradient(const Values &state) const;
|
||||
Values advance(const Values ¤t, const double alpha,
|
||||
const VectorValues &g) const;
|
||||
double error(const State &state) const;
|
||||
Gradient gradient(const State &state) const;
|
||||
State advance(const State ¤t, const double alpha,
|
||||
const Gradient &g) const;
|
||||
};
|
||||
|
||||
public:
|
||||
|
@ -170,8 +172,8 @@ std::tuple<V, int> nonlinearConjugateGradient(
|
|||
}
|
||||
|
||||
V currentValues = initial;
|
||||
VectorValues currentGradient = system.gradient(currentValues), prevGradient,
|
||||
direction = currentGradient;
|
||||
typename S::Gradient currentGradient = system.gradient(currentValues),
|
||||
prevGradient, direction = currentGradient;
|
||||
|
||||
/* do one step of gradient descent */
|
||||
V prevValues = currentValues;
|
||||
|
|
|
@ -69,7 +69,7 @@ class TestScenario(GtsamTestCase):
|
|||
lmParams = LevenbergMarquardtParams.CeresDefaults()
|
||||
lmParams.setLinearSolverType("ITERATIVE")
|
||||
cgParams = PCGSolverParameters()
|
||||
cgParams.setPreconditionerParams(DummyPreconditionerParameters())
|
||||
cgParams.preconditioner = DummyPreconditionerParameters()
|
||||
lmParams.setIterativeParams(cgParams)
|
||||
actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values, lmParams).optimize()
|
||||
self.assertAlmostEqual(0, self.fg.error(actual))
|
||||
|
|
|
@ -95,9 +95,9 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint )
|
|||
VectorValues zeros = config.zeroVectors();
|
||||
|
||||
ConjugateGradientParameters parameters;
|
||||
parameters.setEpsilon_abs(1e-3);
|
||||
parameters.setEpsilon_rel(1e-5);
|
||||
parameters.setMaxIterations(100);
|
||||
parameters.epsilon_abs = 1e-3;
|
||||
parameters.epsilon_rel = 1e-5;
|
||||
parameters.maxIterations = 100;
|
||||
VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters);
|
||||
|
||||
VectorValues expected;
|
||||
|
@ -122,9 +122,9 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint )
|
|||
VectorValues zeros = config.zeroVectors();
|
||||
|
||||
ConjugateGradientParameters parameters;
|
||||
parameters.setEpsilon_abs(1e-3);
|
||||
parameters.setEpsilon_rel(1e-5);
|
||||
parameters.setMaxIterations(100);
|
||||
parameters.epsilon_abs = 1e-3;
|
||||
parameters.epsilon_rel = 1e-5;
|
||||
parameters.maxIterations = 100;
|
||||
VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters);
|
||||
|
||||
VectorValues expected;
|
||||
|
|
|
@ -54,23 +54,23 @@ TEST( PCGsolver, verySimpleLinearSystem) {
|
|||
// Solve the system using Preconditioned Conjugate Gradient solver
|
||||
// Common PCG parameters
|
||||
gtsam::PCGSolverParameters::shared_ptr pcg = std::make_shared<gtsam::PCGSolverParameters>();
|
||||
pcg->setMaxIterations(500);
|
||||
pcg->setEpsilon_abs(0.0);
|
||||
pcg->setEpsilon_rel(0.0);
|
||||
pcg->maxIterations = 500;
|
||||
pcg->epsilon_abs = 0.0;
|
||||
pcg->epsilon_rel = 0.0;
|
||||
//pcg->setVerbosity("ERROR");
|
||||
|
||||
// With Dummy preconditioner
|
||||
pcg->setPreconditionerParams(
|
||||
std::make_shared<gtsam::DummyPreconditionerParameters>());
|
||||
pcg->preconditioner =
|
||||
std::make_shared<gtsam::DummyPreconditionerParameters>();
|
||||
VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG);
|
||||
EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7));
|
||||
//deltaPCGDummy.print("PCG Dummy");
|
||||
|
||||
// With Block-Jacobi preconditioner
|
||||
pcg->setPreconditionerParams(
|
||||
std::make_shared<gtsam::BlockJacobiPreconditionerParameters>());
|
||||
pcg->preconditioner =
|
||||
std::make_shared<gtsam::BlockJacobiPreconditionerParameters>();
|
||||
// It takes more than 1000 iterations for this test
|
||||
pcg->setMaxIterations(1500);
|
||||
pcg->maxIterations = 1500;
|
||||
VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG);
|
||||
|
||||
EXPECT(assert_equal(exactSolution, deltaPCGJacobi, 1e-5));
|
||||
|
@ -107,21 +107,21 @@ TEST(PCGSolver, simpleLinearSystem) {
|
|||
// Solve the system using Preconditioned Conjugate Gradient solver
|
||||
// Common PCG parameters
|
||||
gtsam::PCGSolverParameters::shared_ptr pcg = std::make_shared<gtsam::PCGSolverParameters>();
|
||||
pcg->setMaxIterations(500);
|
||||
pcg->setEpsilon_abs(0.0);
|
||||
pcg->setEpsilon_rel(0.0);
|
||||
pcg->maxIterations = 500;
|
||||
pcg->epsilon_abs = 0.0;
|
||||
pcg->epsilon_rel = 0.0;
|
||||
//pcg->setVerbosity("ERROR");
|
||||
|
||||
// With Dummy preconditioner
|
||||
pcg->setPreconditionerParams(
|
||||
std::make_shared<gtsam::DummyPreconditionerParameters>());
|
||||
pcg->preconditioner =
|
||||
std::make_shared<gtsam::DummyPreconditionerParameters>();
|
||||
VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG);
|
||||
EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5));
|
||||
//deltaPCGDummy.print("PCG Dummy");
|
||||
|
||||
// With Block-Jacobi preconditioner
|
||||
pcg->setPreconditionerParams(
|
||||
std::make_shared<gtsam::BlockJacobiPreconditionerParameters>());
|
||||
pcg->preconditioner =
|
||||
std::make_shared<gtsam::BlockJacobiPreconditionerParameters>();
|
||||
VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG);
|
||||
EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5));
|
||||
//deltaPCGJacobi.print("PCG Jacobi");
|
||||
|
|
|
@ -48,7 +48,7 @@ static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
|
|||
TEST( SubgraphSolver, Parameters )
|
||||
{
|
||||
LONGS_EQUAL(SubgraphSolverParameters::SILENT, kParameters.verbosity());
|
||||
LONGS_EQUAL(500, kParameters.maxIterations());
|
||||
LONGS_EQUAL(500, kParameters.maxIterations);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
@ -83,7 +83,7 @@ int main(int argc, char* argv[]) {
|
|||
// params.setVerbosityLM("SUMMARY");
|
||||
// params.linearSolverType = LevenbergMarquardtParams::Iterative;
|
||||
// auto pcg = std::make_shared<PCGSolverParameters>();
|
||||
// pcg->preconditioner_ =
|
||||
// pcg->preconditioner =
|
||||
// std::make_shared<SubgraphPreconditionerParameters>();
|
||||
// std::make_shared<BlockJacobiPreconditionerParameters>();
|
||||
// params.iterativeParams = pcg;
|
||||
|
|
Loading…
Reference in New Issue