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

added a test case which uses getOrdering instead of defining the order manually.

release/4.3a0
Manohar Paluri 2009-12-09 21:22:16 +00:00
parent a9dc57db2c
commit 1ec93f05cd
1 changed files with 35 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
cpp/testVSLAMGraph.cpp
View File

@ -149,6 +149,41 @@ TEST( VSLAMGraph, optimizeLM2)
CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config())));
}
/* ************************************************************************* */
TEST( VSLAMGraph, CHECK_ORDERING)
{
// build a graph
VSLAMGraph graph = testGraph();
// add 2 camera constraints
graph.addCameraConstraint(1, camera1);
graph.addCameraConstraint(2, camera2);
// Create an initial configuration corresponding to the ground truth
boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
initialEstimate->addCameraPose(1, camera1);
initialEstimate->addCameraPose(2, camera2);
initialEstimate->addLandmarkPoint(1, landmark1);
initialEstimate->addLandmarkPoint(2, landmark2);
initialEstimate->addLandmarkPoint(3, landmark3);
initialEstimate->addLandmarkPoint(4, landmark4);
Ordering ordering = graph.getOrdering();
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
Optimizer optimizer(graph, ordering, initialEstimate, 1e-5);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started
// with the ground truth
Optimizer afterOneIteration = optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct
CHECK(assert_equal(*initialEstimate,*(afterOneIteration.config())));
}
/* ************************************************************************* */
int main() { TestResult tr; TestRegistry::runAllTests(tr); return 0;}
/* ************************************************************************* */