diff --git a/nonlinear/NonlinearOptimizer.h b/nonlinear/NonlinearOptimizer.h index 172730f99..0482ee376 100644 --- a/nonlinear/NonlinearOptimizer.h +++ b/nonlinear/NonlinearOptimizer.h @@ -201,6 +201,28 @@ namespace gtsam { return result.config(); } + /** + * Static interface to GN 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 optimizeGN(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 S(ord)); + NonlinearOptimizer optimizer(graph, config, solver); + + // Gauss-Newton + NonlinearOptimizer result = optimizer.gaussNewton(relativeThreshold, + absoluteThreshold, verbosity); + return result.config(); + } + }; /** diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 50d317087..f29d0fb6a 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -175,7 +175,7 @@ TEST( NonlinearOptimizer, optimize ) } /* ************************************************************************* */ -TEST( NonlinearOptimizer, SimpleOptimizer ) +TEST( NonlinearOptimizer, SimpleLMOptimizer ) { shared_ptr fg(new example::Graph( example::createReallyNonlinearFactorGraph())); @@ -194,6 +194,26 @@ TEST( NonlinearOptimizer, SimpleOptimizer ) DOUBLES_EQUAL(0,fg->error(*actual),tol); } +/* ************************************************************************* */ +TEST( NonlinearOptimizer, SimpleGNOptimizer ) +{ + 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::optimizeGN(fg, c0); + DOUBLES_EQUAL(0,fg->error(*actual),tol); +} + /* ************************************************************************* */ TEST( NonlinearOptimizer, Factorization ) {