From ca121c2872a93510552ab1cae75df45fce56bced Mon Sep 17 00:00:00 2001
From: Stephen Williams <stephen.vincent.williams@gmail.com>
Date: Tue, 29 Nov 2011 19:02:36 +0000
Subject: [PATCH] added retract_ function for matlab wrapper of Pose2

---
 gtsam.h                | 7 +++++--
 gtsam/geometry/Pose2.h | 5 +++++
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index 331bdd09a..834e1368b 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -41,6 +41,7 @@ class Pose2 {
 	Pose2* compose_(const Pose2& p2);
 	Pose2* between_(const Pose2& p2);
 	Vector localCoordinates(const Pose2& p);
+	Pose2* retract_(Vector v);
 };
 
 class SharedGaussian {
@@ -181,6 +182,8 @@ class PlanarSLAMOdometry {
 
 class GaussianSequentialSolver {
   GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR);
-  GaussianBayesNet* eliminate();
-  VectorValues* optimize();
+  GaussianBayesNet* eliminate() const;
+  VectorValues* optimize() const;
+  GaussianFactor* marginalFactor(int j) const;
+  Matrix marginalCovariance(int j) const;
 };
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index de311249a..cdbf141c3 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -126,6 +126,11 @@ public:
   /// Retraction from R^3 to Pose2 manifold neighborhood around current pose
 	Pose2 retract(const Vector& v) const;
 
+  /// MATLAB version returns shared pointer
+  boost::shared_ptr<Pose2> retract_(const Vector& v) {
+    return boost::shared_ptr<Pose2>(new Pose2(retract(v)));
+  }
+
   /// Local 3D coordinates of Pose2 manifold neighborhood around current pose
 	Vector localCoordinates(const Pose2& p2) const;