diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 2e3437573..1eb4c8db5 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -83,12 +83,12 @@ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
 Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
     OptionalJacobian<6,6> H) {
   if (H) {
-    (*H).setZero();
+    H->setZero();
     for (int i = 0; i < 6; ++i) {
       Vector6 dxi;
       dxi.setZero();
       dxi(i) = 1.0;
-      Matrix GTi = adjointMap(dxi).transpose();
+      Matrix6 GTi = adjointMap(dxi).transpose();
       (*H).col(i) = GTi * y;
     }
   }
@@ -360,13 +360,13 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
 }
 
 // Compute SVD
-  Matrix U, V;
+  Matrix U,V;
   Vector S;
   svd(H, U, S, V);
 
   // Recover transform with correction from Eggert97machinevisionandapplications
-  Matrix UVtranspose = U * V.transpose();
-  Matrix detWeighting = eye(3, 3);
+  Matrix3 UVtranspose = U * V.transpose();
+  Matrix3 detWeighting = Eigen::Matrix3d::Identity();
   detWeighting(2, 2) = UVtranspose.determinant();
   Rot3 R(Matrix(V * detWeighting * U.transpose()));
   Point3 t = Point3(cq) - R * Point3(cp);
diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index 846e0e070..db82c6f1e 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -72,10 +72,11 @@ Point3 Rot3::operator*(const Point3& p) const {
 
 /* ************************************************************************* */
 Unit3 Rot3::rotate(const Unit3& p,
-    boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
-  Unit3 q = Unit3(rotate(p.point3(Hp)));
+    OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
+  Matrix32 Dp;
+  Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0)));
   if (Hp)
-    (*Hp) = q.basis().transpose() * matrix() * (*Hp);
+    (*Hp) = q.basis().transpose() * matrix() * Dp;
   if (HR)
     (*HR) = -q.basis().transpose() * matrix() * p.skew();
   return q;
