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

Formatting only 

# Conflicts:
#	gtsam/nonlinear/NonlinearOptimizer.cpp
#	tests/testPCGSolver.cpp
release/4.3a0
Frank dellaert 2020-05-21 14:44:00 -04:00
parent c66b227410
commit 3d42e4e76a
4 changed files with 65 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/linear/Preconditioner.cpp
View File

@ -183,21 +183,21 @@ void BlockJacobiPreconditioner::clean() {
} }
/***************************************************************************************/ /***************************************************************************************/
boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters) { boost::shared_ptr<Preconditioner> createPreconditioner(
const boost::shared_ptr<PreconditionerParameters> params) {
if ( DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast<DummyPreconditionerParameters>(parameters) ) { using boost::dynamic_pointer_cast;
if (dynamic_pointer_cast<DummyPreconditionerParameters>(params)) {
return boost::make_shared<DummyPreconditioner>(); return boost::make_shared<DummyPreconditioner>();
} } else if (dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(
else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(parameters) ) { params)) {
return boost::make_shared<BlockJacobiPreconditioner>(); return boost::make_shared<BlockJacobiPreconditioner>();
} } else if (auto subgraph =
else if ( SubgraphPreconditionerParameters::shared_ptr subgraph = boost::dynamic_pointer_cast<SubgraphPreconditionerParameters>(parameters) ) { dynamic_pointer_cast<SubgraphPreconditionerParameters>(
params)) {
return boost::make_shared<SubgraphPreconditioner>(*subgraph); return boost::make_shared<SubgraphPreconditioner>(*subgraph);
} }
throw invalid_argument("createPreconditioner: unexpected preconditioner parameter type"); throw invalid_argument(
"createPreconditioner: unexpected preconditioner parameter type");
} }
} // namespace gtsam
}

