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

DoglegOptimizer unit tests

release/4.3a0
Richard Roberts 2012-03-23 03:43:28 +00:00
parent a7f9c861ad
commit cdd89a43f5
1 changed files with 17 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
tests/testNonlinearOptimizer.cpp
View File

@ -31,10 +31,9 @@ using namespace boost;
#include <gtsam/slam/pose2SLAM.h> #include <gtsam/slam/pose2SLAM.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
// template definitions
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h> #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
using namespace gtsam; using namespace gtsam;
@ -100,6 +99,10 @@ TEST( NonlinearOptimizer, optimize )
// Levenberg-Marquardt // Levenberg-Marquardt
NonlinearOptimizer::auto_ptr actual2 = LevenbergMarquardtOptimizer(fg, c0, LevenbergMarquardtParams(), ord).optimize(); NonlinearOptimizer::auto_ptr actual2 = LevenbergMarquardtOptimizer(fg, c0, LevenbergMarquardtParams(), ord).optimize();
DOUBLES_EQUAL(0,fg->error(*(actual2->values())),tol); DOUBLES_EQUAL(0,fg->error(*(actual2->values())),tol);
// Dogleg
NonlinearOptimizer::auto_ptr actual3 = DoglegOptimizer(fg, c0, DoglegParams(), ord).optimize();
DOUBLES_EQUAL(0,fg->error(*(actual2->values())),tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -116,19 +119,6 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer )
DOUBLES_EQUAL(0,fg->error(*actual),tol); DOUBLES_EQUAL(0,fg->error(*actual),tol);
} }
/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleLMOptimizer_noshared )
{
example::Graph fg = example::createReallyNonlinearFactorGraph();
Point2 x0(3,3);
Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
Values::const_shared_ptr actual = LevenbergMarquardtOptimizer(fg, c0).optimize()->values();
DOUBLES_EQUAL(0,fg.error(*actual),tol);
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleGNOptimizer ) TEST( NonlinearOptimizer, SimpleGNOptimizer )
{ {
@ -144,16 +134,17 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleGNOptimizer_noshared ) TEST( NonlinearOptimizer, SimpleDLOptimizer )
{ {
example::Graph fg = example::createReallyNonlinearFactorGraph(); shared_ptr<example::Graph> fg(new example::Graph(
example::createReallyNonlinearFactorGraph()));
Point2 x0(3,3); Point2 x0(3,3);
Values c0; boost::shared_ptr<Values> c0(new Values);
c0.insert(simulated2D::PoseKey(1), x0); c0->insert(simulated2D::PoseKey(1), x0);
Values::const_shared_ptr actual = GaussNewtonOptimizer(fg, c0).optimize()->values(); Values::const_shared_ptr actual = DoglegOptimizer(fg, c0).optimize()->values();
DOUBLES_EQUAL(0,fg.error(*actual),tol); DOUBLES_EQUAL(0,fg->error(*actual),tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -231,6 +222,10 @@ TEST(NonlinearOptimizer, NullFactor) {
// Levenberg-Marquardt // Levenberg-Marquardt
NonlinearOptimizer::auto_ptr actual2 = LevenbergMarquardtOptimizer(fg, c0, ord).optimize(); NonlinearOptimizer::auto_ptr actual2 = LevenbergMarquardtOptimizer(fg, c0, ord).optimize();
DOUBLES_EQUAL(0,fg.error(*actual2->values()),tol); DOUBLES_EQUAL(0,fg.error(*actual2->values()),tol);
// Dogleg
NonlinearOptimizer::auto_ptr actual3 = DoglegOptimizer(fg, c0, ord).optimize();
DOUBLES_EQUAL(0,fg.error(*actual2->values()),tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */