diff --git a/nonlinear/NonlinearOptimizer.h b/nonlinear/NonlinearOptimizer.h index 0482ee376..ea6c6dbd5 100644 --- a/nonlinear/NonlinearOptimizer.h +++ b/nonlinear/NonlinearOptimizer.h @@ -9,6 +9,7 @@ #define NONLINEAROPTIMIZER_H_ #include +#include #include "VectorConfig.h" #include "NonlinearFactorGraph.h" #include "Factorization.h" @@ -192,7 +193,7 @@ namespace gtsam { double relativeThreshold = 1e-5, absoluteThreshold = 1e-5; // initial optimization state is the same in both cases tested - shared_solver solver(new S(ord)); + shared_solver solver(new S(ord, false)); NonlinearOptimizer optimizer(graph, config, solver); // Levenberg-Marquardt @@ -201,6 +202,15 @@ namespace gtsam { return result.config(); } + /** + * Static interface to LM optimization (no shared_ptr arguments) - see above + */ + inline static shared_config optimizeLM(const G& graph, const T& config, + verbosityLevel verbosity = SILENT) { + return optimizeLM(boost::make_shared(graph), + boost::make_shared(config), verbosity); + } + /** * Static interface to GN optimization using default ordering and thresholds * @param graph Nonlinear factor graph to optimize @@ -214,7 +224,7 @@ namespace gtsam { double relativeThreshold = 1e-5, absoluteThreshold = 1e-5; // initial optimization state is the same in both cases tested - shared_solver solver(new S(ord)); + shared_solver solver(new S(ord, false)); NonlinearOptimizer optimizer(graph, config, solver); // Gauss-Newton @@ -223,6 +233,15 @@ namespace gtsam { return result.config(); } + /** + * Static interface to GN optimization (no shared_ptr arguments) - see above + */ + inline static shared_config optimizeGN(const G& graph, const T& config, + verbosityLevel verbosity = SILENT) { + return optimizeGN(boost::make_shared(graph), + boost::make_shared(config), verbosity); + } + }; /** diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 2c7cb73bc..093e9aed4 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -177,41 +177,55 @@ TEST( NonlinearOptimizer, optimize ) /* ************************************************************************* */ TEST( NonlinearOptimizer, SimpleLMOptimizer ) { - shared_ptr fg(new example::Graph( - example::createReallyNonlinearFactorGraph())); + shared_ptr fg(new example::Graph( + example::createReallyNonlinearFactorGraph())); - // test error at minimum - Point2 xstar(0,0); - example::Config cstar; - cstar.insert(simulated2D::PoseKey(1), xstar); + Point2 x0(3,3); + boost::shared_ptr c0(new example::Config); + c0->insert(simulated2D::PoseKey(1), x0); - // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = - Point2 x0(3,3); - boost::shared_ptr c0(new example::Config); - c0->insert(simulated2D::PoseKey(1), x0); + Optimizer::shared_config actual = Optimizer::optimizeLM(fg, c0); + DOUBLES_EQUAL(0,fg->error(*actual),tol); +} - Optimizer::shared_config actual = Optimizer::optimizeLM(fg, c0); - DOUBLES_EQUAL(0,fg->error(*actual),tol); +/* ************************************************************************* */ +TEST( NonlinearOptimizer, SimpleLMOptimizer_noshared ) +{ + example::Graph fg = example::createReallyNonlinearFactorGraph(); + + Point2 x0(3,3); + example::Config c0; + c0.insert(simulated2D::PoseKey(1), x0); + + Optimizer::shared_config actual = Optimizer::optimizeLM(fg, c0); + DOUBLES_EQUAL(0,fg.error(*actual),tol); } /* ************************************************************************* */ TEST( NonlinearOptimizer, SimpleGNOptimizer ) { - shared_ptr fg(new example::Graph( - example::createReallyNonlinearFactorGraph())); + shared_ptr fg(new example::Graph( + example::createReallyNonlinearFactorGraph())); - // test error at minimum - Point2 xstar(0,0); - example::Config cstar; - cstar.insert(simulated2D::PoseKey(1), xstar); + Point2 x0(3,3); + boost::shared_ptr c0(new example::Config); + c0->insert(simulated2D::PoseKey(1), x0); - // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = - Point2 x0(3,3); - boost::shared_ptr c0(new example::Config); - c0->insert(simulated2D::PoseKey(1), x0); + Optimizer::shared_config actual = Optimizer::optimizeGN(fg, c0); + DOUBLES_EQUAL(0,fg->error(*actual),tol); +} - Optimizer::shared_config actual = Optimizer::optimizeGN(fg, c0); - DOUBLES_EQUAL(0,fg->error(*actual),tol); +/* ************************************************************************* */ +TEST( NonlinearOptimizer, SimpleGNOptimizer_noshared ) +{ + example::Graph fg = example::createReallyNonlinearFactorGraph(); + + Point2 x0(3,3); + example::Config c0; + c0.insert(simulated2D::PoseKey(1), x0); + + Optimizer::shared_config actual = Optimizer::optimizeGN(fg, c0); + DOUBLES_EQUAL(0,fg.error(*actual),tol); } /* ************************************************************************* */