Added a static nonlinear optimization function that uses default ordering and thresholds and returns an updated config.

nonlinear/NonlinearOptimizer-inl.h

@@ -269,6 +269,6 @@ namespace gtsam {
 					verbosity, maxIterations-1, lambdaFactor, lambdaMode);
 	}
 
-/* ************************************************************************* */
+	/* ************************************************************************* */
 
 }

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<gtsam::Ordering> 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();
+		}
+
 	};
 
 	/**

tests/testNonlinearOptimizer.cpp

@@ -174,6 +174,26 @@ TEST( NonlinearOptimizer, optimize )
 	DOUBLES_EQUAL(0,fg->error(*(actual2.config())),tol);
 }
 
+/* ************************************************************************* */
+TEST( NonlinearOptimizer, SimpleOptimizer )
+{
+	  shared_ptr<example::Graph> 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<example::Config> 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 )
 {