From 64703d09da3340816b33b86454cf9c54cc62c88d Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 18 Jan 2012 14:37:41 +0000 Subject: [PATCH] (in branch) more implementation for DynamicValues and unit tests --- gtsam/nonlinear/DynamicValues-inl.h | 6 +- gtsam/nonlinear/DynamicValues.h | 1 + gtsam/nonlinear/Makefile.am | 2 +- gtsam/nonlinear/tests/testDynamicValues.cpp | 285 ++++++++++++++++++++ 4 files changed, 290 insertions(+), 4 deletions(-) create mode 100644 gtsam/nonlinear/tests/testDynamicValues.cpp diff --git a/gtsam/nonlinear/DynamicValues-inl.h b/gtsam/nonlinear/DynamicValues-inl.h index 9c81b0293..f11b2f9f5 100644 --- a/gtsam/nonlinear/DynamicValues-inl.h +++ b/gtsam/nonlinear/DynamicValues-inl.h @@ -65,7 +65,7 @@ namespace gtsam { /* ************************************************************************* */ template - boost::optional DynamicValues::exists(const Symbol& j) const { + boost::optional DynamicValues::exists(const Symbol& j) const { // Find the item const_iterator item = values_.find(j); @@ -83,7 +83,7 @@ namespace gtsam { /* ************************************************************************* */ template - boost::optional exists(const TypedKey& j) const { + boost::optional DynamicValues::exists(const TypedKey& j) const { // Convert to Symbol const Symbol symbol(j.symbol()); @@ -103,7 +103,7 @@ namespace gtsam { template void DynamicValues::update(const Symbol& j, const ValueType& val) { iterator item = values_.find(j); - if(item == values_end) + if(item == values_.end()) throw DynamicValuesKeyDoesNotExist("update", j); item->second = val.clone_(); } diff --git a/gtsam/nonlinear/DynamicValues.h b/gtsam/nonlinear/DynamicValues.h index 7d7dbadd7..745384cd7 100644 --- a/gtsam/nonlinear/DynamicValues.h +++ b/gtsam/nonlinear/DynamicValues.h @@ -25,6 +25,7 @@ #pragma once #include +#include #include #include diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index 1e791a622..defcd51e4 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -18,7 +18,7 @@ check_PROGRAMS = # Lie Groups headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h DynamicValues-inl.h sources += DynamicValues.cpp -check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering +check_PROGRAMS += tests/testValues tests/testDynamicValues tests/testKey tests/testOrdering # Nonlinear nonlinear headers += Key.h diff --git a/gtsam/nonlinear/tests/testDynamicValues.cpp b/gtsam/nonlinear/tests/testDynamicValues.cpp new file mode 100644 index 000000000..4cd9e8068 --- /dev/null +++ b/gtsam/nonlinear/tests/testDynamicValues.cpp @@ -0,0 +1,285 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testDynamicValues.cpp + * @author Richard Roberts + */ + +#include +#include +#include +#include // for operator += +using namespace boost::assign; + +#include +#include +#include + +using namespace gtsam; +using namespace std; +static double inf = std::numeric_limits::infinity(); + +typedef TypedSymbol VecKey; +typedef DynamicValues DynamicValues; + +VecKey key1(1), key2(2), key3(3), key4(4); + +/* ************************************************************************* */ +TEST( DynamicValues, equals1 ) +{ + DynamicValues expected; + Vector v = Vector_(3, 5.0, 6.0, 7.0); + expected.insert(key1,v); + DynamicValues actual; + actual.insert(key1,v); + CHECK(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST( DynamicValues, equals2 ) +{ + DynamicValues cfg1, cfg2; + Vector v1 = Vector_(3, 5.0, 6.0, 7.0); + Vector v2 = Vector_(3, 5.0, 6.0, 8.0); + cfg1.insert(key1, v1); + cfg2.insert(key1, v2); + CHECK(!cfg1.equals(cfg2)); + CHECK(!cfg2.equals(cfg1)); +} + +/* ************************************************************************* */ +TEST( DynamicValues, equals_nan ) +{ + DynamicValues cfg1, cfg2; + Vector v1 = Vector_(3, 5.0, 6.0, 7.0); + Vector v2 = Vector_(3, inf, inf, inf); + cfg1.insert(key1, v1); + cfg2.insert(key1, v2); + CHECK(!cfg1.equals(cfg2)); + CHECK(!cfg2.equals(cfg1)); +} + +/* ************************************************************************* */ +TEST( DynamicValues, insert_good ) +{ + DynamicValues cfg1, cfg2, expected; + Vector v1 = Vector_(3, 5.0, 6.0, 7.0); + Vector v2 = Vector_(3, 8.0, 9.0, 1.0); + Vector v3 = Vector_(3, 2.0, 4.0, 3.0); + Vector v4 = Vector_(3, 8.0, 3.0, 7.0); + cfg1.insert(key1, v1); + cfg1.insert(key2, v2); + cfg2.insert(key3, v4); + + cfg1.insert(cfg2); + + expected.insert(key1, v1); + expected.insert(key2, v2); + expected.insert(key3, v4); + + CHECK(assert_equal(cfg1, expected)); +} + +/* ************************************************************************* */ +TEST( DynamicValues, insert_bad ) +{ + DynamicValues cfg1, cfg2; + Vector v1 = Vector_(3, 5.0, 6.0, 7.0); + Vector v2 = Vector_(3, 8.0, 9.0, 1.0); + Vector v3 = Vector_(3, 2.0, 4.0, 3.0); + Vector v4 = Vector_(3, 8.0, 3.0, 7.0); + cfg1.insert(key1, v1); + cfg1.insert(key2, v2); + cfg2.insert(key2, v3); + cfg2.insert(key3, v4); + + CHECK_EXCEPTION(cfg1.insert(cfg2), const KeyAlreadyExists); +} + +/* ************************************************************************* */ +TEST( DynamicValues, update_element ) +{ + DynamicValues cfg; + Vector v1 = Vector_(3, 5.0, 6.0, 7.0); + Vector v2 = Vector_(3, 8.0, 9.0, 1.0); + + cfg.insert(key1, v1); + CHECK(cfg.size() == 1); + CHECK(assert_equal(v1, cfg.at(key1))); + + cfg.update(key1, v2); + CHECK(cfg.size() == 1); + CHECK(assert_equal(v2, cfg.at(key1))); +} + +///* ************************************************************************* */ +//TEST(DynamicValues, dim_zero) +//{ +// DynamicValues config0; +// config0.insert(key1, Vector_(2, 2.0, 3.0)); +// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); +// LONGS_EQUAL(5,config0.dim()); +// +// VectorValues expected; +// expected.insert(key1, zero(2)); +// expected.insert(key2, zero(3)); +// CHECK(assert_equal(expected, config0.zero())); +//} + +/* ************************************************************************* */ +TEST(DynamicValues, expmap_a) +{ + DynamicValues config0; + config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); + config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); + + Ordering ordering(*config0.orderingArbitrary()); + VectorValues increment(config0.dims(ordering)); + increment[ordering[key1]] = Vector_(3, 1.0, 1.1, 1.2); + increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5); + + DynamicValues expected; + expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); + expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); + + CHECK(assert_equal(expected, config0.retract(increment, ordering))); +} + +///* ************************************************************************* */ +//TEST(DynamicValues, expmap_b) +//{ +// DynamicValues config0; +// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); +// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); +// +// Ordering ordering(*config0.orderingArbitrary()); +// VectorValues increment(config0.dims(ordering)); +// increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5); +// +// DynamicValues expected; +// expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); +// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); +// +// CHECK(assert_equal(expected, config0.retract(increment, ordering))); +//} + +///* ************************************************************************* */ +//TEST(DynamicValues, expmap_c) +//{ +// DynamicValues config0; +// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); +// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); +// +// Vector increment = Vector_(6, +// 1.0, 1.1, 1.2, +// 1.3, 1.4, 1.5); +// +// DynamicValues expected; +// expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); +// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); +// +// CHECK(assert_equal(expected, config0.retract(increment))); +//} + +/* ************************************************************************* */ +/*TEST(DynamicValues, expmap_d) +{ + DynamicValues config0; + config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); + config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); + //config0.print("config0"); + CHECK(equal(config0, config0)); + CHECK(config0.equals(config0)); + + DynamicValues poseconfig; + poseconfig.insert("p1", Pose2(1,2,3)); + poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5)); + //poseconfig.print("poseconfig"); + CHECK(equal(config0, config0)); + CHECK(config0.equals(config0)); +}*/ + +/* ************************************************************************* */ +/*TEST(DynamicValues, extract_keys) +{ + typedef TypedSymbol PoseKey; + DynamicValues config; + + config.insert(PoseKey(1), Pose2()); + config.insert(PoseKey(2), Pose2()); + config.insert(PoseKey(4), Pose2()); + config.insert(PoseKey(5), Pose2()); + + list expected, actual; + expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5); + actual = config.keys(); + + CHECK(actual.size() == expected.size()); + list::const_iterator itAct = actual.begin(), itExp = expected.begin(); + for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) { + CHECK(assert_equal(*itExp, *itAct)); + } +}*/ + +/* ************************************************************************* */ +TEST(DynamicValues, exists_) +{ + DynamicValues config0; + config0.insert(key1, Vector_(1, 1.)); + config0.insert(key2, Vector_(1, 2.)); + + boost::optional v = config0.exists_(key1); + CHECK(assert_equal(Vector_(1, 1.),*v)); +} + +/* ************************************************************************* */ +TEST(DynamicValues, update) +{ + DynamicValues config0; + config0.insert(key1, Vector_(1, 1.)); + config0.insert(key2, Vector_(1, 2.)); + + DynamicValues superset; + superset.insert(key1, Vector_(1, -1.)); + superset.insert(key2, Vector_(1, -2.)); + superset.insert(key3, Vector_(1, -3.)); + config0.update(superset); + + DynamicValues expected; + expected.insert(key1, Vector_(1, -1.)); + expected.insert(key2, Vector_(1, -2.)); + CHECK(assert_equal(expected,config0)); +} + +/* ************************************************************************* */ +TEST(DynamicValues, dummy_initialization) +{ + typedef TypedSymbol Key; + + DynamicValues init1; + init1.insert(Key(1), Vector_(2, 1.0, 2.0)); + init1.insert(Key(2), Vector_(2, 4.0, 3.0)); + + DynamicValues init2; + init2.insert(key1, Vector_(2, 1.0, 2.0)); + init2.insert(key2, Vector_(2, 4.0, 3.0)); + + DynamicValues actual1(init2); + DynamicValues actual2(init1); + + EXPECT(actual1.empty()); + EXPECT(actual2.empty()); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */