From d788e9833d67904d6776fe16ad9ef8a599fc4dd3 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 24 Nov 2009 15:12:59 +0000 Subject: [PATCH] Added tests to ensure that configs will only update existing values when using exmap and not add new ones if there are more values in the delta config. VectorConfig and VSLAMConfig both do this properly now. --- .cproject | 47 ++++++++++++++++++++++++++++------------ cpp/Makefile.am | 4 +++- cpp/VSLAMConfig.cpp | 12 ++++++---- cpp/testVSLAMConfig.cpp | 36 ++++++++++++++++++++++++++++++ cpp/testVectorConfig.cpp | 19 ++++++++++++++++ 5 files changed, 99 insertions(+), 19 deletions(-) create mode 100644 cpp/testVSLAMConfig.cpp diff --git a/.cproject b/.cproject index 3811dd5c4..221a18374 100644 --- a/.cproject +++ b/.cproject @@ -300,6 +300,7 @@ make + install true true @@ -307,6 +308,7 @@ make + check true true @@ -322,6 +324,7 @@ make + testSimpleCamera.run true true @@ -337,7 +340,6 @@ make - testVSLAMFactor.run true true @@ -345,6 +347,7 @@ make + testCalibratedCamera.run true true @@ -352,7 +355,6 @@ make - testGaussianConditional.run true true @@ -360,6 +362,7 @@ make + testPose2.run true true @@ -367,6 +370,7 @@ make + testRot3.run true true @@ -374,7 +378,6 @@ make - testNonlinearOptimizer.run true true @@ -382,6 +385,7 @@ make + testGaussianFactor.run true true @@ -389,6 +393,7 @@ make + testGaussianFactorGraph.run true true @@ -396,7 +401,6 @@ make - testNonlinearFactorGraph.run true true @@ -404,6 +408,7 @@ make + testPose3.run true true @@ -411,7 +416,6 @@ make - testVectorConfig.run true true @@ -419,7 +423,6 @@ make - testPoint2.run true true @@ -427,6 +430,7 @@ make + testNonlinearFactor.run true true @@ -434,6 +438,7 @@ make + timeGaussianFactor.run true true @@ -441,6 +446,7 @@ make + timeGaussianFactorGraph.run true true @@ -448,6 +454,7 @@ make + testGaussianBayesNet.run true true @@ -455,7 +462,6 @@ make - testBayesTree.run true false @@ -463,6 +469,7 @@ make + testSymbolicBayesNet.run true false @@ -470,7 +477,6 @@ make - testSymbolicFactorGraph.run true false @@ -478,6 +484,7 @@ make + testVector.run true true @@ -485,6 +492,7 @@ make + testMatrix.run true true @@ -492,7 +500,6 @@ make - testVSLAMGraph.run true true @@ -500,7 +507,6 @@ make - testNonlinearEquality.run true true @@ -508,6 +514,7 @@ make + testSQP.run true true @@ -515,15 +522,29 @@ make - testNonlinearConstraint.run true true true + +make + +testSQPOptimizer.run +true +true +true + + +make + +testVSLAMConfig.run +true +true +true + make - install true true @@ -531,7 +552,6 @@ make - clean true true @@ -539,7 +559,6 @@ make - check true true diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 618eae62c..160888a8d 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -170,11 +170,13 @@ testSimpleCamera_LDADD = libgtsam.la # Visual SLAM sources += VSLAMConfig.cpp VSLAMGraph.cpp VSLAMFactor.cpp -check_PROGRAMS += testVSLAMFactor testVSLAMGraph +check_PROGRAMS += testVSLAMFactor testVSLAMGraph testVSLAMConfig testVSLAMFactor_SOURCES = testVSLAMFactor.cpp testVSLAMFactor_LDADD = libgtsam.la testVSLAMGraph_SOURCES = testVSLAMGraph.cpp testVSLAMGraph_LDADD = libgtsam.la +testVSLAMConfig_SOURCES = testVSLAMConfig.cpp +testVSLAMConfig_LDADD = libgtsam.la headers += Point2Prior.h Simulated2DOdometry.h Simulated2DMeasurement.h smallExample.h headers += $(sources:.cpp=.h) diff --git a/cpp/VSLAMConfig.cpp b/cpp/VSLAMConfig.cpp index ab26e43d9..c6e498abc 100644 --- a/cpp/VSLAMConfig.cpp +++ b/cpp/VSLAMConfig.cpp @@ -32,13 +32,17 @@ VSLAMConfig VSLAMConfig::exmap(const VectorConfig & delta) const { string key = it->first; if (key[0] == 'x') { int cameraNumber = atoi(key.substr(1, key.size() - 1).c_str()); - Pose3 basePose = cameraPose(cameraNumber); - newConfig.addCameraPose(cameraNumber, basePose.exmap(it->second)); + if (cameraPoseExists(cameraNumber)) { + Pose3 basePose = cameraPose(cameraNumber); + newConfig.addCameraPose(cameraNumber, basePose.exmap(it->second)); + } } if (key[0] == 'l') { int landmarkNumber = atoi(key.substr(1, key.size() - 1).c_str()); - Point3 basePoint = landmarkPoint(landmarkNumber); - newConfig.addLandmarkPoint(landmarkNumber, basePoint.exmap(it->second)); + if (landmarkPointExists(landmarkNumber)) { + Point3 basePoint = landmarkPoint(landmarkNumber); + newConfig.addLandmarkPoint(landmarkNumber, basePoint.exmap(it->second)); + } } } diff --git a/cpp/testVSLAMConfig.cpp b/cpp/testVSLAMConfig.cpp new file mode 100644 index 000000000..89abc04de --- /dev/null +++ b/cpp/testVSLAMConfig.cpp @@ -0,0 +1,36 @@ +/* + * @file testVSLAMConfig.cpp + * @brief Tests for the Visual SLAM configuration class + * @author Alex Cunningham + */ + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( VSLAMConfig, update_with_large_delta) { + // this test ensures that if the update for delta is larger than + // the size of the config, it only updates existing variables + VSLAMConfig init; + init.addCameraPose(1, Pose3()); + init.addLandmarkPoint(1, Point3(1.0, 2.0, 3.0)); + + VectorConfig delta; + delta.insert("x1", Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1)); + delta.insert("l1", Vector_(3, 0.1, 0.1, 0.1)); + delta.insert("x2", Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1)); + + VSLAMConfig actual = init.exmap(delta); + VSLAMConfig expected; + expected.addCameraPose(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); + expected.addLandmarkPoint(1, Point3(1.1, 2.1, 3.1)); + + CHECK(assert_equal(actual, expected)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/cpp/testVectorConfig.cpp b/cpp/testVectorConfig.cpp index e2e012226..444e834a9 100644 --- a/cpp/testVectorConfig.cpp +++ b/cpp/testVectorConfig.cpp @@ -103,6 +103,25 @@ TEST( VectorConfig, scale) { CHECK(assert_equal(actual, expected)); } +/* ************************************************************************* */ +TEST( VectorConfig, update_with_large_delta) { + // this test ensures that if the update for delta is larger than + // the size of the config, it only updates existing variables + VectorConfig init, delta; + init.insert("x", Vector_(2, 1.0, 2.0)); + init.insert("y", Vector_(2, 3.0, 4.0)); + delta.insert("x", Vector_(2, 0.1, 0.1)); + delta.insert("y", Vector_(2, 0.1, 0.1)); + delta.insert("penguin", Vector_(2, 0.1, 0.1)); + + VectorConfig actual = init.exmap(delta); + VectorConfig expected; + expected.insert("x", Vector_(2, 1.1, 2.1)); + expected.insert("y", Vector_(2, 3.1, 4.1)); + + CHECK(assert_equal(actual, expected)); +} + /* ************************************************************************* */ #ifdef HAVE_BOOST_SERIALIZATION TEST( VectorConfig, serialize)