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) {
if ( DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast<DummyPreconditionerParameters>(parameters) ) {
boost::shared_ptr<Preconditioner> createPreconditioner(
const boost::shared_ptr<PreconditionerParameters> params) {
using boost::dynamic_pointer_cast;
if (dynamic_pointer_cast<DummyPreconditionerParameters>(params)) {
return boost::make_shared<DummyPreconditioner>();
}
else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(parameters) ) {
} else if (dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(
params)) {
return boost::make_shared<BlockJacobiPreconditioner>();
}
else if ( SubgraphPreconditionerParameters::shared_ptr subgraph = boost::dynamic_pointer_cast<SubgraphPreconditionerParameters>(parameters) ) {
} else if (auto subgraph =
dynamic_pointer_cast<SubgraphPreconditionerParameters>(
params)) {
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()) {
// Conjugate Gradient -> needs 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 =
boost::dynamic_pointer_cast<PCGSolverParameters>(params.iterativeParams)) {
if (auto pcg = boost::dynamic_pointer_cast<PCGSolverParameters>(
params.iterativeParams)) {
delta = PCGSolver(*pcg).optimize(gfg);
} else if (boost::shared_ptr<SubgraphSolverParameters> spcg =
boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams)) {
} else if (auto spcg =
boost::dynamic_pointer_cast<SubgraphSolverParameters>(
params.iterativeParams)) {
if (!params.ordering)
throw std::runtime_error("SubgraphSolver needs an ordering");
delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize();

38
tests/testNonlinearOptimizer.cpp
View File

@ -493,23 +493,24 @@ TEST(NonlinearOptimizer, disconnected_graph) {
/* ************************************************************************* */
#include <gtsam/linear/iterative.h>
class IterativeLM: public LevenbergMarquardtOptimizer {
class IterativeLM : public LevenbergMarquardtOptimizer {
/// Solver specific parameters
ConjugateGradientParameters cgParams_;
Values initial_;
public:
public:
/// Constructor
IterativeLM(const NonlinearFactorGraph& graph, const Values& initialValues,
const ConjugateGradientParameters &p,
const LevenbergMarquardtParams& params = LevenbergMarquardtParams::LegacyDefaults()) :
LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p), initial_(initialValues) {
}
const ConjugateGradientParameters& p,
const LevenbergMarquardtParams& params =
LevenbergMarquardtParams::LegacyDefaults())
: LevenbergMarquardtOptimizer(graph, initialValues, params),
cgParams_(p),
initial_(initialValues) {}
/// Solve that uses conjugate gradient
virtual VectorValues solve(const GaussianFactorGraph &gfg,
const NonlinearOptimizerParams& params) const {
virtual VectorValues solve(const GaussianFactorGraph& gfg,
const NonlinearOptimizerParams& params) const {
VectorValues zeros = initial_.zeroVectors();
return conjugateGradientDescent(gfg, zeros, cgParams_);
}
@ -518,19 +519,20 @@ public:
/* ************************************************************************* */
TEST(NonlinearOptimizer, subclass_solver) {
Values expected;
expected.insert(X(1), Pose2(0.,0.,0.));
expected.insert(X(2), Pose2(1.5,0.,0.));
expected.insert(X(3), Pose2(3.0,0.,0.));
expected.insert(X(1), Pose2(0., 0., 0.));
expected.insert(X(2), Pose2(1.5, 0., 0.));
expected.insert(X(3), Pose2(3.0, 0., 0.));
Values init;
init.insert(X(1), Pose2(0.,0.,0.));
init.insert(X(2), Pose2(0.,0.,0.));
init.insert(X(3), Pose2(0.,0.,0.));
init.insert(X(1), Pose2(0., 0., 0.));
init.insert(X(2), Pose2(0., 0., 0.));
init.insert(X(3), Pose2(0., 0., 0.));
NonlinearFactorGraph graph;
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.addPrior(X(3), Pose2(3.,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.addPrior(X(3), Pose2(3., 0., 0.), noiseModel::Isotropic::Sigma(3, 1));
ConjugateGradientParameters p;
Values actual = IterativeLM(graph, init, p).optimize();

54
tests/testPCGSolver.cpp
View File

@ -126,65 +126,63 @@ TEST( GaussianFactorGraphSystem, multiply_getb)
/* ************************************************************************* */
// Test Dummy Preconditioner
TEST( PCGSolver, dummy )
{
LevenbergMarquardtParams paramsPCG;
paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
TEST(PCGSolver, dummy) {
LevenbergMarquardtParams params;
params.linearSolverType = LevenbergMarquardtParams::Iterative;
auto pcg = boost::make_shared<PCGSolverParameters>();
pcg->preconditioner_ = boost::make_shared<DummyPreconditionerParameters>();
paramsPCG.iterativeParams = pcg;
params.iterativeParams = pcg;
NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
Point2 x0(10,10);
Point2 x0(10, 10);
Values c0;
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( PCGSolver, blockjacobi )
{
LevenbergMarquardtParams paramsPCG;
paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
pcg->preconditioner_ = boost::make_shared<BlockJacobiPreconditionerParameters>();
paramsPCG.iterativeParams = pcg;
TEST(PCGSolver, blockjacobi) {
LevenbergMarquardtParams params;
params.linearSolverType = LevenbergMarquardtParams::Iterative;
auto pcg = boost::make_shared<PCGSolverParameters>();
pcg->preconditioner_ =
boost::make_shared<BlockJacobiPreconditionerParameters>();
params.iterativeParams = pcg;
NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
Point2 x0(10,10);
Point2 x0(10, 10);
Values c0;
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( PCGSolver, subgraph )
{
LevenbergMarquardtParams paramsPCG;
paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
TEST(PCGSolver, subgraph) {
LevenbergMarquardtParams params;
params.linearSolverType = LevenbergMarquardtParams::Iterative;
auto pcg = boost::make_shared<PCGSolverParameters>();
pcg->preconditioner_ = boost::make_shared<SubgraphPreconditionerParameters>();
paramsPCG.iterativeParams = pcg;
params.iterativeParams = pcg;
NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
Point2 x0(10,10);
Point2 x0(10, 10);
Values c0;
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);
}
/* ************************************************************************* */