Added ability to insert whole configs at a time to LieConfig and TupleConfig

cpp/LieConfig-inl.h

@@ -60,6 +60,13 @@ namespace gtsam {
     dim_ += gtsam::dim(val);
   }
 
+  template<class J, class T>
+  void LieConfig<J,T>::insert(const LieConfig<J,T>& cfg) {
+	  BOOST_FOREACH(const typename Values::value_type& v, cfg.values_)
+		 insert(v.first, v.second);
+	  dim_ += cfg.dim_;
+  }
+
   template<class J, class T>
   void LieConfig<J,T>::erase(const J& j) {
     size_t dim; // unused

cpp/LieConfig.h

@@ -82,6 +82,9 @@ namespace gtsam {
     /** Add a variable with the given j */
     void insert(const J& j, const T& val);
 
+    /** Add a set of variables */
+    void insert(const LieConfig& cfg);
+
     /** Remove a variable from the config */
     void erase(const J& j);
 

cpp/TupleConfig.h

@@ -59,6 +59,14 @@ namespace gtsam {
 	  void insert(const Key& key, const Value& value) {second_.insert(key, value);}
 	  void insert(const Key1& key, const Value1& value) {first_.insert(key, value);}
 
+	  // insert function for whole configs
+	  template<class Cfg1, class Cfg2>
+	  void insert(const TupleConfig<Cfg1, Cfg2>& config) { second_.insert(config); }
+	  void insert(const TupleConfig<Config1, Config2>& config) {
+		  first_.insert(config.first_);
+		  second_.insert(config.second_);
+	  }
+
 	  // erase an element by key
 	  template<class Key>
 	  void erase(const Key& j)  { second_.erase(j); }
@@ -140,6 +148,9 @@ namespace gtsam {
 
 	  void insert(const Key1& key, const Value1& value) {first_.insert(key, value); }
 
+	  // insert function for whole configs
+	  void insert(const TupleConfigEnd<Config>& config) {first_.insert(config.first_); }
+
 	  const Value1& operator[](const Key1& j) const { return first_[j]; }
 
 	  const Config& config() const { return first_; }

cpp/testLieConfig.cpp

@@ -53,6 +53,29 @@ TEST( LieConfig, equals_nan )
   CHECK(!cfg2.equals(cfg1));
 }
 
+/* ************************************************************************* */
+TEST( LieConfig, insert_config )
+{
+  LieConfig<string,Vector> 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("x1", v1);
+  cfg1.insert("x2", v2);
+  cfg2.insert("x2", v3);
+  cfg2.insert("x3", v4);
+
+  cfg1.insert(cfg2);
+
+  expected.insert("x1", v1);
+  expected.insert("x2", v2);
+  expected.insert("x2", v3);
+  expected.insert("x3", v4);
+
+  CHECK(assert_equal(cfg1, expected));
+}
+
 /* ************************************************************************* */
 TEST(LieConfig, expmap_a)
 {

cpp/testTupleConfig.cpp

@@ -251,6 +251,37 @@ TEST(TupleConfig, basic_functions) {
 	CHECK(configB.size() == 2);
 }
 
+/* ************************************************************************* */
+TEST(TupleConfig, insert_config) {
+	ConfigB config1, config2, expected;
+
+	PoseKey x1(1), x2(2);
+	PointKey l1(1), l2(2);
+	LamKey L1(1), L2(2);
+	Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0);
+	Point2 point1(2.0, 3.0), point2(5.0, 6.0);
+	Vector lam1 = Vector_(1, 2.3), lam2 = Vector_(1, 4.5);
+
+	config1.insert(x1, pose1);
+	config1.insert(l1, point1);
+	config1.insert(L1, lam1);
+
+	config2.insert(x2, pose2);
+	config2.insert(l2, point2);
+	config2.insert(L2, lam2);
+
+	config1.insert(config2);
+
+	expected.insert(x1, pose1);
+	expected.insert(l1, point1);
+	expected.insert(L1, lam1);
+	expected.insert(x2, pose2);
+	expected.insert(l2, point2);
+	expected.insert(L2, lam2);
+
+	CHECK(assert_equal(expected, config1));
+}
+
 /* ************************************************************************* */
 TEST( TupleConfig, equals )
 {