From 1ec93f05cd4d1a00733661417f1a046581555c26 Mon Sep 17 00:00:00 2001 From: Manohar Paluri Date: Wed, 9 Dec 2009 21:22:16 +0000 Subject: [PATCH] added a test case which uses getOrdering instead of defining the order manually. --- cpp/testVSLAMGraph.cpp | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/cpp/testVSLAMGraph.cpp b/cpp/testVSLAMGraph.cpp index 8b1696f8f..3337898cd 100644 --- a/cpp/testVSLAMGraph.cpp +++ b/cpp/testVSLAMGraph.cpp @@ -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 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;} /* ************************************************************************* */