12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

easy interfaces of nonlinear optimization

release/4.3a0
Kai Ni 2010-10-17 18:52:52 +00:00
parent 6f2df1e036
commit 9d8498617c
10 changed files with 143 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
config.h.in
View File

@ -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

6
examples/PlanarSLAMExample.cpp
View File

@ -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);
result->print("Final Result");
Values result = optimize<Graph, Values>(graph, initialEstimate);
result.print("Final Result");
return 0;
}

5
examples/Pose2SLAMExample.cpp
View File

@ -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);
result->print("Final Result");
Values result = optimize<Graph, Values>(*graph, *initialEstimate);
result.print("Final Result");
/* 4.2.1 Alternatively, you can go through the process step by step
* Choose an ordering */

9
examples/SimpleRotation.cpp
View File

@ -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);
result->print("Final config");
Optimizer::Parameters parameters;
parameters.verbosity_ = Optimizer::Parameters::LAMBDA;
Values result = optimize<Graph, Values>(graph, initialEstimate, parameters);
result.print("Final config");
return 0;
}

2
nonlinear/Makefile.am
View File

@ -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

72
nonlinear/NonlinearOptimization-inl.h Normal file
View File

@ -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

51
nonlinear/NonlinearOptimization.h Normal file
View File

@ -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);
}

6
nonlinear/NonlinearOptimizationParameters.h
View File

@ -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){}

6
nonlinear/NonlinearOptimizer-inl.h
View File

@ -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) ;
}

4
nonlinear/NonlinearOptimizer.h
View File

@ -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