diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp
index 88cbc4939..da246f23f 100644
--- a/gtsam/geometry/CalibratedCamera.cpp
+++ b/gtsam/geometry/CalibratedCamera.cpp
@@ -64,7 +64,7 @@ Point2 CalibratedCamera::project(const Point3& point,
 #ifdef CALIBRATEDCAMERA_CHAIN_RULE
   Matrix36 Dpose_;
   Matrix3 Dpoint_;
-  Point3 q = pose_.transform_to(point, Dpose_, Dpoint_);
+  Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0);
 #else
   Point3 q = pose_.transform_to(point);
 #endif
@@ -77,10 +77,12 @@ Point2 CalibratedCamera::project(const Point3& point,
   if (Dpose || Dpoint) {
 #ifdef CALIBRATEDCAMERA_CHAIN_RULE
     // just implement chain rule
-    Matrix23 H;
-    project_to_camera(q,H);
-    if (Dpose) *Dpose = H * (*Dpose_);
-    if (Dpoint) *Dpoint = H * (*Dpoint_);
+    if(Dpose && Dpoint) {
+      Matrix23 H;
+      project_to_camera(q,H);
+      if (Dpose) *Dpose = H * (*Dpose_);
+      if (Dpoint) *Dpoint = H * (*Dpoint_);
+    }
 #else
     // optimized version, see CalibratedCamera.nb
     const double z = q.z(), d = 1.0 / z;
diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp
index e65e5d097..aa1dee352 100644
--- a/gtsam/geometry/EssentialMatrix.cpp
+++ b/gtsam/geometry/EssentialMatrix.cpp
@@ -14,7 +14,7 @@ namespace gtsam {
 
 /* ************************************************************************* */
 EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
-    boost::optional<Matrix&> H) {
+    OptionalJacobian<5,6> H) {
   const Rot3& _1R2_ = _1P2_.rotation();
   const Point3& _1T2_ = _1P2_.translation();
   if (!H) {
@@ -25,13 +25,12 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
     // Calculate the 5*6 Jacobian H = D_E_1P2
     // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation
     // First get 2*3 derivative from Unit3::FromPoint3
-    Matrix D_direction_1T2;
+    Matrix23 D_direction_1T2;
     Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
-    H->resize(5, 6);
-    H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left
-    H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left
-    H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right
-    H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right
+    H->block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); // upper left
+    H->block<2, 3>(3, 0).setZero(); // lower left
+    H->block<3, 3>(0, 3).setZero(); // upper right
+    H->block<2, 3>(3, 3) = D_direction_1T2 * _1R2_.matrix(); // lower right
     return EssentialMatrix(_1R2_, direction);
   }
 }
@@ -61,16 +60,18 @@ Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
 
 /* ************************************************************************* */
 Point3 EssentialMatrix::transform_to(const Point3& p,
-    boost::optional<Matrix&> DE, boost::optional<Matrix&> Dpoint) const {
+    OptionalJacobian<3,5> DE, OptionalJacobian<3,3> Dpoint) const {
   Pose3 pose(aRb_, aTb_.point3());
-  Point3 q = pose.transform_to(p, DE, Dpoint);
+  Matrix36 DE_;
+  Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint);
   if (DE) {
     // DE returned by pose.transform_to is 3*6, but we need it to be 3*5
     // The last 3 columns are derivative with respect to change in translation
     // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
     // Duy made an educated guess that this needs to be rotated to the local frame
-    Matrix H(3, 5);
-    H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
+    Matrix35 H;
+    H.block<3,3>(0,0) = DE_.block<3, 3>(0, 0);
+    H.block<3,2>(0,3)  = -aRb_.transpose() * aTb_.basis();
     *DE = H;
   }
   return q;
@@ -78,7 +79,7 @@ Point3 EssentialMatrix::transform_to(const Point3& p,
 
 /* ************************************************************************* */
 EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
-    boost::optional<Matrix&> HE, boost::optional<Matrix&> HR) const {
+    OptionalJacobian<5,5> HE, OptionalJacobian<5,3> HR) const {
 
   // The rotation must be conjugated to act in the camera frame
   Rot3 c1Rc2 = aRb_.conjugate(cRb);
@@ -89,18 +90,18 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
     return EssentialMatrix(c1Rc2, c1Tc2);
   } else {
     // Calculate derivatives
-    Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2
+    Matrix23 D_c1Tc2_cRb; // 2*3
+    Matrix2 D_c1Tc2_aTb;  // 2*2
     Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
     if (HE) {
-      *HE = zeros(5, 5);
-      HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2
-      HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2)
+      HE->setZero();
+      HE->block<3, 3>(0, 0) = cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2
+      HE->block<2, 2>(3, 3) = D_c1Tc2_aTb; // (2*2)
     }
     if (HR) {
       throw runtime_error(
           "EssentialMatrix::rotate: derivative HR not implemented yet");
       /*
-       HR->resize(5, 3);
        HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ?
        HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ?
        */
@@ -110,15 +111,15 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
 }
 
 /* ************************************************************************* */
-double EssentialMatrix::error(const Vector& vA, const Vector& vB, //
-    boost::optional<Matrix&> H) const {
+double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
+    OptionalJacobian<1,5> H) const {
   if (H) {
-    H->resize(1, 5);
     // See math.lyx
-    Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
-    Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
+    Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
+    Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
         * aTb_.basis();
-    *H << HR, HD;
+    H->block<1,3>(0,0) = HR;
+    H->block<1,2>(0,3) = HD;
   }
   return dot(vA, E_ * vB);
 }
diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h
index fd5f45160..b25286412 100644
--- a/gtsam/geometry/EssentialMatrix.h
+++ b/gtsam/geometry/EssentialMatrix.h
@@ -50,7 +50,7 @@ public:
 
   /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
   static EssentialMatrix FromPose3(const Pose3& _1P2_,
-      boost::optional<Matrix&> H = boost::none);
+      OptionalJacobian<5, 6> H = boost::none);
 
   /// Random, using Rot3::Random and Unit3::Random
   template<typename Engine>
