The recursive TupleConfig should be ready for use now.

cpp/TupleConfig.h

@@ -149,8 +149,6 @@ namespace gtsam {
    *  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 Config1, class Config2>
   class TupleConfig : public Testable<TupleConfig<Config1, Config2> > {
@@ -166,7 +164,18 @@ namespace gtsam {
 	  typedef class Config1::Value Value1;
 
   public:
+
+	  /** default constructor */
 	  TupleConfig() {}
+
+	  /** Copy constructor */
+	  TupleConfig(const TupleConfig<Config1, Config2>& config) :
+		  first_(config.first_), second_(config.second_) {}
+
+	  /** Construct from configs */
+	  TupleConfig(const Config1& cfg1, const Config2& cfg2) :
+		  first_(cfg1), second_(cfg2) {}
+
 	  virtual ~TupleConfig() {}
 
 	  /** Print */
@@ -174,7 +183,7 @@ namespace gtsam {
 
 	  /** Test for equality in keys and values */
 	  bool equals(const TupleConfig<Config1, Config2>& c, double tol=1e-9) const {
-		  return false;
+		  return first_.equals(c.first_) && second_.equals(c.second_);
 	  }
 
 	  // insert function that uses the second (recursive) config
@@ -208,8 +217,25 @@ namespace gtsam {
 	  // dim function
 	  size_t dim() const { return first_.dim() + second_.dim(); }
 
+	  // Expmap
+	  TupleConfig<Config1, Config2> expmap(const VectorConfig& delta) const {
+	        return TupleConfig(gtsam::expmap(first_, delta), second_.expmap(delta));
+	  }
+
+	  /** logmap each element */
+	  VectorConfig logmap(const TupleConfig<Config1, Config2>& cp) const {
+		  VectorConfig ret(gtsam::logmap(first_, cp.first_));
+		  ret.insert(second_.logmap(cp.second_));
+		  return ret;
+	  }
+
   };
 
+  /**
+   * End of a recursive TupleConfig - contains only one config
+   *
+   * This should not be used directly
+   */
   template<class Config>
   class TupleConfigEnd : public Testable<TupleConfigEnd<Config> > {
   protected:
@@ -223,6 +249,13 @@ namespace gtsam {
 
   public:
 	  TupleConfigEnd() {}
+
+	  TupleConfigEnd(const TupleConfigEnd<Config>& config) :
+		  first_(config.first_) {}
+
+	  TupleConfigEnd(const Config& cfg) :
+		  first_(cfg) {}
+
 	  virtual ~TupleConfigEnd() {}
 
 	  /** Print */
@@ -230,7 +263,7 @@ namespace gtsam {
 
 	  /** Test for equality in keys and values */
 	  bool equals(const TupleConfigEnd<Config>& c, double tol=1e-9) const {
-		  return false;
+		  return first_.equals(c.first_);
 	  }
 
 	  void insert(const Key1& key, const Value1& value) {first_.insert(key, value); }
@@ -247,5 +280,35 @@ namespace gtsam {
 
 	  size_t dim() const { return first_.dim(); }
 
+	  TupleConfigEnd<Config> expmap(const VectorConfig& delta) const {
+	        return TupleConfigEnd(gtsam::expmap(first_, delta));
+	  }
+
+	  VectorConfig logmap(const TupleConfigEnd<Config>& cp) const {
+		  VectorConfig ret(gtsam::logmap(first_, cp.first_));
+		  return ret;
+	  }
   };
+
+  /** Exmap static functions */
+  template<class Config1, class Config2>
+  inline TupleConfig<Config1, Config2> expmap(const TupleConfig<Config1, Config2> c, const VectorConfig& delta) {
+	  return c.expmap(delta);
+  }
+
+  template<class Config>
+  inline TupleConfigEnd<Config> expmap(const TupleConfigEnd<Config> c, const VectorConfig& delta) {
+	  return c.expmap(delta);
+  }
+
+  /** logmap static functions */
+  template<class Config1, class Config2>
+    inline VectorConfig logmap(const TupleConfig<Config1, Config2> c0, const TupleConfig<Config1, Config2>& cp) {
+	  return c0.logmap(cp);
+  }
+
+  template<class Config>
+    inline VectorConfig logmap(const TupleConfigEnd<Config> c0, const TupleConfigEnd<Config>& cp) {
+	  return c0.logmap(cp);
+  }
 }

cpp/testTupleConfig.cpp

@@ -231,6 +231,85 @@ TEST(TupleConfig, basic_functions) {
 	// dim
 	CHECK(configA.dim() == 5);
 	CHECK(configB.dim() == 6);
+
+	// erase
+	configA.erase(x1);
+	CHECK(!configA.exists(x1));
+	CHECK(configA.size() == 1);
+	configA.erase(l1);
+	CHECK(!configA.exists(l1));
+	CHECK(configA.size() == 0);
+	configB.erase(L1);
+	CHECK(!configB.exists(L1));
+	CHECK(configB.size() == 2);
+}
+
+/* ************************************************************************* */
+TEST( TupleConfig, equals )
+{
+	Pose2 x1(1,2,3), x2(6,7,8), x2_alt(5,6,7);
+	PoseKey x1k(1), x2k(2);
+	Point2 l1(4,5), l2(9,10);
+	PointKey l1k(1), l2k(2);
+
+	ConfigA cfg1, cfg2, cfg3, cfg4, cfg5;
+
+	cfg1.insert(x1k, x1);
+	cfg1.insert(x2k, x2);
+	cfg1.insert(l1k, l1);
+	cfg1.insert(l2k, l2);
+
+	cfg2.insert(x1k, x1);
+	cfg2.insert(x2k, x2);
+	cfg2.insert(l1k, l1);
+	cfg2.insert(l2k, l2);
+
+	cfg3.insert(x2k, x2);
+	cfg3.insert(l1k, l1);
+
+	cfg4.insert(x1k, x1);
+	cfg4.insert(x2k, x2_alt);
+	cfg4.insert(l1k, l1);
+	cfg4.insert(l2k, l2);
+
+	ConfigA cfg6(cfg1);
+
+	CHECK(assert_equal(cfg1,cfg2));
+	CHECK(assert_equal(cfg1,cfg1));
+	CHECK(!cfg1.equals(cfg3));
+	CHECK(!cfg1.equals(cfg4));
+	CHECK(!cfg1.equals(cfg5));
+	CHECK(assert_equal(cfg1, cfg6));
+}
+
+/* ************************************************************************* */
+TEST(TupleConfig, expmap)
+{
+	Pose2 x1(1,2,3), x2(6,7,8);
+	PoseKey x1k(1), x2k(2);
+	Point2 l1(4,5), l2(9,10);
+	PointKey l1k(1), l2k(2);
+
+	ConfigA cfg1;
+	cfg1.insert(x1k, x1);
+	cfg1.insert(x2k, x2);
+	cfg1.insert(l1k, l1);
+	cfg1.insert(l2k, l2);
+
+	VectorConfig increment;
+	increment.insert("x1", Vector_(3, 1.0, 1.1, 1.2));
+	increment.insert("x2", Vector_(3, 1.3, 1.4, 1.5));
+	increment.insert("l1", Vector_(2, 1.0, 1.1));
+	increment.insert("l2", Vector_(2, 1.3, 1.4));
+
+	ConfigA expected;
+	expected.insert(x1k, expmap(x1, Vector_(3, 1.0, 1.1, 1.2)));
+	expected.insert(x2k, expmap(x2, Vector_(3, 1.3, 1.4, 1.5)));
+	expected.insert(l1k, Point2(5.0, 6.1));
+	expected.insert(l2k, Point2(10.3, 11.4));
+
+	CHECK(assert_equal(expected, expmap(cfg1, increment)));
+	CHECK(assert_equal(increment, logmap(cfg1, expected)));
 }