diff --git a/config.h.in b/config.h.in deleted file mode 100644 index b3b1ddf3d..000000000 --- a/config.h.in +++ /dev/null @@ -1,86 +0,0 @@ -/* config.h.in. Generated from configure.ac by autoheader. */ - -/* Define to 1 if you have the header file. */ -#undef HAVE_DLFCN_H - -/* Define to 1 if you have the header file. */ -#undef HAVE_INTTYPES_H - -/* Define to 1 if you have the 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 header file. */ -#undef HAVE_STDINT_H - -/* Define to 1 if you have the header file. */ -#undef HAVE_STDLIB_H - -/* Define to 1 if you have the header file. */ -#undef HAVE_STRINGS_H - -/* Define to 1 if you have the header file. */ -#undef HAVE_STRING_H - -/* Define to 1 if you have the header file. */ -#undef HAVE_SYS_STAT_H - -/* Define to 1 if you have the header file. */ -#undef HAVE_SYS_TYPES_H - -/* Define to 1 if you have the 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 does not define. */ -#undef size_t diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index c23346723..09e83e831 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -21,6 +21,7 @@ // pull in the planar SLAM domain with all typedefs and helper functions defined #include +#include 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); - - result->print("Final Result"); + Values result = optimize(graph, initialEstimate); + result.print("Final Result"); return 0; } diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index a35bc2f0b..c44e6077b 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -22,6 +22,7 @@ // pull in the Pose2 SLAM domain with all typedefs and helper functions defined #include +#include 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); - result->print("Final Result"); + Values result = optimize(*graph, *initialEstimate); + result.print("Final Result"); /* 4.2.1 Alternatively, you can go through the process step by step * Choose an ordering */ diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 811e1505c..ca319cb50 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -29,8 +29,7 @@ #include #include #include -#include - +#include /* * 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); - result->print("Final config"); + Optimizer::Parameters parameters; + parameters.verbosity_ = Optimizer::Parameters::LAMBDA; + Values result = optimize(graph, initialEstimate, parameters); + result.print("Final config"); return 0; } diff --git a/nonlinear/Makefile.am b/nonlinear/Makefile.am index d00b6cff4..3d9d58c1d 100644 --- a/nonlinear/Makefile.am +++ b/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 diff --git a/nonlinear/NonlinearOptimization-inl.h b/nonlinear/NonlinearOptimization-inl.h new file mode 100644 index 000000000..7c0c827f9 --- /dev/null +++ b/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 +#include +#include + +using namespace std; + +namespace gtsam { + + /** + * The Elimination solver + */ + template + 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 Optimizer; + typename Optimizer::shared_solver solver(new Factorization(ordering)); + Optimizer optimizer(boost::make_shared(graph), + boost::make_shared(initialEstimate), solver); + + // Levenberg-Marquardt + Optimizer result = optimizer.levenbergMarquardt(parameters); + return *result.config(); + } + + /** + * The multifrontal solver + */ + template + T optimizeMultiFrontal(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters) { + throw runtime_error("optimizeMultiFrontal: not implemented"); + } + + /** + * optimization that returns the values + */ + template + T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters, + const enum LinearSolver& solver) { + switch (solver) { + case ELIMINATION: + return optimizeElimination(graph, initialEstimate, parameters); + case MULTIFRONTAL: + return optimizeMultiFrontal(graph, initialEstimate, parameters); + } + } + +} //namespace gtsam diff --git a/nonlinear/NonlinearOptimization.h b/nonlinear/NonlinearOptimization.h new file mode 100644 index 000000000..220f14d24 --- /dev/null +++ b/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 + +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 + T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(), + const enum LinearSolver& solver = ELIMINATION); + +} + diff --git a/nonlinear/NonlinearOptimizationParameters.h b/nonlinear/NonlinearOptimizationParameters.h index 5a3f3d332..f196e5656 100644 --- a/nonlinear/NonlinearOptimizationParameters.h +++ b/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){} diff --git a/nonlinear/NonlinearOptimizer-inl.h b/nonlinear/NonlinearOptimizer-inl.h index d6a838f98..134525b4a 100644 --- a/nonlinear/NonlinearOptimizer-inl.h +++ b/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 NonlinearOptimizer NonlinearOptimizer:: - levenbergMarquardt(const NonLinearOptimizerParameters ¶) const { + levenbergMarquardt(const NonlinearOptimizationParameters ¶) 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) ; } diff --git a/nonlinear/NonlinearOptimizer.h b/nonlinear/NonlinearOptimizer.h index e5f0fb572..cd296595d 100644 --- a/nonlinear/NonlinearOptimizer.h +++ b/nonlinear/NonlinearOptimizer.h @@ -72,7 +72,7 @@ namespace gtsam { typedef boost::shared_ptr shared_graph; typedef boost::shared_ptr shared_solver; typedef const S solver; - typedef NonLinearOptimizerParameters Parameters; + typedef NonlinearOptimizationParameters Parameters; private: @@ -187,7 +187,7 @@ namespace gtsam { NonlinearOptimizer - levenbergMarquardt(const NonLinearOptimizerParameters ¶) const; + levenbergMarquardt(const NonlinearOptimizationParameters ¶) const; /** * Static interface to LM optimization using default ordering and thresholds