@@ -132,16 +132,16 @@ public:
    * @return point in pose coordinates
    */
   Point3 transform_to(const Point3& p,
-      boost::optional<Matrix&> DE = boost::none,
-      boost::optional<Matrix&> Dpoint = boost::none) const;
+      OptionalJacobian<3,5> DE = boost::none,
+      OptionalJacobian<3,3> Dpoint = boost::none) const;
 
   /**
    * Given essential matrix E in camera frame B, convert to body frame C
    * @param cRb rotation from body frame to camera frame
    * @param E essential matrix E in camera frame C
    */
-  EssentialMatrix rotate(const Rot3& cRb, boost::optional<Matrix&> HE =
-      boost::none, boost::optional<Matrix&> HR = boost::none) const;
+  EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE =
+      boost::none, OptionalJacobian<5, 3> HR = boost::none) const;
 
   /**
    * Given essential matrix E in camera frame B, convert to body frame C
@@ -153,8 +153,8 @@ public:
   }
 
   /// epipolar error, algebraic
-  double error(const Vector& vA, const Vector& vB, //
-      boost::optional<Matrix&> H = boost::none) const;
+  double error(const Vector3& vA, const Vector3& vB, //
+      OptionalJacobian<1,5> H = boost::none) const;
 
   /// @}
 
diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp
index daff6b00b..137d96a06 100644
--- a/gtsam/geometry/Unit3.cpp
+++ b/gtsam/geometry/Unit3.cpp
@@ -39,14 +39,13 @@ using namespace std;
 namespace gtsam {
 
 /* ************************************************************************* */
-Unit3 Unit3::FromPoint3(const Point3& point, boost::optional<Matrix&> H) {
+Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
   Unit3 direction(point);
   if (H) {
     // 3*3 Derivative of representation with respect to point is 3*3:
     Matrix D_p_point;
     point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
     // Calculate the 2*3 Jacobian
-    H->resize(2, 3);
     *H << direction.basis().transpose() * D_p_point;
   }
   return direction;
diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h
index a906302af..9b79dcd18 100644
--- a/gtsam/geometry/Unit3.h
+++ b/gtsam/geometry/Unit3.h
@@ -57,7 +57,7 @@ public:
   }
 
   /// Named constructor from Point3 with optional Jacobian
-  static Unit3 FromPoint3(const Point3& point, boost::optional<Matrix&> H =
+  static Unit3 FromPoint3(const Point3& point, OptionalJacobian<2,3> H =
       boost::none);
 
   /// Random direction, using boost::uniform_on_sphere