From c45f64657f314b3f21f64d0749d6ba40d8251928 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Wed, 16 Dec 2009 21:40:41 +0000
Subject: [PATCH] Changes to make Pose2Config work with NonlinearOptimizer

---
 cpp/Pose2Config.h | 58 +++++++++++++++++++++++++++++++++++++++++++----
 1 file changed, 54 insertions(+), 4 deletions(-)

diff --git a/cpp/Pose2Config.h b/cpp/Pose2Config.h
index 481aa09c5..399f77ba1 100644
--- a/cpp/Pose2Config.h
+++ b/cpp/Pose2Config.h
@@ -1,24 +1,74 @@
 #pragma once
 
 #include <map>
+#include <boost/exception.hpp>
+#include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <iostream>
+#include <string>
 #include "Pose2.h"
+#include "Testable.h"
+#include "VectorConfig.h"
+#include "Vector.h"
+
+// trick from some reading group
+#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
+
 
 namespace gtsam {
 
-class Pose2Config: public std::map<std::string, Pose2> {
+class Pose2Config: public Testable<Pose2Config> {
+
+private:
+  std::map<std::string, Pose2> values;
 
 public:
 
 	Pose2Config() {}
+	Pose2Config(const Pose2Config &config) : values(config.values) { }
+	virtual ~Pose2Config() { }
 
 	Pose2 get(std::string key) const {
-		std::map<std::string, Pose2>::const_iterator it = find(key);
-		if (it == end())
+		std::map<std::string, Pose2>::const_iterator it = values.find(key);
+		if (it == values.end())
 			throw std::invalid_argument("invalid key");
 		return it->second;
 	}
 	void insert(const std::string& name, const Pose2& val){
-		std::map<std::string, Pose2>::insert(make_pair(name, val));
+		values.insert(make_pair(name, val));
 	}
+
+	Pose2Config& operator=(Pose2Config& rhs) {
+	  values = rhs.values;
+	  return (*this);
+	}
+
+	bool equals(const Pose2Config& expected, double tol) const {
+	  std::cerr << "Pose2Config::equals not implemented!" << std::endl;
+	  throw "Pose2Config::equals not implemented!";
+	}
+
+	void print(const std::string &s) const {
+	  std::cout << s << std::endl;
+	}
+
+    /**
+     * Add a delta config, needed for use in NonlinearOptimizer
+     */
+    Pose2Config exmap(const VectorConfig& delta) const {
+      Pose2Config newConfig;
+      std::string j; Pose2 vj;
+      FOREACH_PAIR(j, vj, values) {
+          if (delta.contains(j)) {
+              const Vector& dj = delta[j];
+              //check_size(j,vj,dj);
+              newConfig.insert(j, vj.exmap(dj));
+          } else {
+              newConfig.insert(j, vj);
+          }
+      }
+      return newConfig;
+    }
+
 };
 } // namespace