More functionality in TupleConfigs

cpp/TupleConfig.h

@@ -184,23 +184,30 @@ namespace gtsam {
 
 	  // erase an element by key
 	  template<class Key>
-	  void erase(const Key& j)  {  }
-	  void erase(const Key1& j)  {  }
+	  void erase(const Key& j)  { second_.erase(j); }
+	  void erase(const Key1& j)  { first_.erase(j); }
 
 	  // determine whether an element exists
 	  template<class Key>
 	  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
+	  // access operator
 	  template<class Key>
 	  const typename Key::Value_t & operator[](const Key& j) const { return second_[j]; }
 	  const Value1& operator[](const Key1& j) const { return first_[j]; }
 
+	  // at access function
 	  template<class Key>
 	  const typename Key::Value_t & at(const Key& j) const { return second_.at(j); }
 	  const Value1& at(const Key1& j) const { return first_.at(j); }
 
+	  // size function - adds recursively
+	  size_t size() const { return first_.size() + second_.size(); }
+
+	  // dim function
+	  size_t dim() const { return first_.dim() + second_.dim(); }
+
   };
 
   template<class Config>
@@ -230,11 +237,15 @@ namespace gtsam {
 
 	  const Value1& operator[](const Key1& j) const { return first_[j]; }
 
-	  void erase(const Key1& j) {  }
+	  void erase(const Key1& j) { first_.erase(j); }
 
 	  bool exists(const Key1& j) const { return first_.exists(j); }
 
 	  const Value1& at(const Key1& j) const { return first_.at(j); }
 
+	  size_t size() const { return first_.size(); }
+
+	  size_t dim() const { return first_.dim(); }
+
   };
 }

cpp/testTupleConfig.cpp

@@ -183,7 +183,7 @@ typedef TupleConfig<PoseConfig, TupleConfigEnd<PointConfig> > ConfigA;
 typedef TupleConfig<PoseConfig, TupleConfig<PointConfig, TupleConfigEnd<LamConfig> > > ConfigB;
 
 /* ************************************************************************* */
-TEST(TupleConfig, create_insert) {
+TEST(TupleConfig, basic_functions) {
 	// create some tuple configs
 	ConfigA configA;
 	ConfigB configB;
@@ -223,6 +223,14 @@ TEST(TupleConfig, create_insert) {
 	CHECK(assert_equal(configB.at(x1), pose1));
 	CHECK(assert_equal(configB.at(l1), point1));
 	CHECK(assert_equal(configB.at(L1), lam1));
+
+	// size
+	CHECK(configA.size() == 2);
+	CHECK(configB.size() == 3);
+
+	// dim
+	CHECK(configA.dim() == 5);
+	CHECK(configB.dim() == 6);
 }