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
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
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
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
EXPECT(optimizer->error() < 0.5 * 1e-5 * nMeasurements);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * 1e-5 * nMeasurements);
}
/* ************************************************************************* */
@ -217,8 +217,8 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -257,8 +257,8 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -313,8 +313,8 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -356,8 +356,8 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -380,7 +380,7 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
LevenbergMarquardtParams params;
params.absoluteErrorTol = 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));
}
@ -404,7 +404,7 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
LevenbergMarquardtParams params;
params.absoluteErrorTol = 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));
}

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

@ -176,8 +176,8 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// Create an ordering of the variables
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
EXPECT(optimizer->error() < 0.5 * 1e-5 * nMeasurements);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * 1e-5 * nMeasurements);
}
/* ************************************************************************* */
@ -218,8 +218,8 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
const double reproj_error = 1e-5;
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -258,8 +258,8 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -310,8 +310,8 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
EXPECT(final->error < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -353,8 +353,8 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
const double reproj_error = 1e-5 ;
Ordering ordering = *getOrdering(X,L);
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(graph, values, ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
NonlinearOptimizer::SharedState final = LevenbergMarquardtOptimizer(graph, ordering).optimize(values);
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) {
// create a Pose graph with one equality constraint and one measurement
shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph);
fg->addPoseConstraint(0, Pose2(0,0,0));
fg->addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance);
pose2SLAM::Graph fg;
fg.addPoseConstraint(0, Pose2(0,0,0));
fg.addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance);
// Create initial config
boost::shared_ptr<Values> initial(new Values());
initial->insert(pose2SLAM::PoseKey(0), Pose2(0,0,0));
initial->insert(pose2SLAM::PoseKey(1), Pose2(0,0,0));
Values initial;
initial.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0));
initial.insert(pose2SLAM::PoseKey(1), Pose2(0,0,0));
// Choose an ordering and optimize
shared_ptr<Ordering> ordering(new Ordering);
*ordering += kx0, kx1;
Ordering ordering;
ordering += kx0, kx1;
LevenbergMarquardtParams params;
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
Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0));
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);
// create a Pose graph with one equality constraint and one measurement
shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph);
fg->addPoseConstraint(0, p0);
pose2SLAM::Graph fg;
fg.addPoseConstraint(0, p0);
Pose2 delta = p0.between(p1);
fg->addOdometry(0, 1, delta, covariance);
fg->addOdometry(1, 2, delta, covariance);
fg->addOdometry(2, 0, delta, covariance);
fg.addOdometry(0, 1, delta, covariance);
fg.addOdometry(1, 2, delta, covariance);
fg.addOdometry(2, 0, delta, covariance);
// Create initial config
boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
initial->insertPose(0, p0);
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)));
pose2SLAM::Values initial;
initial.insertPose(0, p0);
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)));
// Choose an ordering
shared_ptr<Ordering> ordering(new Ordering);
*ordering += kx0,kx1,kx2;
Ordering ordering;
ordering += kx0,kx1,kx2;
// optimize
LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15;
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(fg, initial, params, ordering).optimize();
Values actual = *optimizer->values();
params.ordering = ordering;
Values actual = LevenbergMarquardtOptimizer(fg, params).optimized(initial);
// Check with ground truth
CHECK(assert_equal((const Values&)hexagon, actual));
@ -218,35 +218,34 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1);
// create a Pose graph with one equality constraint and one measurement
shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph);
fg->addPoseConstraint(0, p0);
pose2SLAM::Graph fg;
fg.addPoseConstraint(0, p0);
Pose2 delta = p0.between(p1);
fg->addOdometry(0, 1, delta, covariance);
fg->addOdometry(1,2, delta, covariance);
fg->addOdometry(2,3, delta, covariance);
fg->addOdometry(3,4, delta, covariance);
fg->addOdometry(4,5, delta, covariance);
fg->addOdometry(5, 0, delta, covariance);
fg.addOdometry(0, 1, delta, covariance);
fg.addOdometry(1,2, delta, covariance);
fg.addOdometry(2,3, delta, covariance);
fg.addOdometry(3,4, delta, covariance);
fg.addOdometry(4,5, delta, covariance);
fg.addOdometry(5, 0, delta, covariance);
// Create initial config
boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
initial->insertPose(0, p0);
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(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(5, hexagon.pose(5).retract(Vector_(3,-0.1, 0.1,-0.1)));
pose2SLAM::Values initial;
initial.insertPose(0, p0);
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(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(5, hexagon.pose(5).retract(Vector_(3,-0.1, 0.1,-0.1)));
// Choose an ordering
shared_ptr<Ordering> ordering(new Ordering);
*ordering += kx0,kx1,kx2,kx3,kx4,kx5;
Ordering ordering;
ordering += kx0,kx1,kx2,kx3,kx4,kx5;
// optimize
LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15;
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(fg, initial, params, ordering).optimize();
Values actual = *optimizer->values();
params.ordering = ordering;
Values actual = LevenbergMarquardtOptimizer(fg, params).optimized(initial);
// 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, initial, ordering).optimized();
Values actual = LevenbergMarquardtOptimizer(fg, ordering).optimized(initial);
// 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, values).optimized();
Values actual = LevenbergMarquardtOptimizer(graph).optimized(values);
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, values).optimized();
Values actual = LevenbergMarquardtOptimizer(graph).optimized(values);
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,19 +65,20 @@ TEST( StereoFactor, singlePoint)
Point3 l1(0, 0, 0);
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
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
NonlinearOptimizer::auto_ptr afterOneIteration = optimizer->iterate();
DOUBLES_EQUAL(0.0, afterOneIteration->error(), 1e-9);
NonlinearOptimizer::SharedState afterOneIteration = optimizer.iterate(initial);
DOUBLES_EQUAL(0.0, afterOneIteration->error, 1e-9);
// 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
// We expect the initial to be zero because config is the ground truth
NonlinearOptimizer::auto_ptr optimizer(new LevenbergMarquardtOptimizer(graph, initialEstimate, ordering));
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
const NonlinearOptimizer& optimizer = LevenbergMarquardtOptimizer(graph, ordering);
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
// with the ground truth
NonlinearOptimizer::auto_ptr afterOneIteration = optimizer->iterate();
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
NonlinearOptimizer::SharedState afterOneIteration = optimizer.iterate(initial);
DOUBLES_EQUAL(0.0, afterOneIteration->error, 1e-9);
// 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
// We expect the initial to be zero because config is the ground truth
NonlinearOptimizer::auto_ptr optimizer(new LevenbergMarquardtOptimizer(graph, initialEstimate, ordering));
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
LevenbergMarquardtOptimizer optimizer(graph, ordering);
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
// with the ground truth
NonlinearOptimizer::auto_ptr afterOneIteration = optimizer->iterate();
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
NonlinearOptimizer::SharedState afterOneIteration = optimizer.iterate(initial);
DOUBLES_EQUAL(0.0, afterOneIteration->error, 1e-9);
// 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)
{
// build a graph
shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph(testGraph()));
visualSLAM::Graph graph = testGraph();
// add 2 camera constraints
graph->addPoseConstraint(1, camera1);
graph->addPoseConstraint(2, camera2);
graph.addPoseConstraint(1, camera1);
graph.addPoseConstraint(2, camera2);
// Create an initial values structure corresponding to the ground truth
boost::shared_ptr<Values> initialEstimate(new Values);
initialEstimate->insert(PoseKey(1), camera1);
initialEstimate->insert(PoseKey(2), camera2);
initialEstimate->insert(PointKey(1), landmark1);
initialEstimate->insert(PointKey(2), landmark2);
initialEstimate->insert(PointKey(3), landmark3);
initialEstimate->insert(PointKey(4), landmark4);
Values initialEstimate;
initialEstimate.insert(PoseKey(1), camera1);
initialEstimate.insert(PoseKey(2), camera2);
initialEstimate.insert(PointKey(1), landmark1);
initialEstimate.insert(PointKey(2), landmark2);
initialEstimate.insert(PointKey(3), landmark3);
initialEstimate.insert(PointKey(4), landmark4);
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
NonlinearOptimizer::auto_ptr optimizer(new LevenbergMarquardtOptimizer(graph, initialEstimate));
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
LevenbergMarquardtOptimizer optimizer(graph);
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
// with the ground truth
NonlinearOptimizer::auto_ptr afterOneIteration = optimizer->iterate();
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
NonlinearOptimizer::SharedState afterOneIteration = optimizer.iterate(initial);
DOUBLES_EQUAL(0.0, afterOneIteration->error, 1e-9);
// 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;
initValues.insert(x1, start_pt);
Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimized();
Values actual = LevenbergMarquardtOptimizer(graph).optimized(initValues);
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, initValues).optimized();
Values actual = LevenbergMarquardtOptimizer(graph).optimized(initValues);
Values expected;
expected.insert(x1, goal_pt);
CHECK(assert_equal(expected, actual, tol));