diff --git a/nonlinear/NonlinearOptimizer-inl.h b/nonlinear/NonlinearOptimizer-inl.h index 903f045ff..636184d31 100644 --- a/nonlinear/NonlinearOptimizer-inl.h +++ b/nonlinear/NonlinearOptimizer-inl.h @@ -269,6 +269,6 @@ namespace gtsam { verbosity, maxIterations-1, lambdaFactor, lambdaMode); } -/* ************************************************************************* */ + /* ************************************************************************* */ } diff --git a/nonlinear/NonlinearOptimizer.h b/nonlinear/NonlinearOptimizer.h index 115f96bb5..730d88aa6 100644 --- a/nonlinear/NonlinearOptimizer.h +++ b/nonlinear/NonlinearOptimizer.h @@ -178,6 +178,29 @@ namespace gtsam { verbosityLevel verbosity = SILENT, int maxIterations = 100, double lambdaFactor = 10, LambdaMode lambdaMode = BOUNDED) const; + + /** + * Static interface to LM optimization using default ordering and thresholds + * @param graph Nonlinear factor graph to optimize + * @param config Initial config + * @param verbosity Integer specifying how much output to provide + * @return an optimized configuration + */ + static shared_config optimizeLM(shared_graph graph, shared_config config, + verbosityLevel verbosity = SILENT) { + boost::shared_ptr ord(new gtsam::Ordering(graph->getOrdering())); + double relativeThreshold = 1e-5, absoluteThreshold = 1e-5; + + // initial optimization state is the same in both cases tested + shared_solver solver(new NonlinearOptimizer::solver(ord)); + NonlinearOptimizer optimizer(graph, config, solver); + + // Levenberg-Marquardt + NonlinearOptimizer result = optimizer.levenbergMarquardt(relativeThreshold, + absoluteThreshold, verbosity); + return result.config(); + } + }; /** diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 872240149..50d317087 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -174,6 +174,26 @@ TEST( NonlinearOptimizer, optimize ) DOUBLES_EQUAL(0,fg->error(*(actual2.config())),tol); } +/* ************************************************************************* */ +TEST( NonlinearOptimizer, SimpleOptimizer ) +{ + 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); + + // 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); +} + /* ************************************************************************* */ TEST( NonlinearOptimizer, Factorization ) {