From d2d51d9f6812ba653745b542e01923bb0ca4a1fd Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Tue, 4 May 2010 13:41:46 +0000
Subject: [PATCH] Added single-element update function to TupleConfigs

---
 cpp/TupleConfig.h       |  7 +++++++
 cpp/testLieConfig.cpp   |  4 ++--
 cpp/testTupleConfig.cpp | 26 ++++++++++++++++++++++++++
 3 files changed, 35 insertions(+), 2 deletions(-)

diff --git a/cpp/TupleConfig.h b/cpp/TupleConfig.h
index aa8322244..05ca6da61 100644
--- a/cpp/TupleConfig.h
+++ b/cpp/TupleConfig.h
@@ -75,6 +75,11 @@ namespace gtsam {
 	  	second_.update(config.second_);
 	  }
 
+	  // update function for single elements
+	  template<class Key, class Value>
+	  void update(const Key& key, const Value& value) { second_.update(key, value); }
+	  void update(const Key1& key, const Value1& value) { first_.update(key, value); }
+
 	  // insert a subconfig
 	  template<class Cfg>
 	  void insertSub(const Cfg& config) { second_.insertSub(config); }
@@ -172,6 +177,8 @@ namespace gtsam {
 	  // update function for whole configs
 	  void update(const TupleConfigEnd<Config>& config) {first_.update(config.first_); }
 
+	  void update(const Key1& key, const Value1& value) { first_.update(key, value); }
+
 	  // insert function for sub configs
 	  void insertSub(const Config& config) {first_.insert(config); }
 
diff --git a/cpp/testLieConfig.cpp b/cpp/testLieConfig.cpp
index 0395fa8bb..a7c2049b3 100644
--- a/cpp/testLieConfig.cpp
+++ b/cpp/testLieConfig.cpp
@@ -80,7 +80,7 @@ TEST( LieConfig, insert_config )
 }
 
 /* ************************************************************************* */
-TEST( LieConfig, insert_overlap )
+TEST( LieConfig, update_element )
 {
   LieConfig<string,Vector> cfg;
   Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
@@ -92,7 +92,7 @@ TEST( LieConfig, insert_overlap )
 
   cfg.update("x1", v2);
   CHECK(cfg.size() == 1);
-  CHECK(assert_equal(v2, cfg.at("x1"))); // fails - need to change behavior
+  CHECK(assert_equal(v2, cfg.at("x1")));
 }
 
 /* ************************************************************************* */
diff --git a/cpp/testTupleConfig.cpp b/cpp/testTupleConfig.cpp
index 56448dc9f..cedcd8053 100644
--- a/cpp/testTupleConfig.cpp
+++ b/cpp/testTupleConfig.cpp
@@ -294,6 +294,32 @@ TEST(TupleConfig, insert_config) {
 	CHECK(assert_equal(expected, config1));
 }
 
+/* ************************************************************************* */
+TEST( TupleConfig, update_element )
+{
+	TupleConfig2<PoseConfig, PointConfig> cfg;
+	Pose2 x1(2.0, 1.0, 2.0), x2(3.0, 4.0, 5.0);
+	Point2 l1(1.0, 2.0), l2(3.0, 4.0);
+	PoseKey xk(1);
+	PointKey lk(1);
+
+	cfg.insert(xk, x1);
+	CHECK(cfg.size() == 1);
+	CHECK(assert_equal(x1, cfg.at(xk)));
+
+	cfg.update(xk, x2);
+	CHECK(cfg.size() == 1);
+	CHECK(assert_equal(x2, cfg.at(xk)));
+
+	cfg.insert(lk, l1);
+	CHECK(cfg.size() == 2);
+	CHECK(assert_equal(l1, cfg.at(lk)));
+
+	cfg.update(lk, l2);
+	CHECK(cfg.size() == 2);
+	CHECK(assert_equal(l2, cfg.at(lk)));
+}
+
 /* ************************************************************************* */
 TEST( TupleConfig, equals )
 {