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

Unit tests now compile with new NonlinearOptimizer

release/4.3a0
Stephen Williams 2012-05-14 21:07:56 +00:00
parent 7f0881f2e4
commit 4b541e1f62
10 changed files with 116 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -175,8 +175,9 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// Create an ordering of the variables
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * 1e-5 * nMeasurements);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements);
}
/* ************************************************************************* */
@ -217,8 +218,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * reproj_error * nMeasurements);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -257,8 +259,9 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * reproj_error * nMeasurements);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -313,8 +316,9 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * reproj_error * nMeasurements);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -356,8 +360,9 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * reproj_error * nMeasurements);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -380,7 +385,7 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
LevenbergMarquardtParams params;
params.absoluteErrorTol = 1e-9;
params.relativeErrorTol = 1e-9;
Values actual = LevenbergMarquardtOptimizer(graph, params).optimized(init);
Values actual = LevenbergMarquardtOptimizer(graph, init, params).optimize();
EXPECT(assert_equal(expected, actual, 1e-4));
}
@ -404,7 +409,7 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
LevenbergMarquardtParams params;
params.absoluteErrorTol = 1e-9;
params.relativeErrorTol = 1e-9;
Values actual = LevenbergMarquardtOptimizer(graph, params).optimized(init);
Values actual = LevenbergMarquardtOptimizer(graph, init, params).optimize();
EXPECT(assert_equal(expected, actual, 1e-4));
}

25
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -176,8 +176,9 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// Create an ordering of the variables
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * 1e-5 * nMeasurements);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements);
}
/* ************************************************************************* */
@ -218,8 +219,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * reproj_error * nMeasurements);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -258,8 +260,9 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * reproj_error * nMeasurements);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -310,8 +313,9 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * reproj_error * nMeasurements);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -353,8 +357,9 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * reproj_error * nMeasurements);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */

6
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -164,7 +164,7 @@ TEST(Pose2Graph, optimize) {
LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15;
params.ordering = ordering;
Values actual = LevenbergMarquardtOptimizer(fg, params).optimized(initial);
Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize();
// Check with expected config
Values expected;
@ -203,7 +203,7 @@ TEST(Pose2Graph, optimizeThreePoses) {
LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15;
params.ordering = ordering;
Values actual = LevenbergMarquardtOptimizer(fg, params).optimized(initial);
Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize();
// Check with ground truth
CHECK(assert_equal((const Values&)hexagon, actual));
@ -245,7 +245,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15;
params.ordering = ordering;
Values actual = LevenbergMarquardtOptimizer(fg, params).optimized(initial);
Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize();
// Check with ground truth
CHECK(assert_equal((const Values&)hexagon, actual));

6
gtsam/slam/tests/testPose3SLAM.cpp
View File

@ -78,7 +78,7 @@ TEST(Pose3Graph, optimizeCircle) {
Ordering ordering;
ordering += kx0,kx1,kx2,kx3,kx4,kx5;
Values actual = LevenbergMarquardtOptimizer(fg, ordering).optimized(initial);
Values actual = LevenbergMarquardtOptimizer(fg, initial, ordering).optimize();
// Check with ground truth
CHECK(assert_equal(hexagon, actual,1e-4));
@ -113,7 +113,7 @@ TEST(Pose3Graph, partial_prior_height) {
// linearization
EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol);
Values actual = LevenbergMarquardtOptimizer(graph).optimized(values);
Values actual = LevenbergMarquardtOptimizer(graph, values).optimize();
EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
}
@ -165,7 +165,7 @@ TEST(Pose3Graph, partial_prior_xy) {
Values values;
values.insert(key, init);
Values actual = LevenbergMarquardtOptimizer(graph).optimized(values);
Values actual = LevenbergMarquardtOptimizer(graph, values).optimize();
EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
}

13
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -65,20 +65,19 @@ TEST( StereoFactor, singlePoint)
Point3 l1(0, 0, 0);
values.insert(PointKey(1), l1); // add point at origin;
GaussNewtonOptimizer optimizer(graph);
NonlinearOptimizer::SharedState initial = optimizer.initialState(values);
GaussNewtonOptimizer optimizer(graph, values);
// We expect the initial to be zero because config is the ground truth
DOUBLES_EQUAL(0.0, initial->error, 1e-9);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed
NonlinearOptimizer::SharedState afterOneIteration = optimizer.iterate(initial);
DOUBLES_EQUAL(0.0, afterOneIteration->error, 1e-9);
optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Complete solution
NonlinearOptimizer::SharedState final = optimizer.optimize(initial);
optimizer.optimize();
DOUBLES_EQUAL(0.0, final->error, 1e-6);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-6);
}
/* ************************************************************************* */

33
gtsam/slam/tests/testVSLAM.cpp
View File

@ -102,17 +102,16 @@ TEST( Graph, optimizeLM)
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
const NonlinearOptimizer& optimizer = LevenbergMarquardtOptimizer(graph, ordering);
NonlinearOptimizer::SharedState initial = optimizer.initialState(initialEstimate);
DOUBLES_EQUAL(0.0, initial->error, 1e-9);
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started
// with the ground truth
NonlinearOptimizer::SharedState afterOneIteration = optimizer.iterate(initial);
DOUBLES_EQUAL(0.0, afterOneIteration->error, 1e-9);
optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct
CHECK(assert_equal(initialEstimate, afterOneIteration->values));
CHECK(assert_equal(initialEstimate, optimizer.values()));
}
@ -140,17 +139,16 @@ TEST( Graph, optimizeLM2)
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
LevenbergMarquardtOptimizer optimizer(graph, ordering);
NonlinearOptimizer::SharedState initial = optimizer.initialState(initialEstimate);
DOUBLES_EQUAL(0.0, initial->error, 1e-9);
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started
// with the ground truth
NonlinearOptimizer::SharedState afterOneIteration = optimizer.iterate(initial);
DOUBLES_EQUAL(0.0, afterOneIteration->error, 1e-9);
optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct
CHECK(assert_equal(initialEstimate, afterOneIteration->values));
CHECK(assert_equal(initialEstimate, optimizer.values()));
}
@ -174,17 +172,16 @@ TEST( Graph, CHECK_ORDERING)
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
LevenbergMarquardtOptimizer optimizer(graph);
NonlinearOptimizer::SharedState initial = optimizer.initialState(initialEstimate);
DOUBLES_EQUAL(0.0, initial->error, 1e-9);
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started
// with the ground truth
NonlinearOptimizer::SharedState afterOneIteration = optimizer.iterate(initial);
DOUBLES_EQUAL(0.0, afterOneIteration->error, 1e-9);
optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct
CHECK(assert_equal(initialEstimate, afterOneIteration->values));
CHECK(assert_equal(initialEstimate, optimizer.values()));
}
/* ************************************************************************* */

4
tests/testBoundingConstraint.cpp
View File

@ -154,7 +154,7 @@ TEST( testBoundingConstraint, unary_simple_optimization1) {
Values initValues;
initValues.insert(x1, start_pt);
Values actual = LevenbergMarquardtOptimizer(graph).optimized(initValues);
Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
Values expected;
expected.insert(x1, goal_pt);
CHECK(assert_equal(expected, actual, tol));
@ -176,7 +176,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
Values initValues;
initValues.insert(x1, start_pt);
Values actual = LevenbergMarquardtOptimizer(graph).optimized(initValues);
Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
Values expected;
expected.insert(x1, goal_pt);
CHECK(assert_equal(expected, actual, tol));

16
tests/testNonlinearEquality.cpp
View File

@ -196,12 +196,12 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
// optimize
Ordering ordering;
ordering.push_back(key1);
NonlinearOptimizer::auto_ptr result = LevenbergMarquardtOptimizer(graph, init, ordering).optimize();
Values result = LevenbergMarquardtOptimizer(graph, init, ordering).optimize();
// verify
Values expected;
expected.insert(key1, feasible1);
EXPECT(assert_equal(expected, *result->values()));
EXPECT(assert_equal(expected, result));
}
/* ************************************************************************* */
@ -230,7 +230,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
// optimize
Ordering ordering;
ordering.push_back(key1);
Values actual = *LevenbergMarquardtOptimizer(graph, init, ordering).optimized();
Values actual = LevenbergMarquardtOptimizer(graph, init, ordering).optimize();
// verify
Values expected;
@ -317,7 +317,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
EXPECT(constraint->active(expected));
EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol);
Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimized();
Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
EXPECT(assert_equal(expected, actual, tol));
}
@ -408,7 +408,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
initValues.insert(key1, Point2());
initValues.insert(key2, badPt);
Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimized();
Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
Values expected;
expected.insert(key1, truth_pt1);
expected.insert(key2, truth_pt2);
@ -447,7 +447,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
initialEstimate.insert(l1, Point2(1.0, 6.0)); // ground truth
initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
Values actual = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimized();
Values actual = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
Values expected;
expected.insert(x1, pt_x1);
@ -491,7 +491,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
initialEstimate.insert(x2, Point2( 0.0, 0.0)); // other pose starts at origin
// optimize
Values actual = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimized();
Values actual = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
Values expected;
expected.insert(x1, Point2(1.0, 1.0));
@ -557,7 +557,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
initValues.insert(l2, landmark2);
// optimize
Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimized();
Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
// create config
Values truthValues;

