From 080496db1dfe370f4a4d67f4eb06cd3321439262 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 3 Nov 2011 21:44:25 +0000
Subject: [PATCH] Wrapped some more Pose2/PlanarSLAM functions with Richard for
 ASPN/CircleDemo.m

---
 gtsam.h                 | 2 ++
 gtsam/geometry/Pose2.h  | 5 +++++
 gtsam/slam/planarSLAM.h | 6 ++++++
 3 files changed, 13 insertions(+)

diff --git a/gtsam.h b/gtsam.h
index 034f9ef85..ba6c10006 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -38,6 +38,7 @@ class Pose2 {
 	double y() const;
 	double theta() const;
 	size_t dim() const;
+	Pose2* between_(const Pose2& p2);
 };
 
 class SharedGaussian {
@@ -129,6 +130,7 @@ class Ordering{
 class PlanarSLAMValues {
 	PlanarSLAMValues();
 	void print(string s) const;
+	Pose2* pose(int key);
 	void insertPose(int key, const Pose2& pose);
 	void insertPoint(int key, const Point2& point);
 };
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 783220db5..e697fe745 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -136,6 +136,11 @@ namespace gtsam {
     	boost::optional<Matrix&> H1=boost::none,
     	boost::optional<Matrix&> H2=boost::none) const;
 
+    /// MATLAB version returns shared pointer
+    boost::shared_ptr<Pose2> between_(const Pose2& p2) {
+    	return boost::shared_ptr<Pose2>(new Pose2(between(p2)));
+    }
+
     /** return transformation matrix */
     Matrix matrix() const;
 
diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h
index f400b54f3..86773b661 100644
--- a/gtsam/slam/planarSLAM.h
+++ b/gtsam/slam/planarSLAM.h
@@ -70,6 +70,12 @@ namespace gtsam {
 
 			// Convenience for MATLAB wrapper, which does not allow for identically named methods
 
+			/// get a pose
+	    boost::shared_ptr<Pose2> pose(int key) {
+	    	Pose2 pose = (*this)[PoseKey(key)];
+	    	return boost::shared_ptr<Pose2>(new Pose2(pose));
+	    }
+
 			/// insert a pose
 		  void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); }