Added wrapper classes for TupleConfig for ease of use.

cpp/TupleConfig-inl.h

@@ -20,21 +20,83 @@
 
 namespace gtsam {
 
-  template<class J1, class X1, class J2, class X2>
+/** PairConfig implementations */
-  void PairConfig<J1,X1,J2,X2>::print(const std::string& s) const {
+/* ************************************************************************* */
-    std::cout << "TupleConfig " << s << ", size " << size_ << "\n";
+template<class J1, class X1, class J2, class X2>
-    first().print(s + "Config1: ");
+void PairConfig<J1,X1,J2,X2>::print(const std::string& s) const {
-    second().print(s + "Config2: ");
+	std::cout << "TupleConfig " << s << ", size " << size_ << "\n";
-  }
+	first().print(s + "Config1: ");
+	second().print(s + "Config2: ");
+}
 
-  template<class J1, class X1, class J2, class X2>
+/* ************************************************************************* */
-  void PairConfig<J1,X1,J2,X2>::insert(const PairConfig& config) {
+template<class J1, class X1, class J2, class X2>
-  	for (typename Config1::const_iterator it = config.first().begin(); it!=config.first().end(); it++) {
+void PairConfig<J1,X1,J2,X2>::insert(const PairConfig& config) {
-  		insert(it->first, it->second);
+	for (typename Config1::const_iterator it = config.first().begin(); it!=config.first().end(); it++) {
-  	}
+		insert(it->first, it->second);
-  	for (typename Config2::const_iterator it = config.second().begin(); it!=config.second().end(); it++) {
+	}
-  		insert(it->first, it->second);
+	for (typename Config2::const_iterator it = config.second().begin(); it!=config.second().end(); it++) {
-  	}
+		insert(it->first, it->second);
-  }
+	}
+}
+
+/** TupleConfigN Implementations */
+/* ************************************************************************* */
+template<class Config1, class Config2>
+TupleConfig2<Config1, Config2>::TupleConfig2(const TupleConfig2<Config1, Config2>& config) :
+		  TupleConfig<Config1, TupleConfigEnd<Config2> >(config) {}
+
+template<class Config1, class Config2>
+TupleConfig2<Config1, Config2>::TupleConfig2(const Config1& cfg1, const Config2& cfg2) :
+			  TupleConfig<Config1, TupleConfigEnd<Config2> >(
+					  cfg1, TupleConfigEnd<Config2>(cfg2)) {}
+
+template<class Config1, class Config2, class Config3>
+TupleConfig3<Config1, Config2, Config3>::TupleConfig3(const TupleConfig3<Config1, Config2, Config3>& config) :
+		  TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > >(config) {}
+
+template<class Config1, class Config2, class Config3>
+TupleConfig3<Config1, Config2, Config3>::TupleConfig3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3) :
+			  TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > >(
+					  cfg1, TupleConfig<Config2, TupleConfigEnd<Config3> >(
+							  cfg2, TupleConfigEnd<Config3>(cfg3))) {}
+
+template<class Config1, class Config2, class Config3, class Config4>
+TupleConfig4<Config1, Config2, Config3, Config4>::TupleConfig4(const TupleConfig4<Config1, Config2, Config3, Config4>& config) :
+	TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfigEnd<Config4> > > >(config) {}
+
+template<class Config1, class Config2, class Config3, class Config4>
+TupleConfig4<Config1, Config2, Config3, Config4>::TupleConfig4(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,const Config4& cfg4) :
+		  TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfigEnd<Config4> > > >(
+				  cfg1, TupleConfig<Config2, TupleConfig<Config3, TupleConfigEnd<Config4> > >(
+						  cfg2, TupleConfig<Config3, TupleConfigEnd<Config4> >(
+								  cfg3, TupleConfigEnd<Config4>(cfg4)))) {}
+
+template<class Config1, class Config2, class Config3, class Config4, class Config5>
+TupleConfig5<Config1, Config2, Config3, Config4, Config5>::TupleConfig5(const TupleConfig5<Config1, Config2, Config3, Config4, Config5>& config) :
+		  TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfigEnd<Config5> > > > >(config) {}
+
+template<class Config1, class Config2, class Config3, class Config4, class Config5>
+TupleConfig5<Config1, Config2, Config3, Config4, Config5>::TupleConfig5(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
+				   const Config4& cfg4, const Config5& cfg5) :
+						   TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfigEnd<Config5> > > > >(
+								   cfg1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfigEnd<Config5> > > >(
+										   cfg2, TupleConfig<Config3, TupleConfig<Config4, TupleConfigEnd<Config5> > >(
+												   cfg3, TupleConfig<Config4, TupleConfigEnd<Config5> >(
+														   cfg4, TupleConfigEnd<Config5>(cfg5))))) {}
+
+template<class Config1, class Config2, class Config3, class Config4, class Config5, class Config6>
+TupleConfig6<Config1, Config2, Config3, Config4, Config5, Config6>::TupleConfig6(const TupleConfig6<Config1, Config2, Config3, Config4, Config5, Config6>& config) :
+			  TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > > > >(config) {}
+
+template<class Config1, class Config2, class Config3, class Config4, class Config5, class Config6>
+TupleConfig6<Config1, Config2, Config3, Config4, Config5, Config6>::TupleConfig6(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
+				   const Config4& cfg4, const Config5& cfg5, const Config6& cfg6) :
+						   TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > > > >(
+								   cfg1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > > >(
+										   cfg2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > >(
+												   cfg3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > >(
+														   cfg4, TupleConfig<Config5, TupleConfigEnd<Config6> >(
+																   cfg5, TupleConfigEnd<Config6>(cfg6)))))) {}
 
 }

