Added insertSub() to tupleconfig to allow the insertion of a subtype of config at once.

cpp/TupleConfig.h

@@ -67,6 +67,11 @@ namespace gtsam {
 		  second_.insert(config.second_);
 	  }
 
+	  // insert a subconfig
+	  template<class Cfg>
+	  void insertSub(const Cfg& config) { second_.insertSub(config); }
+	  void insertSub(const Config1& config) { first_.insert(config);  }
+
 	  // erase an element by key
 	  template<class Key>
 	  void erase(const Key& j)  { second_.erase(j); }
@@ -151,6 +156,9 @@ namespace gtsam {
 	  // insert function for whole configs
 	  void insert(const TupleConfigEnd<Config>& config) {first_.insert(config.first_); }
 
+	  // insert function for sub configs
+	  void insertSub(const Config& config) {first_.insert(config); }
+
 	  const Value1& operator[](const Key1& j) const { return first_[j]; }
 
 	  const Config& config() const { return first_; }

cpp/testTupleConfig.cpp

@@ -457,18 +457,33 @@ TEST(TupleConfig, insert_config_typedef) {
 }
 
 /* ************************************************************************* */
-#include "NonlinearFactorGraph-inl.h"
+TEST(TupleConfig, partial_insert) {
-TEST( TupleConfig, graphs_and_factors )
+	TupleConfig3<PoseConfig, PointConfig, LamConfig> init, expected;
-{
-	typedef TupleConfig3<PoseConfig, PointConfig, LamConfig> ConfigC;
-	typedef NonlinearFactorGraph<ConfigC> GraphC;
-	typedef NonlinearFactor1<ConfigC, PoseKey, Pose2> FactorC;
 
-	// test creation
+	PoseKey x1(1), x2(2);
-	GraphC graph;
+	PointKey l1(1), l2(2);
-	ConfigC config;
+	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);
 
+	init.insert(x1, pose1);
+	init.insert(l1, point1);
+	init.insert(L1, lam1);
+
+	PoseConfig cfg1;
+	cfg1.insert(x2, pose2);
+
+	init.insertSub(cfg1);
+
+	expected.insert(x1, pose1);
+	expected.insert(l1, point1);
+	expected.insert(L1, lam1);
+	expected.insert(x2, pose2);
+
+	CHECK(assert_equal(expected, init));
 }
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */