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

Updating unit tests and SLAM namespaces

release/4.3a0
Richard Roberts 2012-04-05 02:45:50 +00:00
parent c83a399bba
commit 89e05a6875
9 changed files with 107 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/slam/planarSLAM.h
View File

@ -110,7 +110,7 @@ namespace planarSLAM {
/// Optimize /// Optimize
Values optimize(const Values& initialEstimate) { Values optimize(const Values& initialEstimate) {
return *LevenbergMarquardtOptimizer(*this, initialEstimate).optimized(); return LevenbergMarquardtOptimizer(*this).optimized(initialEstimate);
} }
}; };

2
gtsam/slam/pose2SLAM.h
View File

@ -97,7 +97,7 @@ namespace pose2SLAM {
/// Optimize /// Optimize
Values optimize(const Values& initialEstimate) { Values optimize(const Values& initialEstimate) {
return *LevenbergMarquardtOptimizer(*this).optimized(initialEstimate); return LevenbergMarquardtOptimizer(*this).optimized(initialEstimate);
} }
}; };

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

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

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

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

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

@ -148,28 +148,29 @@ TEST( Pose2SLAM, linearization )
TEST(Pose2Graph, optimize) { TEST(Pose2Graph, optimize) {
// create a Pose graph with one equality constraint and one measurement // create a Pose graph with one equality constraint and one measurement
shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph); pose2SLAM::Graph fg;
fg->addPoseConstraint(0, Pose2(0,0,0)); fg.addPoseConstraint(0, Pose2(0,0,0));
fg->addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance); fg.addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance);
// Create initial config // Create initial config
boost::shared_ptr<Values> initial(new Values()); Values initial;
initial->insert(pose2SLAM::PoseKey(0), Pose2(0,0,0)); initial.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0));
initial->insert(pose2SLAM::PoseKey(1), Pose2(0,0,0)); initial.insert(pose2SLAM::PoseKey(1), Pose2(0,0,0));
// Choose an ordering and optimize // Choose an ordering and optimize
shared_ptr<Ordering> ordering(new Ordering); Ordering ordering;
*ordering += kx0, kx1; ordering += kx0, kx1;
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15; params.relativeErrorTol = 1e-15;
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(fg, initial, params, ordering).optimize(); params.ordering = ordering;
Values actual = LevenbergMarquardtOptimizer(fg, params).optimized(initial);
// Check with expected config // Check with expected config
Values expected; Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0)); expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0));
expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI_2)); expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI_2));
CHECK(assert_equal(expected, *optimizer->values())); CHECK(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -181,29 +182,28 @@ TEST(Pose2Graph, optimizeThreePoses) {
Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1); Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1);
// create a Pose graph with one equality constraint and one measurement // create a Pose graph with one equality constraint and one measurement
shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph); pose2SLAM::Graph fg;
fg->addPoseConstraint(0, p0); fg.addPoseConstraint(0, p0);
Pose2 delta = p0.between(p1); Pose2 delta = p0.between(p1);
fg->addOdometry(0, 1, delta, covariance); fg.addOdometry(0, 1, delta, covariance);
fg->addOdometry(1, 2, delta, covariance); fg.addOdometry(1, 2, delta, covariance);
fg->addOdometry(2, 0, delta, covariance); fg.addOdometry(2, 0, delta, covariance);
// Create initial config // Create initial config
boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values()); pose2SLAM::Values initial;
initial->insertPose(0, p0); initial.insertPose(0, p0);
initial->insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1))); initial.insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1))); initial.insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1)));
// Choose an ordering // Choose an ordering
shared_ptr<Ordering> ordering(new Ordering); Ordering ordering;
*ordering += kx0,kx1,kx2; ordering += kx0,kx1,kx2;
// optimize // optimize
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15; params.relativeErrorTol = 1e-15;
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(fg, initial, params, ordering).optimize(); params.ordering = ordering;
Values actual = LevenbergMarquardtOptimizer(fg, params).optimized(initial);
Values actual = *optimizer->values();
// Check with ground truth // Check with ground truth
CHECK(assert_equal((const Values&)hexagon, actual)); CHECK(assert_equal((const Values&)hexagon, actual));
@ -218,35 +218,34 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1); Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1);
// create a Pose graph with one equality constraint and one measurement // create a Pose graph with one equality constraint and one measurement
shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph); pose2SLAM::Graph fg;
fg->addPoseConstraint(0, p0); fg.addPoseConstraint(0, p0);
Pose2 delta = p0.between(p1); Pose2 delta = p0.between(p1);
fg->addOdometry(0, 1, delta, covariance); fg.addOdometry(0, 1, delta, covariance);
fg->addOdometry(1,2, delta, covariance); fg.addOdometry(1,2, delta, covariance);
fg->addOdometry(2,3, delta, covariance); fg.addOdometry(2,3, delta, covariance);
fg->addOdometry(3,4, delta, covariance); fg.addOdometry(3,4, delta, covariance);
fg->addOdometry(4,5, delta, covariance); fg.addOdometry(4,5, delta, covariance);
fg->addOdometry(5, 0, delta, covariance); fg.addOdometry(5, 0, delta, covariance);
// Create initial config // Create initial config
boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values()); pose2SLAM::Values initial;
initial->insertPose(0, p0); initial.insertPose(0, p0);
initial->insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1))); initial.insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1))); initial.insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1)));
initial->insertPose(3, hexagon.pose(3).retract(Vector_(3,-0.1, 0.1,-0.1))); initial.insertPose(3, hexagon.pose(3).retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insertPose(4, hexagon.pose(4).retract(Vector_(3, 0.1,-0.1, 0.1))); initial.insertPose(4, hexagon.pose(4).retract(Vector_(3, 0.1,-0.1, 0.1)));
initial->insertPose(5, hexagon.pose(5).retract(Vector_(3,-0.1, 0.1,-0.1))); initial.insertPose(5, hexagon.pose(5).retract(Vector_(3,-0.1, 0.1,-0.1)));
// Choose an ordering // Choose an ordering
shared_ptr<Ordering> ordering(new Ordering); Ordering ordering;
*ordering += kx0,kx1,kx2,kx3,kx4,kx5; ordering += kx0,kx1,kx2,kx3,kx4,kx5;
// optimize // optimize
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15; params.relativeErrorTol = 1e-15;
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(fg, initial, params, ordering).optimize(); params.ordering = ordering;
Values actual = LevenbergMarquardtOptimizer(fg, params).optimized(initial);
Values actual = *optimizer->values();
// Check with ground truth // Check with ground truth
CHECK(assert_equal((const Values&)hexagon, actual)); 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 ordering;
ordering += kx0,kx1,kx2,kx3,kx4,kx5; ordering += kx0,kx1,kx2,kx3,kx4,kx5;
Values actual = *LevenbergMarquardtOptimizer(fg, initial, ordering).optimized(); Values actual = LevenbergMarquardtOptimizer(fg, ordering).optimized(initial);
// Check with ground truth // Check with ground truth
CHECK(assert_equal(hexagon, actual,1e-4)); CHECK(assert_equal(hexagon, actual,1e-4));
@ -113,7 +113,7 @@ TEST(Pose3Graph, partial_prior_height) {
// linearization // linearization
EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol); EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol);
Values actual = *LevenbergMarquardtOptimizer(graph, values).optimized(); Values actual = LevenbergMarquardtOptimizer(graph).optimized(values);
EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol)); EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
} }
@ -165,7 +165,7 @@ TEST(Pose3Graph, partial_prior_xy) {
Values values; Values values;
values.insert(key, init); values.insert(key, init);
Values actual = *LevenbergMarquardtOptimizer(graph, values).optimized(); Values actual = LevenbergMarquardtOptimizer(graph).optimized(values);
EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol)); EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
} }

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

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

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

