diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index dfda7b3b1..91e915bd0 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -33,15 +33,21 @@ INSTANTIATE_LIE(Pose2);
 /** instantiate concept checks */
 GTSAM_CONCEPT_POSE_INST(Pose2);
 
-static const Matrix I3 = eye(3), Z12 = zeros(1,2);
+static const Matrix3 I3 = Eigen::Matrix3d::Identity(), Z12 = Eigen::Matrix3d::Zero();
 static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
 
 /* ************************************************************************* */
-Matrix Pose2::matrix() const {
-  Matrix R = r_.matrix();
-  R = stack(2, &R, &Z12);
-  Matrix T = (Matrix(3, 1) <<  t_.x(), t_.y(), 1.0).finished();
-  return collect(2, &R, &T);
+Matrix3 Pose2::matrix() const {
+  Matrix2 R = r_.matrix();
+  Matrix32 R0;
+  R0.block(0,0,2,2) = R;
+  R0.block(2,0,1,2).setZero();
+  Matrix31 T;
+  T <<  t_.x(), t_.y(), 1.0;
+  Matrix3 RT_;
+  RT_.block(0,0,3,2) = R0;
+  RT_.block(0,2,3,1) = T;
+  return RT_;
 }
 
 /* ************************************************************************* */
@@ -140,8 +146,8 @@ Point2 Pose2::transform_to(const Point2& point,
 
 /* ************************************************************************* */
 // see doc/math.lyx, SE(2) section
-Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H2) const {
+Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1,
+    OptionalJacobian<3,3> H2) const {
   // TODO: inline and reuse?
   if(H1) *H1 = p2.inverse().AdjointMap();
   if(H2) *H2 = I3;
@@ -154,10 +160,14 @@ Point2 Pose2::transform_from(const Point2& p,
     OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
   const Point2 q = r_ * p;
   if (H1 || H2) {
-    const Matrix R = r_.matrix();
-    const Matrix Drotate1 = (Matrix(2, 1) <<  -q.y(), q.x()).finished();
-    if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
-    if (H2) *H2 = R;                         // R
+    const Matrix2 R = r_.matrix();
+    Matrix21 Drotate1;
+    Drotate1 <<  -q.y(), q.x();
+    if (H1) {
+        (*H1).block(0,0,2,2) = R; // [R R_{pi/2}q]
+        (*H1).block(0,2,2,1) = Drotate1;
+    }
+    if (H2) *H2 = R; // R
   }
   return q + t_;
 }
@@ -198,24 +208,27 @@ Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1,
 
 /* ************************************************************************* */
 Rot2 Pose2::bearing(const Point2& point,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-  Point2 d = transform_to(point, H1, H2);
+    OptionalJacobian<1, 3> H1, OptionalJacobian<1, 2> H2) const {
+  // make temporary matrices
+  Matrix23 D1; Matrix2 D2;
+  Point2 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); // uses pointer version
   if (!H1 && !H2) return Rot2::relativeBearing(d);
-  Matrix D_result_d;
+  Matrix12 D_result_d;
   Rot2 result = Rot2::relativeBearing(d, D_result_d);
-  if (H1) *H1 = D_result_d * (*H1);
-  if (H2) *H2 = D_result_d * (*H2);
+  if (H1) *H1 = D_result_d * (D1);
+  if (H2) *H2 = D_result_d * (D2);
   return result;
 }
 
 /* ************************************************************************* */
 Rot2 Pose2::bearing(const Pose2& pose,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-  Rot2 result = bearing(pose.t(), H1, H2);
+    OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const {
+  Matrix12 D2;
+  Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0);
   if (H2) {
-    Matrix H2_ = *H2 * pose.r().matrix();
-    *H2 = zeros(1, 3);
-    insertSub(*H2, H2_, 0, 0);
+    Matrix12 H2_ = D2 * pose.r().matrix();
+    (*H2).setZero();
+    (*H2).block(0,0,1,2) = H2_;
   }
   return result;
 }
@@ -227,29 +240,37 @@ double Pose2::range(const Point2& point,
   Matrix12 H;
   double r = d.norm(H);
   if (H1) {
-      Matrix23 mvalue; // is this the correct name ?
-      mvalue << -r_.c(),  r_.s(),  0.0,
+      Matrix23 H1_;
+      H1_ << -r_.c(),  r_.s(),  0.0,
               -r_.s(), -r_.c(),  0.0;
-      *H1 = H * mvalue;
+      *H1 = H * H1_;
   }
   if (H2) *H2 = H;
   return r;
 }
 
 /* ************************************************************************* */
-double Pose2::range(const Pose2& pose2,
-    boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H2) const {
-  Point2 d = pose2.t() - t_;
+double Pose2::range(const Pose2& pose,
+    OptionalJacobian<1,3> H1,
+    OptionalJacobian<1,3> H2) const {
+  Point2 d = pose.t() - t_;
   if (!H1 && !H2) return d.norm();
-  Matrix H;
+  Matrix12 H;
   double r = d.norm(H);
-  if (H1) *H1 = H * (Matrix(2, 3) <<
+  if (H1) {
+      Matrix23 H1_;
+      H1_ <<
       -r_.c(),  r_.s(),  0.0,
-      -r_.s(), -r_.c(),  0.0).finished();
-  if (H2) *H2 = H * (Matrix(2, 3) <<
-      pose2.r_.c(), -pose2.r_.s(),  0.0,
-      pose2.r_.s(),  pose2.r_.c(),  0.0).finished();
+      -r_.s(), -r_.c(),  0.0;
+      *H1 = H * H1_;
+  }
+  if (H2) {
+      Matrix23 H2_;
+      H2_ <<
+      pose.r_.c(), -pose.r_.s(),  0.0,
+      pose.r_.s(),  pose.r_.c(),  0.0;
+      *H2 = H * H2_;
+  }
   return r;
 }
 
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 2a9f91508..7e3c29826 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -75,7 +75,7 @@ public:
   Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {}
 
   /** Constructor from 3*3 matrix */
-  Pose2(const Matrix &T) :
+  Pose2(const Matrix &T) : // TODO : Change this to Optional Jacobian ??
     r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
     assert(T.rows() == 3 && T.cols() == 3);
   }
@@ -111,8 +111,8 @@ public:
 
   /// compose this transformation onto another (first *this and then p2)
   Pose2 compose(const Pose2& p2,
-      boost::optional<Matrix&> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none) const;
+      OptionalJacobian<3, 3> H1 = boost::none,
+      OptionalJacobian<3, 3> H2 = boost::none) const;
 
   /// compose syntactic sugar
   inline Pose2 operator*(const Pose2& p2) const {
@@ -168,11 +168,13 @@ public:
    *  v (vx,vy) = 2D velocity
    * @return xihat, 3*3 element of Lie algebra that can be exponentiated
    */
-  static inline Matrix wedge(double vx, double vy, double w) {
-    return (Matrix(3,3) <<
+  static inline Matrix3 wedge(double vx, double vy, double w) {
+     Matrix3 wedge_;
+     wedge_ <<
         0.,-w,  vx,
         w,  0., vy,
-        0., 0.,  0.).finished();
+        0., 0.,  0.;
+      return wedge_;
   }
 
   /// @}
@@ -218,7 +220,7 @@ public:
   inline const Rot2&   rotation() const { return r_; }
 
   //// return transformation matrix
-  Matrix matrix() const;
+  Matrix3 matrix() const;
 
   /**
    * Calculate bearing to a landmark
@@ -226,15 +228,15 @@ public:
    * @return 2D rotation \f$ \in SO(2) \f$
    */
   Rot2 bearing(const Point2& point,
-               boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
+               OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const;
 
   /**
    * Calculate bearing to another pose
    * @param point SO(2) location of other pose
    * @return 2D rotation \f$ \in SO(2) \f$
    */
-  Rot2 bearing(const Pose2& point,
-               boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
+  Rot2 bearing(const Pose2& pose,
+               OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const;
 
   /**
    * Calculate range to a landmark
@@ -251,8 +253,8 @@ public:
    * @return range (double)
    */
   double range(const Pose2& point,
-      boost::optional<Matrix&> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const;
+      OptionalJacobian<1, 3> H1=boost::none,
+      OptionalJacobian<1, 3> H2=boost::none) const;
 
   /// @}
   /// @name Advanced Interface
@@ -287,7 +289,7 @@ private:
 
 /** specialization for pose2 wedge function (generic template in Lie.h) */
 template <>
-inline Matrix wedge<Pose2>(const Vector& xi) {
+inline Matrix wedge<Pose2>(const Vector& xi) { // TODO : Convert to Optional Jacobian ?
   return Pose2::wedge(xi(0),xi(1),xi(2));
 }
 
diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp
index 01111aafe..f12d96899 100644
--- a/gtsam/geometry/tests/testPose2.cpp
+++ b/gtsam/geometry/tests/testPose2.cpp
@@ -44,7 +44,7 @@ TEST(Pose2, constructors) {
   Pose2 origin;
   assert_equal(pose,origin);
   Pose2 t(M_PI/2.0+0.018, Point2(1.015, 2.01));
-  EXPECT(assert_equal(t,Pose2(t.matrix())));
+  EXPECT(assert_equal(t,Pose2((Matrix)t.matrix())));
 }
 
 /* ************************************************************************* */