@@ -83,10 +84,11 @@ Unit3 Rot3::rotate(const Unit3& p,
 
 /* ************************************************************************* */
 Unit3 Rot3::unrotate(const Unit3& p,
-    boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
-  Unit3 q = Unit3(unrotate(p.point3(Hp)));
+    OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
+  Matrix32 Dp;
+  Unit3 q = Unit3(unrotate(p.point3(Dp)));
   if (Hp)
-    (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp);
+    (*Hp) = q.basis().transpose() * matrix().transpose () * Dp;
   if (HR)
     (*HR) = q.basis().transpose() * q.skew();
   return q;
@@ -99,8 +101,8 @@ Unit3 Rot3::operator*(const Unit3& p) const {
 
 /* ************************************************************************* */
 // see doc/math.lyx, SO(3) section
-Point3 Rot3::unrotate(const Point3& p, boost::optional<Matrix3&> H1,
-    boost::optional<Matrix3&> H2) const {
+Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
+    OptionalJacobian<3,3> H2) const {
   const Matrix3& Rt = transpose();
   Point3 q(Rt * p.vector()); // q = Rt*p
   const double wx = q.x(), wy = q.y(), wz = q.z();
@@ -111,32 +113,16 @@ Point3 Rot3::unrotate(const Point3& p, boost::optional<Matrix3&> H1,
   return q;
 }
 
-/* ************************************************************************* */
-// see doc/math.lyx, SO(3) section
-Point3 Rot3::unrotate(const Point3& p,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-  const Matrix3& Rt = transpose();
-  Point3 q(Rt * p.vector()); // q = Rt*p
-  const double wx = q.x(), wy = q.y(), wz = q.z();
-  if (H1) {
-    H1->resize(3,3);
-    *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
-  }
-  if (H2)
-    *H2 = Rt;
-  return q;
-}
-
 /* ************************************************************************* */
 /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
 Matrix3 Rot3::dexpL(const Vector3& v) {
   if(zero(v)) return eye(3);
-  Matrix x = skewSymmetric(v);
-  Matrix x2 = x*x;
+  Matrix3 x = skewSymmetric(v);
+  Matrix3 x2 = x*x;
   double theta = v.norm(), vi = theta/2.0;
   double s1 = sin(vi)/vi;
   double s2 = (theta - sin(theta))/(theta*theta*theta);
-  Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
+  Matrix3 res = I3 - 0.5*s1*s1*x + s2*x2;
   return res;
 }
 
@@ -144,11 +130,11 @@ Matrix3 Rot3::dexpL(const Vector3& v) {
 /// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
 Matrix3 Rot3::dexpInvL(const Vector3& v) {
   if(zero(v)) return eye(3);
-  Matrix x = skewSymmetric(v);
-  Matrix x2 = x*x;
+  Matrix3 x = skewSymmetric(v);
+  Matrix3 x2 = x*x;
   double theta = v.norm(), vi = theta/2.0;
   double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
-  Matrix res = eye(3) + 0.5*x - s2*x2;
+  Matrix3 res = I3 + 0.5*x - s2*x2;
   return res;
 }
 
@@ -167,7 +153,7 @@ Point3 Rot3::column(int index) const{
 
 /* ************************************************************************* */
 Vector3 Rot3::xyz() const {
-  Matrix I;Vector3 q;
+  Matrix3 I;Vector3 q;
   boost::tie(I,q)=RQ(matrix());
   return q;
 }
@@ -255,12 +241,6 @@ ostream &operator<<(ostream &os, const Rot3& R) {
   return os;
 }
 
-/* ************************************************************************* */
-Point3 Rot3::unrotate(const Point3& p) const {
-  // Eigen expression
-  return Point3(transpose()*p.vector()); // q = Rt*p
-}
-
 /* ************************************************************************* */
 Rot3 Rot3::slerp(double t, const Rot3& other) const {
   // amazingly simple in GTSAM :-)
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 4364678a5..edd39619f 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -215,7 +215,7 @@ namespace gtsam {
     }
 
     /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
-    Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const;
+    Rot3 inverse(boost::optional<Matrix3&> H1=boost::none) const;
 
     /// Compose two rotations i.e., R= (*this) * R2
     Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none,
@@ -238,8 +238,8 @@ namespace gtsam {
      * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
      */
     Rot3 between(const Rot3& R2,
-        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;
 
     /// @}
     /// @name Manifold
@@ -321,34 +321,27 @@ namespace gtsam {
     /**
      * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$
      */
-    Point3 rotate(const Point3& p, boost::optional<Matrix&> H1 = boost::none,
-        boost::optional<Matrix&> H2 = boost::none) const;
+    Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
+        OptionalJacobian<3,3> H2 = boost::none) const;
 
     /// rotate point from rotated coordinate frame to world = R*p
     Point3 operator*(const Point3& p) const;
 
     /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
-    Point3 unrotate(const Point3& p) const;
-
-    /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
-    Point3 unrotate(const Point3& p, boost::optional<Matrix3&> H1,
-        boost::optional<Matrix3&> H2) const;
-
-    /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
-    Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1,
-        boost::optional<Matrix&> H2) const;
+    Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
+        OptionalJacobian<3,3> H2=boost::none) const;
 
     /// @}
     /// @name Group Action on Unit3
     /// @{
 
     /// rotate 3D direction from rotated coordinate frame to world frame
-    Unit3 rotate(const Unit3& p, boost::optional<Matrix&> HR = boost::none,
-        boost::optional<Matrix&> Hp = boost::none) const;
+    Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
+        OptionalJacobian<2,2> Hp = boost::none) const;
 
     /// unrotate 3D direction from world frame to rotated coordinate frame
-    Unit3 unrotate(const Unit3& p, boost::optional<Matrix&> HR = boost::none,
-        boost::optional<Matrix&> Hp = boost::none) const;
+    Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
+        OptionalJacobian<2,2> Hp = boost::none) const;
 
     /// rotate 3D direction from rotated coordinate frame to world frame
     Unit3 operator*(const Unit3& p) const;
diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp
index 39648ca1e..d657c7287 100644
--- a/gtsam/geometry/Rot3M.cpp
+++ b/gtsam/geometry/Rot3M.cpp
@@ -162,14 +162,14 @@ Matrix3 Rot3::transpose() const {
 }
 
 /* ************************************************************************* */
-Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
+Rot3 Rot3::inverse(boost::optional<Matrix3 &> H1) const {
   if (H1) *H1 = -rot_;
   return Rot3(Matrix3(transpose()));
 }
 
 /* ************************************************************************* */
 Rot3 Rot3::between (const Rot3& R2,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+    OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
   if (H1) *H1 = -(R2.transpose()*rot_);
   if (H2) *H2 = I3;
   Matrix3 R12 = transpose()*R2.rot_;
@@ -178,7 +178,7 @@ Rot3 Rot3::between (const Rot3& R2,
 
 /* ************************************************************************* */
 Point3 Rot3::rotate(const Point3& p,
-    boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2) const {
+    OptionalJacobian<3,3> H1,  OptionalJacobian<3,3> H2) const {
   if (H1 || H2) {
       if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
       if (H2) *H2 = rot_;
@@ -245,7 +245,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
   } else if(mode == Rot3::CAYLEY) {
     return retractCayley(omega);
   } else if(mode == Rot3::SLOW_CAYLEY) {
-    Matrix Omega = skewSymmetric(omega);
+    Matrix3 Omega = skewSymmetric(omega);
     return (*this)*CayleyFixed<3>(-Omega/2);
   } else {
     assert(false);
diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp
index 5e13129cc..daff6b00b 100644
--- a/gtsam/geometry/Unit3.cpp
+++ b/gtsam/geometry/Unit3.cpp
@@ -67,7 +67,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
 }
 
 /* ************************************************************************* */
-const Unit3::Matrix32& Unit3::basis() const {
+const Matrix32& Unit3::basis() const {
 
   // Return cached version if exists
   if (B_)
@@ -92,7 +92,7 @@ const Unit3::Matrix32& Unit3::basis() const {
   b2 = b2 / b2.norm();
 
   // Create the basis matrix
-  B_.reset(Unit3::Matrix32());
+  B_.reset(Matrix32());
   (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
   return *B_;
 }
@@ -104,7 +104,7 @@ void Unit3::print(const std::string& s) const {
 }
 
 /* ************************************************************************* */
-Matrix Unit3::skew() const {
+Matrix3 Unit3::skew() const {
   return skewSymmetric(p_.x(), p_.y(), p_.z());
 }
 
diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h
index d04e57d4b..a906302af 100644
--- a/gtsam/geometry/Unit3.h
+++ b/gtsam/geometry/Unit3.h
@@ -32,8 +32,6 @@ class GTSAM_EXPORT Unit3{
 
 private:
 
-  typedef Eigen::Matrix<double,3,2> Matrix32;
-
   Point3 p_; ///< The location of the point on the unit sphere
   mutable boost::optional<Matrix32> B_; ///< Cached basis
 
@@ -90,10 +88,10 @@ public:
   const Matrix32& basis() const;
 
   /// Return skew-symmetric associated with 3D point on unit sphere
-  Matrix skew() const;
+  Matrix3 skew() const;
 
   /// Return unit-norm Point3
-  const Point3& point3(boost::optional<Matrix&> H = boost::none) const {
+  const Point3& point3(OptionalJacobian<3,2> H = boost::none) const {
     if (H)
       *H = basis();
     return p_;
diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index 88accb90f..62edd7ea7 100644
--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@@ -359,7 +359,7 @@ TEST( Rot3, inverse )
   Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
 
   Rot3 I;
-  Matrix actualH;
+  Matrix3 actualH;
   Rot3 actual = R.inverse(actualH);
   CHECK(assert_equal(I,R*actual));
   CHECK(assert_equal(I,actual*R));