From 0a48b45b12f796670f98727bc246358ca6246087 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Wed, 17 Mar 2010 16:24:22 +0000
Subject: [PATCH] Added insertSub() to tupleconfig to allow the insertion of a
 subtype of config at once.

---
 cpp/TupleConfig.h       |  8 ++++++++
 cpp/testTupleConfig.cpp | 33 ++++++++++++++++++++++++---------
 2 files changed, 32 insertions(+), 9 deletions(-)

diff --git a/cpp/TupleConfig.h b/cpp/TupleConfig.h
index a870e1d3a..ffe236575 100644
--- a/cpp/TupleConfig.h
+++ b/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_; }
diff --git a/cpp/testTupleConfig.cpp b/cpp/testTupleConfig.cpp
index 063164ab5..adba15d80 100644
--- a/cpp/testTupleConfig.cpp
+++ b/cpp/testTupleConfig.cpp
@@ -457,18 +457,33 @@ TEST(TupleConfig, insert_config_typedef) {
 }
 
 /* ************************************************************************* */
-#include "NonlinearFactorGraph-inl.h"
-TEST( TupleConfig, graphs_and_factors )
-{
-	typedef TupleConfig3<PoseConfig, PointConfig, LamConfig> ConfigC;
-	typedef NonlinearFactorGraph<ConfigC> GraphC;
-	typedef NonlinearFactor1<ConfigC, PoseKey, Pose2> FactorC;
+TEST(TupleConfig, partial_insert) {
+	TupleConfig3<PoseConfig, PointConfig, LamConfig> init, expected;
 
-	// test creation
-	GraphC graph;
-	ConfigC config;
+	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);
 
+	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); }
 /* ************************************************************************* */