diff --git a/gtsam.h b/gtsam.h
index cde64b439..c0e90f4a7 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -37,7 +37,8 @@ class Pose2 {
 	double x() const;
 	double y() const;
 	double theta() const;
-	size_t dim() const;
+	int dim() const;
+	Pose2* compose_(const Pose2& p2);
 	Pose2* between_(const Pose2& p2);
 };
 
@@ -52,19 +53,19 @@ class SharedDiagonal {
 
 class VectorValues {
 	VectorValues();
-	VectorValues(size_t nVars, size_t varDim);
+	VectorValues(int nVars, int varDim);
 	void print(string s) const;
 	bool equals(const VectorValues& expected, double tol) const;
-	size_t size() const;
-  void insert(size_t j, const Vector& value);
+	int size() const;
+	void insert(int j, const Vector& value);
 };
 
 class GaussianConditional {
-	GaussianConditional(size_t key, Vector d, Matrix R, Vector sigmas);
-	GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
+	GaussianConditional(int key, Vector d, Matrix R, Vector sigmas);
+	GaussianConditional(int key, Vector d, Matrix R, int name1, Matrix S,
 			Vector sigmas);
-	GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
-			size_t name2, Matrix T, Vector sigmas);
+	GaussianConditional(int key, Vector d, Matrix R, int name1, Matrix S,
+			int name2, Matrix T, Vector sigmas);
 	void print(string s) const;
 	bool equals(const GaussianConditional &cg, double tol) const;
 };
@@ -86,11 +87,11 @@ class GaussianFactor {
 class JacobianFactor {
 	JacobianFactor();
 	JacobianFactor(Vector b_in);
-	JacobianFactor(size_t i1, Matrix A1, Vector b, const SharedDiagonal& model);
-	JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b,
+	JacobianFactor(int i1, Matrix A1, Vector b, const SharedDiagonal& model);
+	JacobianFactor(int i1, Matrix A1, int i2, Matrix A2, Vector b,
 			const SharedDiagonal& model);
-	JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3,
-			Matrix A3, Vector b, const SharedDiagonal& model);
+	JacobianFactor(int i1, Matrix A1, int i2, Matrix A2, int i3, Matrix A3,
+			Vector b, const SharedDiagonal& model);
 	void print(string s) const;
 	bool equals(const GaussianFactor& lf, double tol) const;
 	bool empty() const;
@@ -104,13 +105,13 @@ class GaussianFactorGraph {
 	void print(string s) const;
 	bool equals(const GaussianFactorGraph& lfgraph, double tol) const;
 
-	size_t size() const;
+	int size() const;
 	void push_back(GaussianFactor* ptr_f);
 	double error(const VectorValues& c) const;
 	double probPrime(const VectorValues& c) const;
 	void combine(const GaussianFactorGraph& lfg);
-  Matrix denseJacobian() const;
-  Matrix denseHessian() const;
+	Matrix denseJacobian() const;
+	Matrix denseHessian() const;
 	Matrix sparseJacobian_() const;
 };
 
@@ -132,9 +133,11 @@ class Landmark2 {
 	double y();
 };
 
-class Ordering{
-  void print(string s) const;
-  bool equals(const Ordering& ord, double tol) const;
+class Ordering {
+	Ordering();
+	void print(string s) const;
+	bool equals(const Ordering& ord, double tol) const;
+	void push_back(string key);
 };
 
 class PlanarSLAMValues {
@@ -152,17 +155,22 @@ class PlanarSLAMGraph {
 
 	double error(const PlanarSLAMValues& c) const;
 	Ordering* orderingCOLAMD(const PlanarSLAMValues& config) const;
-	GaussianFactorGraph* linearize(const PlanarSLAMValues& config, const Ordering& ordering) const;
+	GaussianFactorGraph* linearize(const PlanarSLAMValues& config,
+			const Ordering& ordering) const;
 
-	void addPrior(size_t i, const Pose2& p, const SharedNoiseModel& model);
-	void addPoseConstraint(size_t i, const Pose2& p);
-	void addOdometry(size_t i, size_t j, const Pose2& z,
-			const SharedNoiseModel& model);
-	void addBearing(size_t i, size_t j, const Rot2& z,
-			const SharedNoiseModel& model);
-	void addRange(size_t i, size_t j, double z, const SharedNoiseModel& model);
-	void addBearingRange(size_t i, size_t j, const Rot2& z1, double z2,
+	void addPrior(int i, const Pose2& p, const SharedNoiseModel& model);
+	void addPoseConstraint(int i, const Pose2& p);
+	void addOdometry(int i, int j, const Pose2& z, const SharedNoiseModel& model);
+	void addBearing(int i, int j, const Rot2& z, const SharedNoiseModel& model);
+	void addRange(int i, int j, double z, const SharedNoiseModel& model);
+	void addBearingRange(int i, int j, const Rot2& z1, double z2,
 			const SharedNoiseModel& model);
 	PlanarSLAMValues* optimize_(const PlanarSLAMValues& initialEstimate);
 };
 
+class PlanarSLAMOdometry {
+	PlanarSLAMOdometry(int key1, int key2, const Pose2& measured,
+			const SharedNoiseModel& model);
+	void print(string s) const;
+	GaussianFactor* linearize(const PlanarSLAMValues& c, const Ordering& ordering) const;
+};
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index e697fe745..e45b9d72f 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -104,6 +104,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> compose_(const Pose2& p2) {
+    	return boost::shared_ptr<Pose2>(new Pose2(compose(p2)));
+    }
+
     /** syntactic sugar for transform_from */
     inline Point2 operator*(const Point2& point) const { return transform_from(point);}