easy interfaces of nonlinear optimization

config.h.in

@@ -1,86 +0,0 @@
-/* config.h.in.  Generated from configure.ac by autoheader.  */
-
-/* Define to 1 if you have the <dlfcn.h> header file. */
-#undef HAVE_DLFCN_H
-
-/* Define to 1 if you have the <inttypes.h> header file. */
-#undef HAVE_INTTYPES_H
-
-/* Define to 1 if you have the <memory.h> header file. */
-#undef HAVE_MEMORY_H
-
-/* Define to 1 if you have the `pow' function. */
-#undef HAVE_POW
-
-/* Define to 1 if you have the `sqrt' function. */
-#undef HAVE_SQRT
-
-/* Define to 1 if stdbool.h conforms to C99. */
-#undef HAVE_STDBOOL_H
-
-/* Define to 1 if you have the <stdint.h> header file. */
-#undef HAVE_STDINT_H
-
-/* Define to 1 if you have the <stdlib.h> header file. */
-#undef HAVE_STDLIB_H
-
-/* Define to 1 if you have the <strings.h> header file. */
-#undef HAVE_STRINGS_H
-
-/* Define to 1 if you have the <string.h> header file. */
-#undef HAVE_STRING_H
-
-/* Define to 1 if you have the <sys/stat.h> header file. */
-#undef HAVE_SYS_STAT_H
-
-/* Define to 1 if you have the <sys/types.h> header file. */
-#undef HAVE_SYS_TYPES_H
-
-/* Define to 1 if you have the <unistd.h> header file. */
-#undef HAVE_UNISTD_H
-
-/* Define to 1 if the system has the type `_Bool'. */
-#undef HAVE__BOOL
-
-/* Define to the sub-directory in which libtool stores uninstalled libraries.
-   */
-#undef LT_OBJDIR
-
-/* Name of package */
-#undef PACKAGE
-
-/* Define to the address where bug reports for this package should be sent. */
-#undef PACKAGE_BUGREPORT
-
-/* Define to the full name of this package. */
-#undef PACKAGE_NAME
-
-/* Define to the full name and version of this package. */
-#undef PACKAGE_STRING
-
-/* Define to the one symbol short name of this package. */
-#undef PACKAGE_TARNAME
-
-/* Define to the home page for this package. */
-#undef PACKAGE_URL
-
-/* Define to the version of this package. */
-#undef PACKAGE_VERSION
-
-/* Define to 1 if you have the ANSI C header files. */
-#undef STDC_HEADERS
-
-/* Version number of package */
-#undef VERSION
-
-/* Define to empty if `const' does not conform to ANSI C. */
-#undef const
-
-/* Define to `__inline__' or `__inline' if that's what the C compiler
-   calls it, or to nothing if 'inline' is not supported under any name.  */
-#ifndef __cplusplus
-#undef inline
-#endif
-
-/* Define to `unsigned int' if <sys/types.h> does not define. */
-#undef size_t

examples/PlanarSLAMExample.cpp

@@ -21,6 +21,7 @@
 
 // pull in the planar SLAM domain with all typedefs and helper functions defined
 #include <gtsam/slam/planarSLAM.h>
+#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
 
 using namespace std;
 using namespace gtsam;
@@ -86,9 +87,8 @@ int main(int argc, char** argv) {
 	initialEstimate.print("Initial Estimate");
 
 	// optimize using Levenberg-Marquardt optimization with an ordering from colamd
-	Optimizer::shared_values result = Optimizer::optimizeLM(graph, initialEstimate);
+	Values result = optimize<Graph, Values>(graph, initialEstimate);
-
+	result.print("Final Result");
-	result->print("Final Result");
 
 	return 0;
 }

examples/Pose2SLAMExample.cpp

@@ -22,6 +22,7 @@
 
 // pull in the Pose2 SLAM domain with all typedefs and helper functions defined
 #include <gtsam/slam/pose2SLAM.h>
+#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
 
 using namespace std;
 using namespace gtsam;
