From 638b1745418cde506b549413f808708c5a4aa324 Mon Sep 17 00:00:00 2001
From: Viorela Ila <iviorela@gmail.com>
Date: Wed, 9 Dec 2009 19:55:25 +0000
Subject: [PATCH] Pose2Constraint works in matlab

---
 cpp/Makefile.am       |  1 +
 cpp/Pose2Config.h     | 15 +++++++++++++++
 cpp/Pose2Constraint.h | 10 +---------
 cpp/gtsam.h           | 15 +++++++++++++++
 4 files changed, 32 insertions(+), 9 deletions(-)
 create mode 100644 cpp/Pose2Config.h

diff --git a/cpp/Makefile.am b/cpp/Makefile.am
index 5cba74b96..c0497cfc7 100644
--- a/cpp/Makefile.am
+++ b/cpp/Makefile.am
@@ -124,6 +124,7 @@ headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
 headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h 
 headers += SQPOptimizer.h SQPOptimizer-inl.h  
 headers += NonlinearConstraint.h NonlinearConstraint-inl.h
+headers += Pose2Config.h
 headers += Pose2Constraint.h
 sources += NonlinearFactor.cpp 
 sources += NonlinearEquality.h
diff --git a/cpp/Pose2Config.h b/cpp/Pose2Config.h
new file mode 100644
index 000000000..7a4fa7f1d
--- /dev/null
+++ b/cpp/Pose2Config.h
@@ -0,0 +1,15 @@
+#include <map>
+#include "Pose2.h"
+
+namespace gtsam {
+
+class Pose2Config: public std::map<std::string, Pose2> {
+public:
+	Pose2 get(std::string key) const {
+		std::map<std::string, Pose2>::const_iterator it = find(key);
+		if (it == end())
+			throw std::invalid_argument("invalid key");
+		return it->second;
+	}
+};
+} // namespace
diff --git a/cpp/Pose2Constraint.h b/cpp/Pose2Constraint.h
index 414ea1b92..920623081 100644
--- a/cpp/Pose2Constraint.h
+++ b/cpp/Pose2Constraint.h
@@ -8,18 +8,10 @@
 #include "VectorConfig.h"
 #include "Pose2.h"
 #include "Matrix.h"
+#include "Pose2Config.h"
 
 namespace gtsam {
 
-class Pose2Config : public std::map<std::string,Pose2> {
-public:
-	Pose2 get(std::string key) const {
-		std::map<std::string,Pose2>::const_iterator it = find(key);
-		if (it==end()) throw std::invalid_argument("invalid key");
-		return it->second;
-	}
-};
-
 class Pose2Constraint : public Factor<Pose2Config> {
 private:
 	std::string key1_, key2_; /** The keys of the two poses, order matters */
diff --git a/cpp/gtsam.h b/cpp/gtsam.h
index 0531a937a..7e9701c8d 100644
--- a/cpp/gtsam.h
+++ b/cpp/gtsam.h
@@ -158,6 +158,21 @@ class Simulated2DMeasurement {
   void print(string s) const;
 };
 
+class Pose2{
+	Pose2();
+	Pose2(const Pose2& pose);
+	Pose2(double x, double y, double theta);
+	void print(string s);
+	bool equals(const Pose2& pose, double tol);
+	double x();
+	double y();
+	double theta();
+	size_t dim() const;
+	Pose2 exmap(const Vector& v) const;
+	Vector vector() const;
+	Pose2 rotate(double theta) const;
+};
+
 class Pose2Config{
 	Pose2 get(string key) const;
 };