@ -102,16 +102,17 @@ TEST( Graph, optimizeLM)
// Create an optimizer and check its error // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
NonlinearOptimizer::auto_ptr optimizer(new LevenbergMarquardtOptimizer(graph, initialEstimate, ordering)); const NonlinearOptimizer& optimizer = LevenbergMarquardtOptimizer(graph, ordering);
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); NonlinearOptimizer::SharedState initial = optimizer.initialState(initialEstimate);
DOUBLES_EQUAL(0.0, initial->error, 1e-9);
// Iterate once, and the config should not have changed because we started // Iterate once, and the config should not have changed because we started
// with the ground truth // with the ground truth
NonlinearOptimizer::auto_ptr afterOneIteration = optimizer->iterate(); NonlinearOptimizer::SharedState afterOneIteration = optimizer.iterate(initial);
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); DOUBLES_EQUAL(0.0, afterOneIteration->error, 1e-9);
// check if correct // check if correct
CHECK(assert_equal(initialEstimate,*(afterOneIteration->values()))); CHECK(assert_equal(initialEstimate, afterOneIteration->values));
} }
@ -139,16 +140,17 @@ TEST( Graph, optimizeLM2)
// Create an optimizer and check its error // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
NonlinearOptimizer::auto_ptr optimizer(new LevenbergMarquardtOptimizer(graph, initialEstimate, ordering)); LevenbergMarquardtOptimizer optimizer(graph, ordering);
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); NonlinearOptimizer::SharedState initial = optimizer.initialState(initialEstimate);
DOUBLES_EQUAL(0.0, initial->error, 1e-9);
// Iterate once, and the config should not have changed because we started // Iterate once, and the config should not have changed because we started
// with the ground truth // with the ground truth
NonlinearOptimizer::auto_ptr afterOneIteration = optimizer->iterate(); NonlinearOptimizer::SharedState afterOneIteration = optimizer.iterate(initial);
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); DOUBLES_EQUAL(0.0, afterOneIteration->error, 1e-9);
// check if correct // check if correct
CHECK(assert_equal(initialEstimate,*(afterOneIteration->values()))); CHECK(assert_equal(initialEstimate, afterOneIteration->values));
} }
@ -156,32 +158,33 @@ TEST( Graph, optimizeLM2)
TEST( Graph, CHECK_ORDERING) TEST( Graph, CHECK_ORDERING)
{ {
// build a graph // build a graph
shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph(testGraph())); visualSLAM::Graph graph = testGraph();
// add 2 camera constraints // add 2 camera constraints
graph->addPoseConstraint(1, camera1); graph.addPoseConstraint(1, camera1);
graph->addPoseConstraint(2, camera2); graph.addPoseConstraint(2, camera2);
// Create an initial values structure corresponding to the ground truth // Create an initial values structure corresponding to the ground truth
boost::shared_ptr<Values> initialEstimate(new Values); Values initialEstimate;
initialEstimate->insert(PoseKey(1), camera1); initialEstimate.insert(PoseKey(1), camera1);
initialEstimate->insert(PoseKey(2), camera2); initialEstimate.insert(PoseKey(2), camera2);
initialEstimate->insert(PointKey(1), landmark1); initialEstimate.insert(PointKey(1), landmark1);
initialEstimate->insert(PointKey(2), landmark2); initialEstimate.insert(PointKey(2), landmark2);
initialEstimate->insert(PointKey(3), landmark3); initialEstimate.insert(PointKey(3), landmark3);
initialEstimate->insert(PointKey(4), landmark4); initialEstimate.insert(PointKey(4), landmark4);
// Create an optimizer and check its error // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
NonlinearOptimizer::auto_ptr optimizer(new LevenbergMarquardtOptimizer(graph, initialEstimate)); LevenbergMarquardtOptimizer optimizer(graph);
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); NonlinearOptimizer::SharedState initial = optimizer.initialState(initialEstimate);
DOUBLES_EQUAL(0.0, initial->error, 1e-9);
// Iterate once, and the config should not have changed because we started // Iterate once, and the config should not have changed because we started
// with the ground truth // with the ground truth
NonlinearOptimizer::auto_ptr afterOneIteration = optimizer->iterate(); NonlinearOptimizer::SharedState afterOneIteration = optimizer.iterate(initial);
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); DOUBLES_EQUAL(0.0, afterOneIteration->error, 1e-9);
// check if correct // check if correct
CHECK(assert_equal(*initialEstimate,*(afterOneIteration->values()))); CHECK(assert_equal(initialEstimate, afterOneIteration->values));
} }
/* ************************************************************************* */ /* ************************************************************************* */

4
tests/testBoundingConstraint.cpp
View File

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