@@ -66,8 +67,8 @@ int main(int argc, char** argv) {
 
 	/* 4.1 Single Step:
 	* optimize using Levenberg-Marquardt optimization with an ordering from colamd */
-  Optimizer::shared_values result = Optimizer::optimizeLM(*graph, *initialEstimate);
+  Values result = optimize<Graph, Values>(*graph, *initialEstimate);
-  result->print("Final Result");
+  result.print("Final Result");
 
 	/* 4.2.1 Alternatively, you can go through the process step by step
 	 * Choose an ordering */

examples/SimpleRotation.cpp

@@ -29,8 +29,7 @@
 #include <gtsam/nonlinear/Key.h>
 #include <gtsam/nonlinear/LieValues-inl.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
-#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
+#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
-
 /*
  * TODO: make factors independent of Values
  * TODO: get rid of excessive shared pointer stuff: mostly gone
@@ -70,8 +69,10 @@ int main() {
 	initialEstimate.print("Initialization");
 
 	// create an ordering
-	Optimizer::shared_values result = Optimizer::optimizeLM(graph, initialEstimate, Optimizer::Parameters::LAMBDA);
+	Optimizer::Parameters parameters;
-	result->print("Final config");
+	parameters.verbosity_ = Optimizer::Parameters::LAMBDA;
+	Values result = optimize<Graph, Values>(graph, initialEstimate, parameters);
+	result.print("Final config");
 
 	return 0;
 }

nonlinear/Makefile.am

@@ -21,7 +21,7 @@ check_PROGRAMS += tests/testLieValues tests/testKey
 
 # Nonlinear nonlinear 
 headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
-headers += NonlinearOptimizer-inl.h
+headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h
 headers += NonlinearFactor.h
 sources += NonlinearOptimizer.cpp Ordering.cpp
 

nonlinear/NonlinearOptimization-inl.h

@@ -0,0 +1,72 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * NonlinearOptimization-inl.h
+ *
+ *   Created on: Oct 17, 2010
+ *       Author: Kai Ni
+ *  Description: Easy interfaces for NonlinearOptimizer
+ */
+
+#pragma once
+
+#include <gtsam/linear/Factorization.h>
+#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
+#include <gtsam/nonlinear/NonlinearOptimization.h>
+
+using namespace std;
+
+namespace gtsam {
+
+	/**
+	 * The Elimination solver
+	 */
+	template<class G, class T>
+	T	optimizeElimination(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters) {
+
+		// Use a variable ordering from COLAMD
+	  Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
+
+		// initial optimization state is the same in both cases tested
+	  typedef NonlinearOptimizer<G, T> Optimizer;
+	  typename Optimizer::shared_solver solver(new Factorization<G, T>(ordering));
+	  Optimizer optimizer(boost::make_shared<const G>(graph),
+	  		boost::make_shared<const T>(initialEstimate), solver);
+
+		// Levenberg-Marquardt
+	  Optimizer result = optimizer.levenbergMarquardt(parameters);
+		return *result.config();
+	}
+
+	/**
+	 * The multifrontal solver
+	 */
+	template<class G, class T>
+	T	optimizeMultiFrontal(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters) {
+		throw runtime_error("optimizeMultiFrontal: not implemented");
+	}
+
+	/**
+	 * optimization that returns the values
+	 */
+	template<class G, class T>
+	T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters,
+			const enum LinearSolver& solver) {
+		switch (solver) {
+		case ELIMINATION:
+			return optimizeElimination<G,T>(graph, initialEstimate, parameters);
+		case MULTIFRONTAL:
+			return optimizeMultiFrontal<G,T>(graph, initialEstimate, parameters);
+		}
+	}
+
+} //namespace gtsam

nonlinear/NonlinearOptimization.h

