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

Added a Gauss-Newton static function for nonlinear optimization

release/4.3a0
Alex Cunningham 2010-08-06 15:10:19 +00:00
parent 3dfbb26bc7
commit 7e3442286e
2 changed files with 43 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
nonlinear/NonlinearOptimizer.h
View File

@ -201,6 +201,28 @@ namespace gtsam {
return result.config();
}
/**
* Static interface to GN 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 optimizeGN(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 S(ord));
NonlinearOptimizer optimizer(graph, config, solver);
// Gauss-Newton
NonlinearOptimizer result = optimizer.gaussNewton(relativeThreshold,
absoluteThreshold, verbosity);
return result.config();
}
};
/**

22
tests/testNonlinearOptimizer.cpp
View File

@ -175,7 +175,7 @@ TEST( NonlinearOptimizer, optimize )
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleOptimizer )
TEST( NonlinearOptimizer, SimpleLMOptimizer )
{
shared_ptr<example::Graph> fg(new example::Graph(
example::createReallyNonlinearFactorGraph()));
@ -194,6 +194,26 @@ TEST( NonlinearOptimizer, SimpleOptimizer )
DOUBLES_EQUAL(0,fg->error(*actual),tol);
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleGNOptimizer )
{
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::optimizeGN(fg, c0);
DOUBLES_EQUAL(0,fg->error(*actual),tol);
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, Factorization )
{