From ef0fc9fbace1e887ba918ab1b8f5f6fcdd612df2 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Thu, 12 Nov 2009 18:54:46 +0000 Subject: [PATCH] added testVSLAMGraph unit test. will fail until we can add a constraint to either one of the two cameras. --- .cproject | 90 ++++++++++++++++-------------- cpp/testVSLAMGraph.cpp | 123 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 172 insertions(+), 41 deletions(-) create mode 100644 cpp/testVSLAMGraph.cpp diff --git a/.cproject b/.cproject index 2a3996bb7..a53e1057b 100644 --- a/.cproject +++ b/.cproject @@ -1,4 +1,4 @@ - + @@ -298,22 +298,6 @@ - -make - -install -true -true -true - - -make - -check -true -true -true - make -k @@ -324,7 +308,7 @@ make - + testSimpleCamera.run true true @@ -347,22 +331,22 @@ make - + testCalibratedCamera.run true true true - + make -testGaussianConditional.run +testConditionalGaussian.run true true true make - + testPose2.run true true @@ -370,7 +354,7 @@ make - + testRot3.run true true @@ -383,18 +367,18 @@ true true - + make - -testGaussianFactor.run + +testLinearFactor.run true true true - + make - -testGaussianFactorGraph.run + +testLinearFactorGraph.run true true true @@ -408,7 +392,7 @@ make - + testPose3.run true true @@ -430,31 +414,31 @@ make - + testNonlinearFactor.run true true true - + make - -timeGaussianFactor.run + +timeLinearFactor.run true true true - + make - -timeGaussianFactorGraph.run + +timeLinearFactorGraph.run true true true make - + testGaussianBayesNet.run true true @@ -469,7 +453,7 @@ make - + testSymbolicBayesNet.run true false @@ -484,7 +468,7 @@ make - + testVector.run true true @@ -492,7 +476,7 @@ make - + testMatrix.run true true @@ -505,6 +489,14 @@ true true + +make + +testVSLAMGraph.run +true +true +true + make install @@ -526,6 +518,22 @@ true true + +make + +install +true +true +true + + +make + +check +true +true +true + diff --git a/cpp/testVSLAMGraph.cpp b/cpp/testVSLAMGraph.cpp new file mode 100644 index 000000000..74adf8a6a --- /dev/null +++ b/cpp/testVSLAMGraph.cpp @@ -0,0 +1,123 @@ +/** + * @file testVSLAMGraph.cpp + * @brief Unit test for two cameras and four landmarks + * single camera + * @author Chris Beall + * @author Frank Dellaert + * @author Viorela Ila + */ + +#include + +#include "VSLAMGraph.h" +#include "NonlinearFactorGraph-inl.h" +#include "NonlinearOptimizer-inl.h" + +using namespace std; +using namespace gtsam; +typedef NonlinearOptimizer Optimizer; + +/* ************************************************************************* */ +Point3 landmark1_local(-1.0,-1.0, 0.0); +Point3 landmark2_local(-1.0, 1.0, 0.0); +Point3 landmark3_local( 1.0, 1.0, 0.0); +Point3 landmark4_local( 1.0,-1.0, 0.0); + +Pose3 camera1_local(Matrix_(3,3, + 1., 0., 0., + 0.,-1., 0., + 0., 0.,-1. + ), + Point3(0,0,6.25)); + +Pose3 camera2_local(Matrix_(3,3, + 1., 0., 0., + 0.,-1., 0., + 0., 0.,-1. + ), + Point3(0,0,5.00)); + +Point3 landmark1() { return landmark1_local;} +Point3 landmark2() { return landmark2_local;} +Point3 landmark3() { return landmark3_local;} +Point3 landmark4() { return landmark4_local;} + +Pose3 camera1() { return camera1_local;} +Pose3 camera2() { return camera2_local;} + + +/* ************************************************************************* */ +VSLAMGraph testGraph() { + Point2 z11(-100, 100); + Point2 z12(-100, -100); + Point2 z13(100, -100); + Point2 z14(100, 100); + Point2 z21(-125, 125); + Point2 z22(-125, -125); + Point2 z23(125, -125); + Point2 z24(125, 125); + + double sigma = 1; + Cal3_S2 K(625, 625, 0, 0, 0); + VSLAMGraph g; + g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z11.vector(), sigma, 1, 1, K))); + g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z12.vector(), sigma, 1, 2, K))); + g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z13.vector(), sigma, 1, 3, K))); + g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z14.vector(), sigma, 1, 4, K))); + g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z21.vector(), sigma, 2, 1, K))); + g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z22.vector(), sigma, 2, 2, K))); + g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z23.vector(), sigma, 2, 3, K))); + g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z24.vector(), sigma, 2, 4, K))); + + return g; +} + +/* ************************************************************************* */ +TEST( VSLAMGraph, optimizeLM) +{ + // build a graph + VSLAMGraph graph = testGraph(); + + // Create a configuration corresponding to the ground truth + VSLAMConfig groundTruth; + groundTruth.addCameraPose(1, camera1()); + groundTruth.addCameraPose(2, camera2()); + groundTruth.addLandmarkPoint(1, landmark1()); + groundTruth.addLandmarkPoint(2, landmark2()); + groundTruth.addLandmarkPoint(3, landmark3()); + groundTruth.addLandmarkPoint(4, landmark4()); + + // Create an ordering of the variables + list keys; + keys.push_back("l1"); + keys.push_back("l2"); + keys.push_back("l3"); + keys.push_back("l4"); + keys.push_back("x1"); + keys.push_back("x2"); + Ordering ordering(keys); + + // optimize it + double relativeErrorTreshold=1e-6, absoluteErrorTreshold=1.0; + int verbosity=0; + + Optimizer::shared_config initialConfig(new VSLAMConfig(groundTruth)); + Optimizer optimizer(graph, ordering, initialConfig, 1e-5); + + // We expect the initial to be zero because config is the ground truth + 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); + + //afterOneIteration.config()->print(); + + // check if correct + CHECK(groundTruth.equals(*(afterOneIteration.config())) ); +} + +/* ************************************************************************* */ +int main() { TestResult tr; TestRegistry::runAllTests(tr); return 0;} +/* ************************************************************************* */