105
tests/testNonlinearOptimizer.cpp
View File

@ -46,105 +46,91 @@ Key kl(size_t i) { return Symbol('l',i); }
TEST( NonlinearOptimizer, iterateLM )
{
// really non-linear factor graph
shared_ptr<example::Graph> fg(new example::Graph(
example::createReallyNonlinearFactorGraph()));
example::Graph fg(example::createReallyNonlinearFactorGraph());
// config far from minimum
Point2 x0(3,0);
boost::shared_ptr<Values> config(new Values);
config->insert(simulated2D::PoseKey(1), x0);
// ordering
shared_ptr<Ordering> ord(new Ordering());
ord->push_back(kx(1));
// create initial optimization state, with lambda=0
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(fg, config, LevenbergMarquardtParams(), ord).update(0.0);
Values config;
config.insert(simulated2D::PoseKey(1), x0);
// normal iterate
NonlinearOptimizer::auto_ptr iterated1 = GaussNewtonOptimizer(fg, config, GaussNewtonParams(), ord).iterate();
GaussNewtonOptimizer gnOptimizer(fg, config);
gnOptimizer.iterate();
// LM iterate with lambda 0 should be the same
NonlinearOptimizer::auto_ptr iterated2 = LevenbergMarquardtOptimizer(fg, config, LevenbergMarquardtParams(), ord).update(0.0)->iterate();
LevenbergMarquardtOptimizer lmOptimizer(fg, config);
lmOptimizer.iterate();
CHECK(assert_equal(*iterated1->values(), *iterated2->values(), 1e-9));
CHECK(assert_equal(gnOptimizer.values(), lmOptimizer.values(), 1e-9));
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, optimize )
{
shared_ptr<example::Graph> fg(new example::Graph(
example::createReallyNonlinearFactorGraph()));
example::Graph fg(example::createReallyNonlinearFactorGraph());
// test error at minimum
Point2 xstar(0,0);
Values cstar;
cstar.insert(simulated2D::PoseKey(1), xstar);
DOUBLES_EQUAL(0.0,fg->error(cstar),0.0);
DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);
// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
Point2 x0(3,3);
boost::shared_ptr<Values> c0(new Values);
c0->insert(simulated2D::PoseKey(1), x0);
DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3);
// optimize parameters
shared_ptr<Ordering> ord(new Ordering());
ord->push_back(kx(1));
Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);
// Gauss-Newton
NonlinearOptimizer::auto_ptr actual1 = GaussNewtonOptimizer(fg, c0, GaussNewtonParams(), ord).optimize();
DOUBLES_EQUAL(0,fg->error(*(actual1->values())),tol);
Values actual1 = GaussNewtonOptimizer(fg, c0).optimize();
DOUBLES_EQUAL(0,fg.error(actual1),tol);
// Levenberg-Marquardt
NonlinearOptimizer::auto_ptr actual2 = LevenbergMarquardtOptimizer(fg, c0, LevenbergMarquardtParams(), ord).optimize();
DOUBLES_EQUAL(0,fg->error(*(actual2->values())),tol);
Values actual2 = LevenbergMarquardtOptimizer(fg, c0).optimize();
DOUBLES_EQUAL(0,fg.error(actual2),tol);
// Dogleg
NonlinearOptimizer::auto_ptr actual3 = DoglegOptimizer(fg, c0, DoglegParams(), ord).optimize();
DOUBLES_EQUAL(0,fg->error(*(actual2->values())),tol);
Values actual3 = DoglegOptimizer(fg, c0).optimize();
DOUBLES_EQUAL(0,fg.error(actual3),tol);
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleLMOptimizer )
{
shared_ptr<example::Graph> fg(new example::Graph(
example::createReallyNonlinearFactorGraph()));
example::Graph fg(example::createReallyNonlinearFactorGraph());
Point2 x0(3,3);
boost::shared_ptr<Values> c0(new Values);
c0->insert(simulated2D::PoseKey(1), x0);
Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
Values::const_shared_ptr actual = LevenbergMarquardtOptimizer(fg, c0).optimized();
DOUBLES_EQUAL(0,fg->error(*actual),tol);
Values actual = LevenbergMarquardtOptimizer(fg, c0).optimize();
DOUBLES_EQUAL(0,fg.error(actual),tol);
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleGNOptimizer )
{
shared_ptr<example::Graph> fg(new example::Graph(
example::createReallyNonlinearFactorGraph()));
example::Graph fg(example::createReallyNonlinearFactorGraph());
Point2 x0(3,3);
boost::shared_ptr<Values> c0(new Values);
c0->insert(simulated2D::PoseKey(1), x0);
Point2 x0(3,3);
Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
Values::const_shared_ptr actual = GaussNewtonOptimizer(fg, c0).optimized();
DOUBLES_EQUAL(0,fg->error(*actual),tol);
Values actual = GaussNewtonOptimizer(fg, c0).optimize();
DOUBLES_EQUAL(0,fg.error(actual),tol);
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleDLOptimizer )
{
shared_ptr<example::Graph> fg(new example::Graph(
example::createReallyNonlinearFactorGraph()));
example::Graph fg(example::createReallyNonlinearFactorGraph());
Point2 x0(3,3);
boost::shared_ptr<Values> c0(new Values);
c0->insert(simulated2D::PoseKey(1), x0);
Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
Values::const_shared_ptr actual = DoglegOptimizer(fg, c0).optimized();
DOUBLES_EQUAL(0,fg->error(*actual),tol);
Values actual = DoglegOptimizer(fg, c0).optimize();
DOUBLES_EQUAL(0,fg.error(actual),tol);
}
/* ************************************************************************* */
@ -161,10 +147,10 @@ TEST( NonlinearOptimizer, optimization_method )
Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
Values actualMFQR = *LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimized();
Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimize();
DOUBLES_EQUAL(0,fg.error(actualMFQR),tol);
Values actualMFLDL = *LevenbergMarquardtOptimizer(fg, c0, paramsLDL).optimized();
Values actualMFLDL = LevenbergMarquardtOptimizer(fg, c0, paramsLDL).optimize();
DOUBLES_EQUAL(0,fg.error(actualMFLDL),tol);
}
@ -183,12 +169,13 @@ TEST( NonlinearOptimizer, Factorization )
ordering.push_back(pose2SLAM::PoseKey(1));
ordering.push_back(pose2SLAM::PoseKey(2));
NonlinearOptimizer::auto_ptr optimized = LevenbergMarquardtOptimizer(graph, config, ordering).iterate();
LevenbergMarquardtOptimizer optimizer(graph, config, ordering);
optimizer.iterate();
Values expected;
expected.insert(pose2SLAM::PoseKey(1), Pose2(0.,0.,0.));
expected.insert(pose2SLAM::PoseKey(2), Pose2(1.,0.,0.));
CHECK(assert_equal(expected, *optimized->values(), 1e-5));
CHECK(assert_equal(expected, optimizer.values(), 1e-5));
}
/* ************************************************************************* */
@ -216,16 +203,16 @@ TEST(NonlinearOptimizer, NullFactor) {
ord.push_back(kx(1));
// Gauss-Newton
NonlinearOptimizer::auto_ptr actual1 = GaussNewtonOptimizer(fg, c0, ord).optimize();
DOUBLES_EQUAL(0,fg.error(*actual1->values()),tol);
Values actual1 = GaussNewtonOptimizer(fg, c0, ord).optimize();
DOUBLES_EQUAL(0,fg.error(actual1),tol);
// Levenberg-Marquardt
NonlinearOptimizer::auto_ptr actual2 = LevenbergMarquardtOptimizer(fg, c0, ord).optimize();
DOUBLES_EQUAL(0,fg.error(*actual2->values()),tol);
Values actual2 = LevenbergMarquardtOptimizer(fg, c0, ord).optimize();
DOUBLES_EQUAL(0,fg.error(actual2),tol);
// Dogleg
NonlinearOptimizer::auto_ptr actual3 = DoglegOptimizer(fg, c0, ord).optimize();
DOUBLES_EQUAL(0,fg.error(*actual2->values()),tol);
Values actual3 = DoglegOptimizer(fg, c0, ord).optimize();
DOUBLES_EQUAL(0,fg.error(actual3),tol);
}
/* ************************************************************************* */

2
tests/testRot3Optimization.cpp
View File

@ -47,7 +47,7 @@ TEST(Rot3, optimize) {
fg.add(Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
}
Values final = *GaussNewtonOptimizer(fg, initial).optimized();
Values final = GaussNewtonOptimizer(fg, initial).optimize();
EXPECT(assert_equal(truth, final, 1e-5));
}