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

Added more static optimization helper functions - can now just call optimizeLM(graph,config) or optimizeGN(graph,config) to get an optimized config.

release/4.3a0
Alex Cunningham 2010-08-09 17:20:03 +00:00
parent fa4cf73e8e
commit 38ea7d1ea5
2 changed files with 59 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
nonlinear/NonlinearOptimizer.h
View File

@ -9,6 +9,7 @@
#define NONLINEAROPTIMIZER_H_
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include "VectorConfig.h"
#include "NonlinearFactorGraph.h"
#include "Factorization.h"
@ -192,7 +193,7 @@ namespace gtsam {
double relativeThreshold = 1e-5, absoluteThreshold = 1e-5;
// initial optimization state is the same in both cases tested
shared_solver solver(new S(ord));
shared_solver solver(new S(ord, false));
NonlinearOptimizer optimizer(graph, config, solver);
// Levenberg-Marquardt
@ -201,6 +202,15 @@ namespace gtsam {
return result.config();
}
/**
* Static interface to LM optimization (no shared_ptr arguments) - see above
*/
inline static shared_config optimizeLM(const G& graph, const T& config,
verbosityLevel verbosity = SILENT) {
return optimizeLM(boost::make_shared<const G>(graph),
boost::make_shared<const T>(config), verbosity);
}
/**
* Static interface to GN optimization using default ordering and thresholds
* @param graph Nonlinear factor graph to optimize
@ -214,7 +224,7 @@ namespace gtsam {
double relativeThreshold = 1e-5, absoluteThreshold = 1e-5;
// initial optimization state is the same in both cases tested
shared_solver solver(new S(ord));
shared_solver solver(new S(ord, false));
NonlinearOptimizer optimizer(graph, config, solver);
// Gauss-Newton
@ -223,6 +233,15 @@ namespace gtsam {
return result.config();
}
/**
* Static interface to GN optimization (no shared_ptr arguments) - see above
*/
inline static shared_config optimizeGN(const G& graph, const T& config,
verbosityLevel verbosity = SILENT) {
return optimizeGN(boost::make_shared<const G>(graph),
boost::make_shared<const T>(config), verbosity);
}
};
/**

62
tests/testNonlinearOptimizer.cpp
View File

@ -177,41 +177,55 @@ TEST( NonlinearOptimizer, optimize )
/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleLMOptimizer )
{
shared_ptr<example::Graph> fg(new example::Graph(
example::createReallyNonlinearFactorGraph()));
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);
Point2 x0(3,3);
boost::shared_ptr<example::Config> c0(new example::Config);
c0->insert(simulated2D::PoseKey(1), x0);
// 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);
}
Optimizer::shared_config actual = Optimizer::optimizeLM(fg, c0);
DOUBLES_EQUAL(0,fg->error(*actual),tol);
/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleLMOptimizer_noshared )
{
example::Graph fg = example::createReallyNonlinearFactorGraph();
Point2 x0(3,3);
example::Config c0;
c0.insert(simulated2D::PoseKey(1), x0);
Optimizer::shared_config actual = Optimizer::optimizeLM(fg, c0);
DOUBLES_EQUAL(0,fg.error(*actual),tol);
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleGNOptimizer )
{
shared_ptr<example::Graph> fg(new example::Graph(
example::createReallyNonlinearFactorGraph()));
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);
Point2 x0(3,3);
boost::shared_ptr<example::Config> c0(new example::Config);
c0->insert(simulated2D::PoseKey(1), x0);
// 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::optimizeGN(fg, c0);
DOUBLES_EQUAL(0,fg->error(*actual),tol);
}
Optimizer::shared_config actual = Optimizer::optimizeGN(fg, c0);
DOUBLES_EQUAL(0,fg->error(*actual),tol);
/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleGNOptimizer_noshared )
{
example::Graph fg = example::createReallyNonlinearFactorGraph();
Point2 x0(3,3);
example::Config c0;
c0.insert(simulated2D::PoseKey(1), x0);
Optimizer::shared_config actual = Optimizer::optimizeGN(fg, c0);
DOUBLES_EQUAL(0,fg.error(*actual),tol);
}
/* ************************************************************************* */