12
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -147,13 +147,15 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg,
} else if (params.isIterative()) { } else if (params.isIterative()) {
// Conjugate Gradient -> needs params.iterativeParams // Conjugate Gradient -> needs params.iterativeParams
if (!params.iterativeParams) if (!params.iterativeParams)
throw std::runtime_error("NonlinearOptimizer::solve: cg parameter has to be assigned ..."); throw std::runtime_error(
"NonlinearOptimizer::solve: cg parameter has to be assigned ...");
if (boost::shared_ptr<PCGSolverParameters> pcg = if (auto pcg = boost::dynamic_pointer_cast<PCGSolverParameters>(
boost::dynamic_pointer_cast<PCGSolverParameters>(params.iterativeParams)) { params.iterativeParams)) {
delta = PCGSolver(*pcg).optimize(gfg); delta = PCGSolver(*pcg).optimize(gfg);
} else if (boost::shared_ptr<SubgraphSolverParameters> spcg = } else if (auto spcg =
boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams)) { boost::dynamic_pointer_cast<SubgraphSolverParameters>(
params.iterativeParams)) {
if (!params.ordering) if (!params.ordering)
throw std::runtime_error("SubgraphSolver needs an ordering"); throw std::runtime_error("SubgraphSolver needs an ordering");
delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize(); delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize();

12
tests/testNonlinearOptimizer.cpp
View File

@ -494,7 +494,6 @@ TEST(NonlinearOptimizer, disconnected_graph) {
#include <gtsam/linear/iterative.h> #include <gtsam/linear/iterative.h>
class IterativeLM : public LevenbergMarquardtOptimizer { class IterativeLM : public LevenbergMarquardtOptimizer {
/// Solver specific parameters /// Solver specific parameters
ConjugateGradientParameters cgParams_; ConjugateGradientParameters cgParams_;
Values initial_; Values initial_;
@ -503,9 +502,11 @@ public:
/// Constructor /// Constructor
IterativeLM(const NonlinearFactorGraph& graph, const Values& initialValues, IterativeLM(const NonlinearFactorGraph& graph, const Values& initialValues,
const ConjugateGradientParameters& p, const ConjugateGradientParameters& p,
const LevenbergMarquardtParams& params = LevenbergMarquardtParams::LegacyDefaults()) : const LevenbergMarquardtParams& params =
LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p), initial_(initialValues) { LevenbergMarquardtParams::LegacyDefaults())
} : LevenbergMarquardtOptimizer(graph, initialValues, params),
cgParams_(p),
initial_(initialValues) {}
/// Solve that uses conjugate gradient /// Solve that uses conjugate gradient
virtual VectorValues solve(const GaussianFactorGraph& gfg, virtual VectorValues solve(const GaussianFactorGraph& gfg,
@ -529,7 +530,8 @@ TEST(NonlinearOptimizer, subclass_solver) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.addPrior(X(1), Pose2(0., 0., 0.), noiseModel::Isotropic::Sigma(3, 1)); graph.addPrior(X(1), Pose2(0., 0., 0.), noiseModel::Isotropic::Sigma(3, 1));
graph += BetweenFactor<Pose2>(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1)); graph += BetweenFactor<Pose2>(X(1), X(2), Pose2(1.5, 0., 0.),
noiseModel::Isotropic::Sigma(3, 1));
graph.addPrior(X(3), Pose2(3., 0., 0.), noiseModel::Isotropic::Sigma(3, 1)); graph.addPrior(X(3), Pose2(3., 0., 0.), noiseModel::Isotropic::Sigma(3, 1));
ConjugateGradientParameters p; ConjugateGradientParameters p;

42
tests/testPCGSolver.cpp
View File

@ -126,13 +126,12 @@ TEST( GaussianFactorGraphSystem, multiply_getb)
/* ************************************************************************* */ /* ************************************************************************* */
// Test Dummy Preconditioner // Test Dummy Preconditioner
TEST( PCGSolver, dummy ) TEST(PCGSolver, dummy) {
{ LevenbergMarquardtParams params;
LevenbergMarquardtParams paramsPCG; params.linearSolverType = LevenbergMarquardtParams::Iterative;
paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; auto pcg = boost::make_shared<PCGSolverParameters>();
PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
pcg->preconditioner_ = boost::make_shared<DummyPreconditionerParameters>(); pcg->preconditioner_ = boost::make_shared<DummyPreconditionerParameters>();
paramsPCG.iterativeParams = pcg; params.iterativeParams = pcg;
NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
@ -140,20 +139,20 @@ TEST( PCGSolver, dummy )
Values c0; Values c0;
c0.insert(X(1), x0); c0.insert(X(1), x0);
Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize(); Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, params).optimize();
DOUBLES_EQUAL(0, fg.error(actualPCG), tol); DOUBLES_EQUAL(0, fg.error(actualPCG), tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Test Block-Jacobi Precondioner // Test Block-Jacobi Precondioner
TEST( PCGSolver, blockjacobi ) TEST(PCGSolver, blockjacobi) {
{ LevenbergMarquardtParams params;
LevenbergMarquardtParams paramsPCG; params.linearSolverType = LevenbergMarquardtParams::Iterative;
paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; auto pcg = boost::make_shared<PCGSolverParameters>();
PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>(); pcg->preconditioner_ =
pcg->preconditioner_ = boost::make_shared<BlockJacobiPreconditionerParameters>(); boost::make_shared<BlockJacobiPreconditionerParameters>();
paramsPCG.iterativeParams = pcg; params.iterativeParams = pcg;
NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
@ -161,20 +160,19 @@ TEST( PCGSolver, blockjacobi )
Values c0; Values c0;
c0.insert(X(1), x0); c0.insert(X(1), x0);
Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize(); Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, params).optimize();
DOUBLES_EQUAL(0, fg.error(actualPCG), tol); DOUBLES_EQUAL(0, fg.error(actualPCG), tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Test Incremental Subgraph PCG Solver // Test Incremental Subgraph PCG Solver
TEST( PCGSolver, subgraph ) TEST(PCGSolver, subgraph) {
{ LevenbergMarquardtParams params;
LevenbergMarquardtParams paramsPCG; params.linearSolverType = LevenbergMarquardtParams::Iterative;
paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; auto pcg = boost::make_shared<PCGSolverParameters>();
PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
pcg->preconditioner_ = boost::make_shared<SubgraphPreconditionerParameters>(); pcg->preconditioner_ = boost::make_shared<SubgraphPreconditionerParameters>();
paramsPCG.iterativeParams = pcg; params.iterativeParams = pcg;
NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
@ -182,7 +180,7 @@ TEST( PCGSolver, subgraph )
Values c0; Values c0;
c0.insert(X(1), x0); c0.insert(X(1), x0);
Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize(); Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, params).optimize();
DOUBLES_EQUAL(0, fg.error(actualPCG), tol); DOUBLES_EQUAL(0, fg.error(actualPCG), tol);
} }