diff --git a/cpp/Key.h b/cpp/Key.h index 89113a0c2..f9ae51604 100644 --- a/cpp/Key.h +++ b/cpp/Key.h @@ -35,6 +35,9 @@ namespace gtsam { public: + // typedefs + typedef T Value_t; + // Constructors: TypedSymbol():j_(0) {} diff --git a/cpp/TupleConfig.h b/cpp/TupleConfig.h index 16b11ba50..6f026f1f6 100644 --- a/cpp/TupleConfig.h +++ b/cpp/TupleConfig.h @@ -141,4 +141,100 @@ namespace gtsam { template inline VectorConfig logmap(const PairConfig c0, const PairConfig& cp) { return c0.logmap(cp); } + + /** + * Tuple configs to handle subconfigs of LieConfigs + * + * This uses a recursive structure of config pairs to form a lisp-like + * list, with a special case (TupleConfigEnd) that contains only one config + * at the end. In a final use case, this should be aliased to something clearer + * but still with the same recursive type machinery. + * + * STILL UNDER TESTING - DO NOT USE + */ + template + class TupleConfig : public Testable > { + + protected: + // Data for internal configs + Config1 first_; + Config2 second_; + + public: + // typedefs + typedef class Config1::Key Key1; + typedef class Config1::Value Value1; + + public: + TupleConfig() {} + virtual ~TupleConfig() {} + + /** Print */ + void print(const std::string& s = "") const {} + + /** Test for equality in keys and values */ + bool equals(const TupleConfig& c, double tol=1e-9) const { + return false; + } + + // insert function that uses the second (recursive) config + template + void insert(const Key& key, const Value& value) {second_.insert(key, value);} + void insert(const Key1& key, const Value1& value) {first_.insert(key, value);} + + // erase an element by key + template + void erase(const Key& j) { } + void erase(const Key1& j) { } + + // determine whether an element exists + template + bool exists(const Key& j) const { return second_.exists(j); } + bool exists(const Key1& j) const { return first_.exists(j); } + + // access operator - currently configs after the first one will not be found + template + const Value& operator[](const Key& j) const { return second_[j]; } + const Value1& operator[](const Key1& j) const { return first_[j]; } + + template + const Value& at(const Key& j) const { return second_.at(j); } + const Value1& at(const Key1& j) const { return first_.at(j); } + + }; + + template + class TupleConfigEnd : public Testable > { + protected: + // Data for internal configs + Config first_; + + public: + // typedefs + typedef class Config::Key Key1; + typedef class Config::Value Value1; + + public: + TupleConfigEnd() {} + virtual ~TupleConfigEnd() {} + + /** Print */ + void print(const std::string& s = "") const {} + + /** Test for equality in keys and values */ + bool equals(const TupleConfigEnd& c, double tol=1e-9) const { + return false; + } + + void insert(const Key1& key, const Value1& value) {first_.insert(key, value); } + + const Value1& operator[](const Key1& j) const { return first_[j]; } + + void erase(const Key1& j) { } + + bool exists(const Key1& j) const { return first_.exists(j); } + + const Value1& at(const Key1& j) const { return first_.at(j); } + + }; } diff --git a/cpp/testTupleConfig.cpp b/cpp/testTupleConfig.cpp index 8598fab4e..67d5e5a0a 100644 --- a/cpp/testTupleConfig.cpp +++ b/cpp/testTupleConfig.cpp @@ -166,6 +166,66 @@ TEST(PairConfig, expmap) CHECK(assert_equal(expected, expmap(cfg1, increment))); } +/* ************************************************************************* */ + +// some key types +typedef TypedSymbol PoseKey; +typedef TypedSymbol PointKey; +typedef TypedSymbol LamKey; + +// some config types +typedef LieConfig PoseConfig; +typedef LieConfig PointConfig; +typedef LieConfig LamConfig; + +// some TupleConfig types +typedef TupleConfig > ConfigA; +typedef TupleConfig > > ConfigB; + +/* ************************************************************************* */ +TEST(TupleConfig, create_insert) { + // create some tuple configs + ConfigA configA; + ConfigB configB; + + PoseKey x1(1); + PointKey l1(1); + LamKey L1(1); + Pose2 pose1(1.0, 2.0, 0.3); + Point2 point1(2.0, 3.0); + Vector lam1 = Vector_(1, 2.3); + + // Insert + configA.insert(x1, pose1); + configA.insert(l1, point1); + + configB.insert(x1, pose1); + configB.insert(l1, point1); + configB.insert(L1, lam1); + + // bracket operator - FAILS on config types after first one + CHECK(assert_equal(configA[x1], pose1)); +// CHECK(assert_equal(configA[l1], point1)); + CHECK(assert_equal(configB[x1], pose1)); +// CHECK(assert_equal(configB[l1], point1)); +// CHECK(assert_equal(configB[L1], lam1)); + + // exists + CHECK(configA.exists(x1)); + CHECK(configA.exists(l1)); + CHECK(configB.exists(x1)); + CHECK(configB.exists(l1)); + CHECK(configB.exists(L1)); + + // at - access function - FAILS as with bracket operator + CHECK(assert_equal(configA.at(x1), pose1)); +// CHECK(assert_equal(configA.at(l1), point1)); + CHECK(assert_equal(configB.at(x1), pose1)); +// CHECK(assert_equal(configB.at(l1), point1)); +// CHECK(assert_equal(configB.at(L1), lam1)); +} + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */