Pose2Constraint works in matlab

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

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

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 */

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;
 };