diff --git a/cpp/Point2.h b/cpp/Point2.h
index 4bb12c543..f50b55a32 100644
--- a/cpp/Point2.h
+++ b/cpp/Point2.h
@@ -30,8 +30,8 @@ namespace gtsam {
     /** print with optional string */
     void print(const std::string& s = "") const;
 
-		/** equals with an tolerance, prints out message if unequal*/
-		bool equals(const Point2& q, double tol = 1e-9) const;
+    /** equals with an tolerance, prints out message if unequal*/
+    bool equals(const Point2& q, double tol = 1e-9) const;
 
     /** get functions for x, y */
     double x() const {return x_;}
@@ -40,7 +40,7 @@ namespace gtsam {
     /** return DOF, dimensionality of tangent space */
     size_t dim() const { return 2;}
 		
-    /** Given 3-dim tangent vector, create new rotation */
+    /** Given 2-dim tangent vector, create new point */
     Point2 exmap(const Vector& d) const { 
       return Point2(x_+d(0),y_+d(1));
     }
diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp
index c4f4c82dd..f128bee89 100644
--- a/cpp/Pose2.cpp
+++ b/cpp/Pose2.cpp
@@ -11,88 +11,66 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	void Pose2::print(const string& s) const {
-		cout << s << "(" << x_ << ", " << y_ << ", " << theta_ << ")" << endl;
+		cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl;
 	}
 
 	/* ************************************************************************* */
 	bool Pose2::equals(const Pose2& q, double tol) const {
-		return (fabs(x_ - q.x_) < tol && fabs(y_ - q.y_) < tol && fabs(theta_
-				- q.theta_) < tol);
+		return t_.equals(q.t_, tol) && r_.equals(q.r_, tol);
 	}
 
 	/* ************************************************************************* */
 	Pose2 Pose2::exmap(const Vector& v) const {
-		return Pose2(x_ + v(0), y_ + v(1), theta_ + v(2));
+	  return Pose2(t_+Point2(v(0),v(1)), r_.exmap(Vector_(1, v(2))));
 	}
 
 	/* ************************************************************************* */
 	Vector Pose2::vector() const {
-		Vector v(3);
-		v(0) = x_;
-		v(1) = y_;
-		v(2) = theta_;
-		return v;
+		return Vector_(3, t_.x(), t_.y(), r_.theta());
 	}
 
 	/* ************************************************************************* */
-	Pose2 Pose2::rotate(double theta) const {
-		double c = cos(theta), s = sin(theta);
-		return Pose2(c * x_ - s * y_, s * x_ + c * y_, theta + theta_);
-	}
+//	Pose2 Pose2::rotate(double theta) const {
+//		//return Pose2(t_, Rot2(theta)*r_);
+//	  return Pose2(Point2(0.0,0.0),-theta)*(*this);
+//	}
 
 	/* ************************************************************************* */
 	Point2 transform_to(const Pose2& pose, const Point2& point) {
-		double dx = point.x()-pose.x(), dy = point.y()-pose.y();
-		double ct=cos(pose.theta()), st=sin(pose.theta());
-		return Point2(ct*dx + st*dy, -st*dx + ct*dy);
+	  return pose*point;
 	}
 
 	// TODO, have a combined function that returns both function value and derivative
 	Matrix Dtransform_to1(const Pose2& pose, const Point2& point) {
-		double dx = point.x()-pose.x(), dy = point.y()-pose.y();
-		double ct=cos(pose.theta()), st=sin(pose.theta());
-		double transformed_x = ct*dx + st*dy, transformed_y = -st*dx + ct*dy;
-		return Matrix_(2,3,
-				-ct, -st,  transformed_y,
-				 st, -ct, -transformed_x
-				);
+	  Matrix dx_dt = pose.r().negtranspose();
+	  Matrix dx_dr = Dunrotate1(pose.r(), point-pose.t());
+	  return collect(2, &dx_dt, &dx_dr);
 	}
 
 	Matrix Dtransform_to2(const Pose2& pose, const Point2& point) {
-		double ct=cos(pose.theta()), st=sin(pose.theta());
-		return Matrix_(2,2,
-				 ct,  st,
-				-st,  ct
-				);
+	  return pose.r().transpose();
 	}
 
 	/* ************************************************************************* */
 	Pose2 between(const Pose2& p1, const Pose2& p2) {
-		double dx = p2.x()-p1.x(), dy = p2.y()-p1.y();
-		double ct=cos(p1.theta()), st=sin(p1.theta());
-		return Pose2(ct*dx + st*dy, -st*dx + ct*dy, p2.theta()-p1.theta());
+		return Pose2(
+		    p1.r().invcompose(p2.r()),
+		    p1.r().unrotate(p2.t() - p1.t()));
 	}
 
 	Matrix Dbetween1(const Pose2& p1, const Pose2& p2) {
-		double dx = p2.x()-p1.x(), dy = p2.y()-p1.y();
-		double ct=cos(p1.theta()), st=sin(p1.theta());
-		double transformed_x = ct*dx + st*dy, transformed_y = -st*dx + ct*dy;
-		return Matrix_(3,3,
-				-ct, -st,  transformed_y,
-				 st, -ct, -transformed_x,
-				0.0, 0.0, -1.0
-				);
+	  Matrix dbt_dp = Dtransform_to1(p1, p2.t());
+	  Matrix dbr_dp = Matrix_(1,3, 0.0, 0.0, -1.0);
+	  return stack(2, &dbt_dp, &dbr_dp);
 	}
 
 	Matrix Dbetween2(const Pose2& p1, const Pose2& p2) {
-		double ct=cos(p1.theta()), st=sin(p1.theta());
-		return Matrix_(3,3,
-				 ct,  st, 0.0,
-				-st,  ct, 0.0,
-				0.0, 0.0, 1.0
-				);
+	  Matrix db_dt2 = p1.r().transpose();
+	  return Matrix_(3,3,
+	      db_dt2.data()[0], db_dt2.data()[1], 0.0,
+	      db_dt2.data()[2], db_dt2.data()[3], 0.0,
+	      0.0,              0.0,              1.0);
 	}
 
 	/* ************************************************************************* */
-
 } // namespace gtsam
diff --git a/cpp/Pose2.h b/cpp/Pose2.h
index f451f103b..b01618ea7 100644
--- a/cpp/Pose2.h
+++ b/cpp/Pose2.h
@@ -1,7 +1,8 @@
 /**
  * @file  Pose2.h
- * @brief 3D Pose
+ * @brief 2D Pose
  * @author: Frank Dellaert
+ * @author: Richard Roberts
  */
 
 // \callgraph
@@ -9,93 +10,117 @@
 #pragma once
 
 #include "Point2.h"
+#include "Rot2.h"
 #include "Matrix.h"
 #include "Testable.h"
 
 namespace gtsam {
 
-	/**
-	 * A 2D pose (x,y,theta)
-	 */
-	class Pose2: Testable<Pose2>  {
+/**
+ * A 2D pose (Point2,Rot2)
+ */
+class Pose2: Testable<Pose2>  {
 
-	private:
-		double x_, y_, theta_;
+private:
+  Point2 t_;
+  Rot2 r_;
 
-	public:
+public:
 
-		/** default constructor = origin */
-		Pose2() :
-			x_(0), y_(0), theta_(0) {
-		} // default is origin
+  /** default constructor = origin */
+  Pose2() :
+    t_(0.0, 0.0), r_(0) { } // default is origin
 
-		/** copy constructor */
-		Pose2(const Pose2& pose) :
-			x_(pose.x_), y_(pose.y_), theta_(pose.theta_) {
-		}
+  /** copy constructor */
+  Pose2(const Pose2& pose) :
+    t_(pose.t_), r_(pose.r_) { }
 
-		/**
-		 * construct from (x,y,theta)
-		 * @param x x oordinate
-		 * @param y y coordinate
-		 * @param theta angle with positive X-axis
-		 */
-		Pose2(double x, double y, double theta) :
-			x_(x), y_(y), theta_(theta) {
-		}
+  /**
+   * construct from (x,y,theta)
+   * @param x x coordinate
+   * @param y y coordinate
+   * @param theta angle with positive X-axis
+   */
+  Pose2(double x, double y, double theta) :
+    t_(x,y), r_(theta) { }
 
-		/** construct from rotation and translation */
-		Pose2(const Point2& t, double theta) :
-			x_(t.x()), y_(t.y()), theta_(theta) {
-		}
+  /** construct from rotation and translation */
+  Pose2(const Point2& t, double theta) :
+    t_(t), r_(theta) { }
+  Pose2(double theta, const Point2& t) :
+    t_(t), r_(theta) { }
+  Pose2(const Point2& t, const Rot2& r) :
+    t_(t), r_(r) { }
+  Pose2(const Rot2& r, const Point2& t) :
+    t_(t), r_(r) { }
 
-		/** print with optional string */
-		void print(const std::string& s = "") const;
+  /** print with optional string */
+  void print(const std::string& s = "") const;
 
-		/** assert equality up to a tolerance */
-		bool equals(const Pose2& pose, double tol = 1e-9) const;
+  /** assert equality up to a tolerance */
+  bool equals(const Pose2& pose, double tol = 1e-9) const;
 
-    /** get functions for x, y, theta */
-		inline double x()     const { return x_;}
-		inline double y()     const { return y_;}
-		inline double theta() const { return theta_;}
+  /** get functions for x, y, theta */
+  double x()     const { return t_.x();}
+  double y()     const { return t_.y();}
+  double theta() const { return r_.theta();}
+  Point2 t()     const { return t_; }
+  Rot2 r()       const { return r_; }
 
-		/** return DOF, dimensionality of tangent space = 3 */
-		size_t dim() const {
-			return 3;
-		}
+  /** return DOF, dimensionality of tangent space = 3 */
+  size_t dim() const { return 3; }
 
-		/* exponential map */
-		Pose2 exmap(const Vector& v) const;
+  /* exponential map */
+  Pose2 exmap(const Vector& v) const;
 
-    /** return vectorized form (column-wise)*/
-    Vector vector() const;
+  /** return vectorized form (column-wise) */
+  Vector vector() const;
 
-    /** rotate pose by theta */
-    Pose2 rotate(double theta) const;
+  /** rotate pose by theta */
+//  Pose2 rotate(double theta) const;
 
-    // operators
-    Pose2 operator+(const Pose2& p2) const {
-  		return Pose2(x_ + p2.x_, y_ + p2.y_, theta_ + p2.theta_);
-    }
+  /** inverse transformation */
+  Pose2 inverse() const {
+    return Pose2(r_, r_.unrotate(t_));
+  }
 
-    Pose2 operator-(const Pose2& p2) const {
-  		return Pose2(x_ - p2.x_, y_ - p2.y_, theta_ - p2.theta_);
-    }
-	}; // Pose2
+  /** compose this transformation onto another (pre-multiply this*p1) */
+  Pose2 compose(const Pose2& p1) const {
+    return Pose2(p1.r_*r_, p1.r_*t_+p1.t_);
+  }
 
-	/**
-	 * Return point coordinates in pose coordinate frame
-	 */
-	Point2 transform_to(const Pose2& pose, const Point2& point);
-	Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
-	Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
+  /** same as compose (pre-multiply this*p1) */
+  Pose2 operator*(const Pose2& p1) const {
+    return compose(p1);
+  }
 
-	/**
-	 * Return relative pose between p1 and p2, in p1 coordinate frame
-	 */
-	Pose2 between(const Pose2& p1, const Pose2& p2);
-	Matrix Dbetween1(const Pose2& p1, const Pose2& p2);
-	Matrix Dbetween2(const Pose2& p1, const Pose2& p2);
+  /** Return point coordinates in pose coordinate frame, same as transform_to */
+  Point2 operator*(const Point2& point) const {
+    return r_.unrotate(point-t_);
+  }
+
+  // operators
+  Pose2 operator+(const Pose2& p2) const {
+    return Pose2(t_+p2.t_, r_*p2.r_);
+  }
+
+  Pose2 operator-(const Pose2& p2) const {
+    return Pose2(t_-p2.t_, r_.invcompose(p2.r_));
+  }
+}; // Pose2
+
+/**
+ * Return point coordinates in pose coordinate frame
+ */
+Point2 transform_to(const Pose2& pose, const Point2& point);
+Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
+Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
+
+/**
+ * Return relative pose between p1 and p2, in p1 coordinate frame
+ */
+Pose2 between(const Pose2& p1, const Pose2& p2);
+Matrix Dbetween1(const Pose2& p1, const Pose2& p2);
+Matrix Dbetween2(const Pose2& p1, const Pose2& p2);
 
 } // namespace gtsam
diff --git a/cpp/Rot2.cpp b/cpp/Rot2.cpp
index 5e86c8b48..4bf531444 100644
--- a/cpp/Rot2.cpp
+++ b/cpp/Rot2.cpp
@@ -29,19 +29,29 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	Matrix Rot2::matrix() const {
-		double r[] = { c_, -s_, s_, c_ };
-		return Matrix_(2, 2, r);
+		return Matrix_(2, 2, c_, -s_, s_, c_);
 	}
 
 	/* ************************************************************************* */
 	Matrix Rot2::transpose() const {
-		double r[] = { c_, s_, -s_, c_ };
-		return Matrix_(2, 2, r);
+		return Matrix_(2, 2, c_, s_, -s_, c_);
 	}
 
+    /* ************************************************************************* */
+    Matrix Rot2::negtranspose() const {
+        return Matrix_(2, 2, -c_, -s_, s_, -c_);
+    }
+
 	/* ************************************************************************* */
 	Rot2 Rot2::inverse() const { return Rot2(c_, -s_);}
 
+    /* ************************************************************************* */
+    Rot2 Rot2::invcompose(const Rot2& R) const {
+      return Rot2(
+           c_ * R.c_ + s_ * R.s_,
+          -s_ * R.c_ + c_ * R.s_);
+    }
+
 	/* ************************************************************************* */
 	Rot2 Rot2::operator*(const Rot2& R) const {
 		return Rot2(
diff --git a/cpp/Rot2.h b/cpp/Rot2.h
index 6efaa5dbc..c0470eea3 100644
--- a/cpp/Rot2.h
+++ b/cpp/Rot2.h
@@ -34,7 +34,14 @@ namespace gtsam {
 		Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
 
 		/** return angle */
-		inline double angle() const {	return atan2(s_, c_);	}
+		double theta() const { return atan2(s_,c_); }
+		double angle() const { return atan2(s_,c_); }
+
+		/** return cos */
+		double c() const { return c_; }
+
+		/** return sin */
+		double s() const { return s_; }
 
 		/** print */
 		void print(const std::string& s = "theta") const;
@@ -54,12 +61,18 @@ namespace gtsam {
 		/** return 2*2 rotation matrix */
 		Matrix matrix() const;
 
-		/** return 3*3 transpose (inverse) rotation matrix   */
+		/** return 2*2 transpose (inverse) rotation matrix   */
 		Matrix transpose() const;
 
+		/** return 2*2 negative transpose */
+		Matrix negtranspose() const;
+
 		/** inverse transformation  */
 		Rot2 inverse() const;
 
+		/** compose with the inverse of this rotation */
+		Rot2 invcompose(const Rot2& R) const;
+
 		/** composition via sum and difference formulas */
 		Rot2 operator*(const Rot2& R) const;
 
diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp
index 7460949e6..bbf372fe5 100644
--- a/cpp/testPose2.cpp
+++ b/cpp/testPose2.cpp
@@ -8,6 +8,7 @@
 #include "numericalDerivative.h"
 #include "Pose2.h"
 #include "Point2.h"
+#include "Rot2.h"
 
 using namespace gtsam;
 
@@ -20,13 +21,22 @@ TEST(Pose2, constructors) {
 }
 
 /* ************************************************************************* */
-TEST(Pose2, rotate) {
-	double theta = 0.1, c=cos(theta),s=sin(theta);
-	Pose2 p1(1,0,0.2), p2(0,1,0.4);
-	CHECK(assert_equal(Pose2( c,s,0.3),p1.rotate(theta)));
-	CHECK(assert_equal(Pose2(-s,c,0.5),p2.rotate(theta)));
+TEST(Pose2, exmap) {
+  Pose2 pose(Point2(1,2), M_PI_2);
+  Pose2 expected(Point2(1.01, 1.985), M_PI_2+0.018);
+  Pose2 actual = pose.exmap(Vector_(3, 0.01, -0.015, 0.018));
+  CHECK(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+//TEST(Pose2, rotate) {
+//  std::cout << "rotate\n";
+//	double theta = 0.1, c=cos(theta),s=sin(theta);
+//	Pose2 p1(1,0,0.2), p2(0,1,0.4);
+//	CHECK(assert_equal(Pose2( c,s,0.3),p1.rotate(theta)));
+//	CHECK(assert_equal(Pose2(-s,c,0.5),p2.rotate(theta)));
+//}
+
 /* ************************************************************************* */
 TEST(Pose2, operators) {
 	CHECK(assert_equal(Pose2(2,2,2),Pose2(1,1,1)+Pose2(1,1,1)));
@@ -60,6 +70,37 @@ TEST( Pose2, transform_to )
 	CHECK(assert_equal(numericalH2,actualH2));
 }
 
+/* ************************************************************************* */
+TEST(Pose2, compose_a)
+{
+  Pose2 pose1(Point2(.75, .5), Rot2(M_PI/10.0));
+  Pose2 pose2(Point2(0.701289620636, 1.34933052585), Rot2(M_PI/4.0-M_PI/10.0));
+
+  Pose2 pose_expected(Point2(1.0, 2.0), Rot2(M_PI/4.0));
+
+  Pose2 pose_actual_op = pose2 * pose1;
+  Pose2 pose_actual_fcn = pose2.compose(pose1);
+
+  CHECK(assert_equal(pose_expected, pose_actual_op));
+  CHECK(assert_equal(pose_expected, pose_actual_fcn));
+}
+
+/* ************************************************************************* */
+TEST(Pose2, compose_b)
+{
+  Pose2 pose1(Point2(1.0, 1.0), Rot2(M_PI/4.0));
+  Pose2 pose2(Point2(sqrt(.5), sqrt(.5)), Rot2(M_PI/4.0));
+
+  Pose2 pose_expected(Point2(1.0, 2.0), Rot2(M_PI/2.0));
+
+  Pose2 pose_actual_op = pose2 * pose1;
+  Pose2 pose_actual_fcn = pose2.compose(pose1);
+
+  CHECK(assert_equal(pose_expected, pose_actual_op));
+  CHECK(assert_equal(pose_expected, pose_actual_fcn));
+}
+
+
 /* ************************************************************************* */
 TEST( Pose2, between )
 {