From a1e90af90f52218e361832268e16082def7d4a8a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 11 Dec 2009 22:43:34 +0000
Subject: [PATCH] exmap that takes Vector

---
 cpp/VectorConfig.cpp     | 22 ++++++++++++++++++----
 cpp/VectorConfig.h       |  8 +++++++-
 cpp/testVectorConfig.cpp | 12 ++++++++++--
 3 files changed, 35 insertions(+), 7 deletions(-)

diff --git a/cpp/VectorConfig.cpp b/cpp/VectorConfig.cpp
index 4a3a18238..5fd1ba374 100644
--- a/cpp/VectorConfig.cpp
+++ b/cpp/VectorConfig.cpp
@@ -88,12 +88,11 @@ VectorConfig VectorConfig::operator-(const VectorConfig& b) const {
 }
 
 /* ************************************************************************* */
-VectorConfig VectorConfig::exmap(const VectorConfig & delta) const
+VectorConfig VectorConfig::exmap(const VectorConfig& delta) const
 {
 	VectorConfig newConfig;
-	for (const_iterator it = values.begin(); it!=values.end(); it++) {
-		string j = it->first;
-		const Vector &vj = it->second;
+	string j; Vector vj;
+	FOREACH_PAIR(j, vj, values) {
 		if (delta.contains(j)) {
 			const Vector& dj = delta[j];
 			check_size(j,vj,dj);
@@ -105,6 +104,21 @@ VectorConfig VectorConfig::exmap(const VectorConfig & delta) const
 	return newConfig;
 }
 
+/* ************************************************************************* */
+VectorConfig VectorConfig::exmap(const Vector& delta) const
+{
+	VectorConfig newConfig;
+	size_t i = 0;
+	string j; Vector vj;
+	FOREACH_PAIR(j, vj, values) {
+		size_t mj = vj.size();
+		Vector dj = sub(delta, i, i+mj);
+		newConfig.insert(j, vj + dj);
+		i += mj;
+	}
+	return newConfig;
+}
+
 /* ************************************************************************* */
 const Vector& VectorConfig::get(const std::string& name) const {
   const_iterator it = values.find(name);
diff --git a/cpp/VectorConfig.h b/cpp/VectorConfig.h
index 2a7ca9cb3..e5012c7e0 100644
--- a/cpp/VectorConfig.h
+++ b/cpp/VectorConfig.h
@@ -46,7 +46,13 @@ namespace gtsam {
      * Add a delta config, needed for use in NonlinearOptimizer
      * For VectorConfig, this is just addition.
      */
-    VectorConfig exmap(const VectorConfig & delta) const;
+    VectorConfig exmap(const VectorConfig& delta) const;
+
+    /**
+     * Add a delta vector (not a config)
+     * Will use the ordering that map uses to loop over vectors
+     */
+    VectorConfig exmap(const Vector& delta) const;
 
     const_iterator begin() const {return values.begin();}
     const_iterator end()   const {return values.end();}
diff --git a/cpp/testVectorConfig.cpp b/cpp/testVectorConfig.cpp
index 25c9594c4..b2f4afad9 100644
--- a/cpp/testVectorConfig.cpp
+++ b/cpp/testVectorConfig.cpp
@@ -31,7 +31,7 @@ TEST( VectorConfig, equals1 )
    expected.insert("a",v);
    VectorConfig actual;
    actual.insert("a",v);
-   CHECK(actual.equals(expected));
+   CHECK(assert_equal(expected,actual));
 }
 
 /* ************************************************************************* */
@@ -68,6 +68,14 @@ TEST( VectorConfig, contains)
   CHECK(!fg.contains("gholi"));
 }
 
+/* ************************************************************************* */
+TEST( VectorConfig, exmap)
+{
+	VectorConfig c = createConfig();
+	Vector v = Vector_(6, 0.0,-1.0, 0.0, 0.0, 1.5, 0.0); // l1, x1, x2
+  CHECK(assert_equal(c.exmap(c),c.exmap(v)));
+}
+
 /* ************************************************************************* */
 TEST( VectorConfig, plus)
 {
@@ -85,7 +93,7 @@ TEST( VectorConfig, plus)
 
   // functional
   VectorConfig actual = fg.exmap(delta);
-  CHECK(actual.equals(expected));
+  CHECK(assert_equal(expected,actual));
 }
 
 /* ************************************************************************* */