diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index d83385d32..1c602a963 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -183,21 +183,21 @@ void BlockJacobiPreconditioner::clean() { } /***************************************************************************************/ -boost::shared_ptr createPreconditioner(const boost::shared_ptr parameters) { - - if ( DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast(parameters) ) { +boost::shared_ptr createPreconditioner( + const boost::shared_ptr params) { + using boost::dynamic_pointer_cast; + if (dynamic_pointer_cast(params)) { return boost::make_shared(); - } - else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast(parameters) ) { + } else if (dynamic_pointer_cast( + params)) { return boost::make_shared(); - } - else if ( SubgraphPreconditionerParameters::shared_ptr subgraph = boost::dynamic_pointer_cast(parameters) ) { + } else if (auto subgraph = + dynamic_pointer_cast( + params)) { return boost::make_shared(*subgraph); } - throw invalid_argument("createPreconditioner: unexpected preconditioner parameter type"); + throw invalid_argument( + "createPreconditioner: unexpected preconditioner parameter type"); } - -} - - +} // namespace gtsam diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 0c5d99c0f..328a3facf 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -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 pcg = - boost::dynamic_pointer_cast(params.iterativeParams)) { + if (auto pcg = boost::dynamic_pointer_cast( + params.iterativeParams)) { delta = PCGSolver(*pcg).optimize(gfg); - } else if (boost::shared_ptr spcg = - boost::dynamic_pointer_cast(params.iterativeParams)) { + } else if (auto spcg = + boost::dynamic_pointer_cast( + params.iterativeParams)) { if (!params.ordering) throw std::runtime_error("SubgraphSolver needs an ordering"); delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize(); diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index dd708b37a..5d4c3f844 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -493,23 +493,24 @@ TEST(NonlinearOptimizer, disconnected_graph) { /* ************************************************************************* */ #include -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(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(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(); diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index 66b022c82..9d6cc49ac 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -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(); +TEST(PCGSolver, dummy) { + LevenbergMarquardtParams params; + params.linearSolverType = LevenbergMarquardtParams::Iterative; + auto pcg = boost::make_shared(); pcg->preconditioner_ = boost::make_shared(); - 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(); - pcg->preconditioner_ = boost::make_shared(); - paramsPCG.iterativeParams = pcg; +TEST(PCGSolver, blockjacobi) { + LevenbergMarquardtParams params; + params.linearSolverType = LevenbergMarquardtParams::Iterative; + auto pcg = boost::make_shared(); + pcg->preconditioner_ = + boost::make_shared(); + 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(); +TEST(PCGSolver, subgraph) { + LevenbergMarquardtParams params; + params.linearSolverType = LevenbergMarquardtParams::Iterative; + auto pcg = boost::make_shared(); pcg->preconditioner_ = boost::make_shared(); - 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); } /* ************************************************************************* */