@@ -0,0 +1,51 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * NonlinearOptimization.h
+ *
+ *   Created on: Oct 14, 2010
+ *       Author: Kai Ni
+ *  Description: Easy interfaces for NonlinearOptimizer
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearOptimizationParameters.h>
+
+namespace gtsam {
+
+	/**
+	 * all the nonlinear optimization methods
+	 */
+	enum NonlinearOptimizationMethod {
+		LM,            // Levenberg Marquardt
+		GN             // Gaussian-Newton
+	};
+
+	/**
+	 * all the linear solver types
+	 */
+	enum LinearSolver{
+		ELIMINATION,      // Elimination
+		MULTIFRONTAL      // Multi-frontal
+	};
+
+
+	/**
+	 * optimization that returns the values
+	 */
+	template<class G, class T>
+	T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
+			const enum LinearSolver& solver = ELIMINATION);
+
+}
+

nonlinear/NonlinearOptimizationParameters.h

@@ -11,7 +11,7 @@
 namespace gtsam {
 
 	// a container for all related parameters
-	struct NonLinearOptimizerParameters {
+	struct NonlinearOptimizationParameters {
 		typedef enum {
 			SILENT,
 			ERROR,
@@ -39,10 +39,10 @@ namespace gtsam {
 		verbosityLevel verbosity_;
 		LambdaMode lambdaMode_;
 
-		NonLinearOptimizerParameters(): absDecrease_(1), relDecrease_(1e-3), sumError_(0.0),
+		NonlinearOptimizationParameters(): absDecrease_(1), relDecrease_(1e-3), sumError_(0.0),
 		maxIterations_(100), lambdaFactor_(10.0), verbosity_(ERROR), lambdaMode_(BOUNDED){}
 
-		NonLinearOptimizerParameters(double absDecrease, double relDecrease, double sumError,
+		NonlinearOptimizationParameters(double absDecrease, double relDecrease, double sumError,
 				int iIters = 100, double lambdaFactor = 10, verbosityLevel v = ERROR, LambdaMode lambdaMode = BOUNDED)
 		:absDecrease_(absDecrease), relDecrease_(relDecrease), sumError_(sumError),
 		 maxIterations_(iIters), lambdaFactor_(lambdaFactor), verbosity_(v), lambdaMode_(lambdaMode){}

nonlinear/NonlinearOptimizer-inl.h

@@ -264,14 +264,14 @@ namespace gtsam {
 			double relativeThreshold, double absoluteThreshold,
 			Parameters::verbosityLevel verbosity, int maxIterations, double lambdaFactor, Parameters::LambdaMode lambdaMode) const {
 
-		return levenbergMarquardt(NonLinearOptimizerParameters (absoluteThreshold, relativeThreshold, absoluteThreshold,
+		return levenbergMarquardt(NonlinearOptimizationParameters (absoluteThreshold, relativeThreshold, absoluteThreshold,
 				maxIterations, lambdaFactor, verbosity, lambdaMode)) ;
 	}
 
 
 	template<class G, class C, class L, class S, class W>
 	NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::
-	levenbergMarquardt(const NonLinearOptimizerParameters &para) const {
+	levenbergMarquardt(const NonlinearOptimizationParameters &para) const {
 
 		if (para.maxIterations_ <= 0) return *this;
 
@@ -307,7 +307,7 @@ namespace gtsam {
 				cout << "final lambda = " << next.lambda_ << endl;
 			return next;
 		} else {
-			NonLinearOptimizerParameters newPara = para ;
+			NonlinearOptimizationParameters newPara = para ;
 			newPara.maxIterations_ = newPara.maxIterations_ - 1;
 			return next.levenbergMarquardt(newPara) ;
 		}

nonlinear/NonlinearOptimizer.h

@@ -72,7 +72,7 @@ namespace gtsam {
 		typedef boost::shared_ptr<const G> shared_graph;
 		typedef boost::shared_ptr<const S> shared_solver;
 		typedef const S solver;
-		typedef NonLinearOptimizerParameters Parameters;
+		typedef NonlinearOptimizationParameters Parameters;
 
 	private:
 
@@ -187,7 +187,7 @@ namespace gtsam {
 
 
 		NonlinearOptimizer
-		levenbergMarquardt(const NonLinearOptimizerParameters &para) const;
+		levenbergMarquardt(const NonlinearOptimizationParameters &para) const;
 
 		/**
 		 * Static interface to LM optimization using default ordering and thresholds