cpp/TupleConfig.h

@@ -33,8 +33,6 @@ namespace gtsam {
 	  typedef class Config1::Key Key1;
 	  typedef class Config1::Value Value1;
 
-  public:
-
 	  /** default constructor */
 	  TupleConfig() {}
 
@@ -81,6 +79,10 @@ namespace gtsam {
 	  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); }
 
+	  // direct config access
+	  const Config1& config() const { return first_; }
+	  const Config2& rest() const { return second_; }
+
 	  // size function - adds recursively
 	  size_t size() const { return first_.size() + second_.size(); }
 
@@ -108,6 +110,7 @@ namespace gtsam {
    */
   template<class Config>
   class TupleConfigEnd : public Testable<TupleConfigEnd<Config> > {
+
   protected:
 	  // Data for internal configs
 	  Config first_;
@@ -117,7 +120,6 @@ namespace gtsam {
 	  typedef class Config::Key Key1;
 	  typedef class Config::Value Value1;
 
-  public:
 	  TupleConfigEnd() {}
 
 	  TupleConfigEnd(const TupleConfigEnd<Config>& config) :
@@ -140,6 +142,8 @@ namespace gtsam {
 
 	  const Value1& operator[](const Key1& j) const { return first_[j]; }
 
+	  const Config& config() const { return first_; }
+
 	  void erase(const Key1& j) { first_.erase(j); }
 
 	  bool exists(const Key1& j) const { return first_.exists(j); }
@@ -183,45 +187,110 @@ namespace gtsam {
   }
 
   /**
-   * Typedefs for existing config types
+   * Wrapper classes to act as containers for configs.  Note that these can be cascaded
+   * recursively, as they are TupleConfigs, and are primarily a short form of the config
+   * structure to make use of the TupleConfigs easier.
+   *
+   * The interface is designed to mimic PairConfig, but for 2-6 config types.
    */
   template<class Config1, class Config2>
-  struct TupleConfig2 : TupleConfig<Config1, TupleConfigEnd<Config2> > {
+  class TupleConfig2 : public TupleConfig<Config1, TupleConfigEnd<Config2> > {
+  public:
+	  // typedefs
+	  typedef Config1 Config1_t;
+	  typedef Config2 Config2_t;
+
 	  TupleConfig2() {}
-	  TupleConfig2(const TupleConfig2<Config1, Config2>& config) :
+	  TupleConfig2(const TupleConfig2<Config1, Config2>& config);
-		  TupleConfig<Config1, TupleConfigEnd<Config2> >(config) {}
+	  TupleConfig2(const Config1& cfg1, const Config2& cfg2);
-	  TupleConfig2(const Config1& cfg1, const Config2& cfg2) :
+
-			  TupleConfig<Config1, TupleConfigEnd<Config2> >(cfg1, TupleConfigEnd<Config2>(cfg2)) {}
+	  // access functions
+	  inline const Config1_t& first() const { return this->config(); }
+	  inline const Config2_t& second() const { return this->rest().config(); }
   };
 
   template<class Config1, class Config2, class Config3>
-  struct TupleConfig3 : TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > > {
+  class TupleConfig3 : public TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > > {
+  public:
+	  // typedefs
+	  typedef Config1 Config1_t;
+	  typedef Config2 Config2_t;
+	  typedef Config3 Config3_t;
+
 	  TupleConfig3() {}
-	  TupleConfig3(const TupleConfig3<Config1, Config2, Config3>& config) :
+	  TupleConfig3(const TupleConfig3<Config1, Config2, Config3>& config);
-		  TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > >(config) {}
+	  TupleConfig3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3);
-	  TupleConfig3(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3) :
+	  // access functions
-			  TupleConfig<Config1, TupleConfig<Config2, TupleConfigEnd<Config3> > >(cfg1, TupleConfig<Config2, TupleConfigEnd<Config3> >(cfg2, TupleConfigEnd<Config3>(cfg3))) {}
+	  inline const Config1_t& first() const { return this->config(); }
+	  inline const Config2_t& second() const { return this->rest().config(); }
+	  inline const Config3_t& third() const { return this->rest().rest().config(); }
   };
 
   template<class Config1, class Config2, class Config3, class Config4>
-  struct TupleConfig4 : TupleConfig<Config1, TupleConfig<Config2,TupleConfig<Config3, TupleConfigEnd<Config4> > > > {
+  class TupleConfig4 : public TupleConfig<Config1, TupleConfig<Config2,TupleConfig<Config3, TupleConfigEnd<Config4> > > > {
+  public:
+	  // typedefs
+	  typedef Config1 Config1_t;
+	  typedef Config2 Config2_t;
+	  typedef Config3 Config3_t;
+	  typedef Config4 Config4_t;
+
 	  TupleConfig4() {}
-	  TupleConfig4(const TupleConfig4<Config1, Config2, Config3, Config4>& config) :
+	  TupleConfig4(const TupleConfig4<Config1, Config2, Config3, Config4>& config);
-		  TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfigEnd<Config4> > > >(config) {}
+	  TupleConfig4(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,const Config4& cfg4);
+
+	  // access functions
+	  inline const Config1_t& first() const { return this->config(); }
+	  inline const Config2_t& second() const { return this->rest().config(); }
+	  inline const Config3_t& third() const { return this->rest().rest().config(); }
+	  inline const Config4_t& fourth() const { return this->rest().rest().rest().config(); }
   };
 
   template<class Config1, class Config2, class Config3, class Config4, class Config5>
-  struct TupleConfig5 : TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfigEnd<Config5> > > > > {
+  class TupleConfig5 : public TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfigEnd<Config5> > > > > {
+  public:
+	  // typedefs
+	  typedef Config1 Config1_t;
+	  typedef Config2 Config2_t;
+	  typedef Config3 Config3_t;
+	  typedef Config4 Config4_t;
+	  typedef Config5 Config5_t;
+
 	  TupleConfig5() {}
-	  TupleConfig5(const TupleConfig5<Config1, Config2, Config3, Config4, Config5>& config) :
+	  TupleConfig5(const TupleConfig5<Config1, Config2, Config3, Config4, Config5>& config);
-		  TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfigEnd<Config5> > > > >(config) {}
+	  TupleConfig5(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
+				   const Config4& cfg4, const Config5& cfg5);
+
+	  // access functions
+	  inline const Config1_t& first() const { return this->config(); }
+	  inline const Config2_t& second() const { return this->rest().config(); }
+	  inline const Config3_t& third() const { return this->rest().rest().config(); }
+	  inline const Config4_t& fourth() const { return this->rest().rest().rest().config(); }
+	  inline const Config5_t& fifth() const { return this->rest().rest().rest().rest().config(); }
   };
 
   template<class Config1, class Config2, class Config3, class Config4, class Config5, class Config6>
-  struct TupleConfig6 : TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > > > > {
+  class TupleConfig6 : public TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > > > > {
+  public:
+	  // typedefs
+	  typedef Config1 Config1_t;
+	  typedef Config2 Config2_t;
+	  typedef Config3 Config3_t;
+	  typedef Config4 Config4_t;
+	  typedef Config5 Config5_t;
+	  typedef Config6 Config6_t;
+
 	  TupleConfig6() {}
-	  TupleConfig6(const TupleConfig6<Config1, Config2, Config3, Config4, Config5, Config6>& config) :
+	  TupleConfig6(const TupleConfig6<Config1, Config2, Config3, Config4, Config5, Config6>& config);
-			  TupleConfig<Config1, TupleConfig<Config2, TupleConfig<Config3, TupleConfig<Config4, TupleConfig<Config5, TupleConfigEnd<Config6> > > > > >(config) {}
+	  TupleConfig6(const Config1& cfg1, const Config2& cfg2, const Config3& cfg3,
+				   const Config4& cfg4, const Config5& cfg5, const Config6& cfg6);
+	  // access functions
+	  inline const Config1_t& first() const { return this->config(); }
+	  inline const Config2_t& second() const { return this->rest().config(); }
+	  inline const Config3_t& third() const { return this->rest().rest().config(); }
+	  inline const Config4_t& fourth() const { return this->rest().rest().rest().config(); }
+	  inline const Config5_t& fifth() const { return this->rest().rest().rest().rest().config(); }
+	  inline const Config6_t& sixth() const { return this->rest().rest().rest().rest().rest().config(); }
   };
 
   /**
@@ -229,7 +298,8 @@ namespace gtsam {
    * STILL IN TESTING - will soon replace PairConfig
    */
 //  template<class J1, class X1, class J2, class X2>
-//  struct PairConfig : TupleConfig2<LieConfig<J1, X1>, LieConfig<J2, X2> > {
+//  class PairConfig : public  TupleConfig2<LieConfig<J1, X1>, LieConfig<J2, X2> > {
+//  public:
 //	  PairConfig() {}
 //	  PairConfig(const PairConfig<J1, X1, J2, X2>& config) :
 //		  TupleConfig2<LieConfig<J1, X1>, LieConfig<J2, X2> >(config) {}
@@ -239,6 +309,7 @@ namespace gtsam {
 
   /**
    * PairConfig:  a config that holds two data types.
+   * Note: this should eventually be replaced with a wrapper on TupleConfig2
    */
   template<class J1, class X1, class J2, class X2>
   class PairConfig : public Testable<PairConfig<J1, X1, J2, X2> > {
@@ -288,7 +359,7 @@ namespace gtsam {
     bool equals(const PairConfig<J1, X1, J2, X2>& c, double tol=1e-9) const {
       return first_.equals(c.first_, tol) && second_.equals(c.second_, tol); }
 
-    /** Direct access functions */
+    /** Returns the real config */
     inline const Config1& first() const { return first_; }
     inline const Config2& second() const { return second_; }
 

cpp/testTupleConfig.cpp

@@ -34,16 +34,16 @@ TEST( PairConfig, insert_equals1 )
   Point2 l1(4,5), l2(9,10);
 
   Config expected;
-  expected.insert(1, x1);
+  expected.insert(PoseKey(1), x1);
-  expected.insert(2, x2);
+  expected.insert(PoseKey(2), x2);
-  expected.insert(1, l1);
+  expected.insert(PointKey(1), l1);
-  expected.insert(2, l2);
+  expected.insert(PointKey(2), l2);
 
   Config actual;
-  actual.insert(1, x1);
+  actual.insert(PoseKey(1), x1);
-  actual.insert(2, x2);
+  actual.insert(PoseKey(2), x2);
-  actual.insert(1, l1);
+  actual.insert(PointKey(1), l1);
-  actual.insert(2, l2);
+  actual.insert(PointKey(2), l2);
 
   CHECK(assert_equal(expected,actual));
 }
@@ -55,15 +55,15 @@ TEST( PairConfig, insert_equals2 )
   Point2 l1(4,5), l2(9,10);
 
   Config cfg1;
-  cfg1.insert(1, x1);
+  cfg1.insert(PoseKey(1), x1);
-  cfg1.insert(2, x2);
+  cfg1.insert(PoseKey(2), x2);
-  cfg1.insert(1, l1);
+  cfg1.insert(PointKey(1), l1);
-  cfg1.insert(2, l2);
+  cfg1.insert(PointKey(2), l2);
 
   Config cfg2;
-  cfg2.insert(1, x1);
+  cfg2.insert(PoseKey(1), x1);
-  cfg2.insert(2, x2);
+  cfg2.insert(PoseKey(2), x2);
-  cfg2.insert(1, l1);
+  cfg2.insert(PointKey(1), l1);
 
   CHECK(!cfg1.equals(cfg2));
 
@@ -98,10 +98,10 @@ TEST( PairConfig, size_dim )
   Point2 l1(4,5), l2(9,10);
 
   Config cfg1;
-  cfg1.insert(1, x1);
+  cfg1.insert(PoseKey(1), x1);
-  cfg1.insert(2, x2);
+  cfg1.insert(PoseKey(2), x2);
-  cfg1.insert(1, l1);
+  cfg1.insert(PointKey(1), l1);
-  cfg1.insert(2, l2);
+  cfg1.insert(PointKey(2), l2);
 
   CHECK(cfg1.size() == 4);
   CHECK(cfg1.dim() == 10);
@@ -114,10 +114,10 @@ TEST(PairConfig, at)
   Point2 l1(4,5), l2(9,10);
 
   Config cfg1;
-  cfg1.insert(1, x1);
+  cfg1.insert(PoseKey(1), x1);
-  cfg1.insert(2, x2);
+  cfg1.insert(PoseKey(2), x2);
-  cfg1.insert(1, l1);
+  cfg1.insert(PointKey(1), l1);
-  cfg1.insert(2, l2);
+  cfg1.insert(PointKey(2), l2);
 
   CHECK(assert_equal(x1, cfg1[PoseKey(1)]));
   CHECK(assert_equal(x2, cfg1[PoseKey(2)]));
@@ -148,10 +148,10 @@ TEST(PairConfig, expmap)
   Point2 l1(4,5), l2(9,10);
 
   Config cfg1;
-  cfg1.insert(1, x1);
+  cfg1.insert(PoseKey(1), x1);
-  cfg1.insert(2, x2);
+  cfg1.insert(PoseKey(2), x2);
-  cfg1.insert(1, l1);
+  cfg1.insert(PointKey(1), l1);
-  cfg1.insert(2, l2);
+  cfg1.insert(PointKey(2), l2);
 
   VectorConfig increment;
   increment.insert("x1", Vector_(3, 1.0, 1.1, 1.2));
@@ -160,12 +160,13 @@ TEST(PairConfig, expmap)
   increment.insert("l2", Vector_(2, 1.3, 1.4));
 
   Config expected;
-  expected.insert(1, expmap(x1, Vector_(3, 1.0, 1.1, 1.2)));
+  expected.insert(PoseKey(1), expmap(x1, Vector_(3, 1.0, 1.1, 1.2)));
-  expected.insert(2, expmap(x2, Vector_(3, 1.3, 1.4, 1.5)));
+  expected.insert(PoseKey(2), expmap(x2, Vector_(3, 1.3, 1.4, 1.5)));
-  expected.insert(1, Point2(5.0, 6.1));
+  expected.insert(PointKey(1), Point2(5.0, 6.1));
-  expected.insert(2, Point2(10.3, 11.4));
+  expected.insert(PointKey(2), Point2(10.3, 11.4));
 
-  CHECK(assert_equal(expected, expmap(cfg1, increment)));
+  Config actual = expmap(cfg1, increment);
+  CHECK(assert_equal(expected, actual));
 }
 
 /* ************************************************************************* */
@@ -323,18 +324,33 @@ TEST(TupleConfig, typedefs)
 {
 	TupleConfig2<PoseConfig, PointConfig> cfg1;
 	TupleConfig3<PoseConfig, PointConfig, LamConfig> cfg2;
-	TupleConfig4<PoseConfig, PointConfig, LamConfig, Point3Config> cfg3;
+//	TupleConfig4<PoseConfig, PointConfig, LamConfig, Point3Config> cfg3;
-	TupleConfig5<PoseConfig, PointConfig, LamConfig, Point3Config, Pose3Config> cfg4;
+//	TupleConfig5<PoseConfig, PointConfig, LamConfig, Point3Config, Pose3Config> cfg4;
-	TupleConfig6<PoseConfig, PointConfig, LamConfig, Point3Config, Pose3Config, Point3Config2> cfg5;
+//	TupleConfig6<PoseConfig, PointConfig, LamConfig, Point3Config, Pose3Config, Point3Config2> cfg5;
 }
 
 /* ************************************************************************* */
-TEST( TupleConfig, constructor_insert )
+TEST( TupleConfig, pairconfig_style )
 {
-	PoseConfig cfg1;
+	PoseKey x1(1);
-	PointConfig cfg2;
+	PointKey l1(1);
-	LamConfig cfg3;
+	LamKey L1(1);
+	Pose2 pose1(1.0, 2.0, 0.3);
+	Point2 point1(2.0, 3.0);
+	Vector lam1 = Vector_(1, 2.3);
+
+	PoseConfig cfg1; cfg1.insert(x1, pose1);
+	PointConfig cfg2; cfg2.insert(l1, point1);
+	LamConfig cfg3; cfg3.insert(L1, lam1);
+
+	// Constructor
 	TupleConfig3<PoseConfig, PointConfig, LamConfig> config(cfg1, cfg2, cfg3);
+
+	// access
+	CHECK(assert_equal(cfg1, config.first()));
+	CHECK(assert_equal(cfg2, config.second()));
+	CHECK(assert_equal(cfg3, config.third()));
+
 }
 
 /* ************************************************************************* */