diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index e48f641e3..960373a62 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -49,7 +49,7 @@ typedef Eigen::Matrix<double,4,1> Matrix41;
 typedef Eigen::Matrix<double,5,1> Matrix51;
 typedef Eigen::Matrix<double,6,1> Matrix61;
 
-
+typedef Eigen::Matrix<double,1,1> Matrix1;
 typedef Eigen::Matrix2d Matrix2;
 typedef Eigen::Matrix3d Matrix3;
 typedef Eigen::Matrix4d Matrix4;
@@ -78,46 +78,129 @@ typedef Eigen::Block<const Matrix> ConstSubMatrix;
 
 
 // Identity Matrices
-static const int I_1x1 = 1;
-static const Matrix2 I_2x2 = Matrix2::Identity();
-static const Matrix3 I_3x3 = Matrix3::Identity();
-static const Matrix4 I_4x4 = Matrix4::Identity();
-static const Matrix5 I_5x5 = Matrix5::Identity();
-static const Matrix6 I_6x6 = Matrix6::Identity();
-
-// Negative Identity
-static const int _I_1x1 = -1;
-static const Matrix2 _I_2x2 = -I_2x2;
-static const Matrix3 _I_3x3 = -I_3x3;
-static const Matrix4 _I_4x4 = -I_4x4;
-static const Matrix5 _I_5x5 = -I_5x5;
-static const Matrix6 _I_6x6 = -I_6x6;
+static const Eigen::MatrixBase<Matrix1>::IdentityReturnType I_1x1 = Matrix1::Identity();
+static const Eigen::MatrixBase<Matrix2>::IdentityReturnType I_2x2 = Matrix2::Identity();
+static const Eigen::MatrixBase<Matrix3>::IdentityReturnType I_3x3 = Matrix3::Identity();
+static const Eigen::MatrixBase<Matrix4>::IdentityReturnType I_4x4 = Matrix4::Identity();
+static const Eigen::MatrixBase<Matrix5>::IdentityReturnType I_5x5 = Matrix5::Identity();
+static const Eigen::MatrixBase<Matrix6>::IdentityReturnType I_6x6 = Matrix6::Identity();
 
 // Zero matrices
-// TODO : Make these for rectangular sized matrices as well.
-static const int Z_1x1 = 0;
-static const Matrix2 Z_2x2 = Matrix2::Zero();
-static const Matrix3 Z_3x3 = Matrix3::Zero();
-static const Matrix4 Z_4x4 = Matrix4::Zero();
-static const Matrix5 Z_5x5 = Matrix5::Zero();
-static const Matrix6 Z_6x6 = Matrix6::Zero();
+static const Eigen::DenseBase<Matrix1>::ConstantReturnType Z_1x1 = Matrix1::Zero();
+static const Eigen::DenseBase<Matrix2>::ConstantReturnType Z_2x2 = Matrix2::Zero();
+static const Eigen::DenseBase<Matrix3>::ConstantReturnType Z_3x3 = Matrix3::Zero();
+static const Eigen::DenseBase<Matrix4>::ConstantReturnType Z_4x4 = Matrix4::Zero();
+static const Eigen::DenseBase<Matrix5>::ConstantReturnType Z_5x5 = Matrix5::Zero();
+static const Eigen::DenseBase<Matrix6>::ConstantReturnType Z_6x6 = Matrix6::Zero();
+
+static const Eigen::DenseBase<Matrix12>::ConstantReturnType Z_1x2 = Matrix12::Zero();
+static const Eigen::DenseBase<Matrix13>::ConstantReturnType Z_1x3 = Matrix13::Zero();
+static const Eigen::DenseBase<Matrix14>::ConstantReturnType Z_1x4 = Matrix14::Zero();
+static const Eigen::DenseBase<Matrix15>::ConstantReturnType Z_1x5 = Matrix15::Zero();
+static const Eigen::DenseBase<Matrix16>::ConstantReturnType Z_1x6 = Matrix16::Zero();
+
+
+static const Eigen::DenseBase<Matrix23>::ConstantReturnType Z_2x3 = Matrix23::Zero();
+static const Eigen::DenseBase<Matrix24>::ConstantReturnType Z_2x4 = Matrix24::Zero();
+static const Eigen::DenseBase<Matrix25>::ConstantReturnType Z_2x5 = Matrix25::Zero();
+static const Eigen::DenseBase<Matrix26>::ConstantReturnType Z_2x6 = Matrix26::Zero();
+static const Eigen::DenseBase<Matrix27>::ConstantReturnType Z_2x7 = Matrix27::Zero();
+static const Eigen::DenseBase<Matrix28>::ConstantReturnType Z_2x8 = Matrix28::Zero();
+static const Eigen::DenseBase<Matrix29>::ConstantReturnType Z_2x9 = Matrix29::Zero();
+
+static const Eigen::DenseBase<Matrix32>::ConstantReturnType Z_3x2 = Matrix32::Zero();
+static const Eigen::DenseBase<Matrix34>::ConstantReturnType Z_3x4 = Matrix34::Zero();
+static const Eigen::DenseBase<Matrix35>::ConstantReturnType Z_3x5 = Matrix35::Zero();
+static const Eigen::DenseBase<Matrix36>::ConstantReturnType Z_3x6 = Matrix36::Zero();
+static const Eigen::DenseBase<Matrix37>::ConstantReturnType Z_3x7 = Matrix37::Zero();
+static const Eigen::DenseBase<Matrix38>::ConstantReturnType Z_3x8 = Matrix38::Zero();
+static const Eigen::DenseBase<Matrix39>::ConstantReturnType Z_3x9 = Matrix39::Zero();
+
+static const Eigen::DenseBase<Matrix21>::ConstantReturnType Z_2x1 = Matrix21::Zero();
+static const Eigen::DenseBase<Matrix31>::ConstantReturnType Z_3x1 = Matrix31::Zero();
+static const Eigen::DenseBase<Matrix41>::ConstantReturnType Z_4x1 = Matrix41::Zero();
+static const Eigen::DenseBase<Matrix51>::ConstantReturnType Z_5x1 = Matrix51::Zero();
+static const Eigen::DenseBase<Matrix61>::ConstantReturnType Z_6x1 = Matrix61::Zero();
+
+
+
 
 // Ones matrices
 // TODO : Make these for rectangular sized matrices as well.
-static const int O_1x1 = 1;
-static const Matrix2 O_2x2 = Matrix2::Ones();
-static const Matrix3 O_3x3 = Matrix3::Ones();
-static const Matrix4 O_4x4 = Matrix4::Ones();
-static const Matrix5 O_5x5 = Matrix5::Ones();
-static const Matrix6 O_6x6 = Matrix6::Ones();
+static const Eigen::DenseBase<Matrix1>::ConstantReturnType O_1x1 = Matrix1::Ones();
+static const Eigen::DenseBase<Matrix2>::ConstantReturnType O_2x2 = Matrix2::Ones();
+static const Eigen::DenseBase<Matrix3>::ConstantReturnType O_3x3 = Matrix3::Ones();
+static const Eigen::DenseBase<Matrix4>::ConstantReturnType O_4x4 = Matrix4::Ones();
+static const Eigen::DenseBase<Matrix5>::ConstantReturnType O_5x5 = Matrix5::Ones();
+static const Eigen::DenseBase<Matrix6>::ConstantReturnType O_6x6 = Matrix6::Ones();
+
+static const Eigen::DenseBase<Matrix12>::ConstantReturnType O_1x2 = Matrix12::Ones();
+static const Eigen::DenseBase<Matrix13>::ConstantReturnType O_1x3 = Matrix13::Ones();
+static const Eigen::DenseBase<Matrix14>::ConstantReturnType O_1x4 = Matrix14::Ones();
+static const Eigen::DenseBase<Matrix15>::ConstantReturnType O_1x5 = Matrix15::Ones();
+static const Eigen::DenseBase<Matrix16>::ConstantReturnType O_1x6 = Matrix16::Ones();
+
+
+static const Eigen::DenseBase<Matrix23>::ConstantReturnType O_2x3 = Matrix23::Ones();
+static const Eigen::DenseBase<Matrix24>::ConstantReturnType O_2x4 = Matrix24::Ones();
+static const Eigen::DenseBase<Matrix25>::ConstantReturnType O_2x5 = Matrix25::Ones();
+static const Eigen::DenseBase<Matrix26>::ConstantReturnType O_2x6 = Matrix26::Ones();
+static const Eigen::DenseBase<Matrix27>::ConstantReturnType O_2x7 = Matrix27::Ones();
+static const Eigen::DenseBase<Matrix28>::ConstantReturnType O_2x8 = Matrix28::Ones();
+static const Eigen::DenseBase<Matrix29>::ConstantReturnType O_2x9 = Matrix29::Ones();
+
+static const Eigen::DenseBase<Matrix32>::ConstantReturnType O_3x2 = Matrix32::Ones();
+static const Eigen::DenseBase<Matrix34>::ConstantReturnType O_3x4 = Matrix34::Ones();
+static const Eigen::DenseBase<Matrix35>::ConstantReturnType O_3x5 = Matrix35::Ones();
+static const Eigen::DenseBase<Matrix36>::ConstantReturnType O_3x6 = Matrix36::Ones();
+static const Eigen::DenseBase<Matrix37>::ConstantReturnType O_3x7 = Matrix37::Ones();
+static const Eigen::DenseBase<Matrix38>::ConstantReturnType O_3x8 = Matrix38::Ones();
+static const Eigen::DenseBase<Matrix39>::ConstantReturnType O_3x9 = Matrix39::Ones();
+
+static const Eigen::DenseBase<Matrix21>::ConstantReturnType O_2x1 = Matrix21::Ones();
+static const Eigen::DenseBase<Matrix31>::ConstantReturnType O_3x1 = Matrix31::Ones();
+static const Eigen::DenseBase<Matrix41>::ConstantReturnType O_4x1 = Matrix41::Ones();
+static const Eigen::DenseBase<Matrix51>::ConstantReturnType O_5x1 = Matrix51::Ones();
+static const Eigen::DenseBase<Matrix61>::ConstantReturnType O_6x1 = Matrix61::Ones();
+
 
 // Negative Ones
-static const int _O_1x1 = -1;
-static const Matrix2 _O_2x2 = -O_2x2;
-static const Matrix3 _O_3x3 = -O_3x3;
-static const Matrix4 _O_4x4 = -O_4x4;
-static const Matrix5 _O_5x5 = -O_5x5;
-static const Matrix6 _O_6x6 = -O_6x6;
+static const Eigen::DenseBase<Matrix1>::ConstantReturnType _O_1x1 = Matrix1::Constant(-1);
+static const Eigen::DenseBase<Matrix2>::ConstantReturnType _O_2x2 = Matrix2::Constant(-1);
+static const Eigen::DenseBase<Matrix3>::ConstantReturnType _O_3x3 = Matrix3::Constant(-1);
+static const Eigen::DenseBase<Matrix4>::ConstantReturnType _O_4x4 = Matrix4::Constant(-1);
+static const Eigen::DenseBase<Matrix5>::ConstantReturnType _O_5x5 = Matrix5::Constant(-1);
+static const Eigen::DenseBase<Matrix6>::ConstantReturnType _O_6x6 = Matrix6::Constant(-1);
+
+static const Eigen::DenseBase<Matrix12>::ConstantReturnType _O_1x2 = Matrix12::Constant(-1);
+static const Eigen::DenseBase<Matrix13>::ConstantReturnType _O_1x3 = Matrix13::Constant(-1);
+static const Eigen::DenseBase<Matrix14>::ConstantReturnType _O_1x4 = Matrix14::Constant(-1);
+static const Eigen::DenseBase<Matrix15>::ConstantReturnType _O_1x5 = Matrix15::Constant(-1);
+static const Eigen::DenseBase<Matrix16>::ConstantReturnType _O_1x6 = Matrix16::Constant(-1);
+
+
+static const Eigen::DenseBase<Matrix23>::ConstantReturnType _O_2x3 = Matrix23::Constant(-1);
+static const Eigen::DenseBase<Matrix24>::ConstantReturnType _O_2x4 = Matrix24::Constant(-1);
+static const Eigen::DenseBase<Matrix25>::ConstantReturnType _O_2x5 = Matrix25::Constant(-1);
+static const Eigen::DenseBase<Matrix26>::ConstantReturnType _O_2x6 = Matrix26::Constant(-1);
+static const Eigen::DenseBase<Matrix27>::ConstantReturnType _O_2x7 = Matrix27::Constant(-1);
+static const Eigen::DenseBase<Matrix28>::ConstantReturnType _O_2x8 = Matrix28::Constant(-1);
+static const Eigen::DenseBase<Matrix29>::ConstantReturnType _O_2x9 = Matrix29::Constant(-1);
+
+static const Eigen::DenseBase<Matrix32>::ConstantReturnType _O_3x2 = Matrix32::Constant(-1);
+static const Eigen::DenseBase<Matrix34>::ConstantReturnType _O_3x4 = Matrix34::Constant(-1);
+static const Eigen::DenseBase<Matrix35>::ConstantReturnType _O_3x5 = Matrix35::Constant(-1);
+static const Eigen::DenseBase<Matrix36>::ConstantReturnType _O_3x6 = Matrix36::Constant(-1);
+static const Eigen::DenseBase<Matrix37>::ConstantReturnType _O_3x7 = Matrix37::Constant(-1);
+static const Eigen::DenseBase<Matrix38>::ConstantReturnType _O_3x8 = Matrix38::Constant(-1);
+static const Eigen::DenseBase<Matrix39>::ConstantReturnType _O_3x9 = Matrix39::Constant(-1);
+
+static const Eigen::DenseBase<Matrix21>::ConstantReturnType _O_2x1 = Matrix21::Constant(-1);
+static const Eigen::DenseBase<Matrix31>::ConstantReturnType _O_3x1 = Matrix31::Constant(-1);
+static const Eigen::DenseBase<Matrix41>::ConstantReturnType _O_4x1 = Matrix41::Constant(-1);
+static const Eigen::DenseBase<Matrix51>::ConstantReturnType _O_5x1 = Matrix51::Constant(-1);
+static const Eigen::DenseBase<Matrix61>::ConstantReturnType _O_6x1 = Matrix61::Constant(-1);
+
 
 
 // Matlab-like syntax
diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index d6a20b3db..ccd061d7c 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -141,8 +141,7 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
   Matrix2 Dp;
   uncalibrate(p, Dcal, Dp);
   Matrix25 H;
-  H.block<2,2>(0,0) = Dp;
-  H.block<2,3>(0,2) = Dcal;
+  H << Dp, Dcal;
   return H;
 }
 
diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp
index 8e6c219bc..5bdf89856 100644
--- a/gtsam/geometry/Cal3Unified.cpp
+++ b/gtsam/geometry/Cal3Unified.cpp
@@ -81,9 +81,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
     Vector2 DU;
     DU << -xs * sqrt_nx * xi_sqrt_nx2, //
         -ys * sqrt_nx * xi_sqrt_nx2;
-
-    H1->block<2,9>(0,0) = H1base;
-    H1->block<2,1>(0,9) = H2base * DU;
+    *H1 << H1base, H2base * DU;
   }
 
   // Inlined derivative for points
@@ -95,7 +93,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
     DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
         mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
 
-    *H2 = H2base * DU;
+    *H2 << H2base * DU;
   }
 
   return puncalib;
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index 61eafa283..3896ccb43 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -173,8 +173,8 @@ public:
   inline Cal3_S2 between(const Cal3_S2& q,
       OptionalJacobian<5,5> H1=boost::none,
       OptionalJacobian<5,5> H2=boost::none) const {
-    if(H1) *H1 = _I_5x5;
-    if(H2) *H2 = I_5x5;
+    if(H1) *H1 << -I_5x5;
+    if(H2) *H2 <<  I_5x5;
     return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
   }
 
diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp
index be4bba33f..5ea05e10d 100644
--- a/gtsam/geometry/EssentialMatrix.cpp
+++ b/gtsam/geometry/EssentialMatrix.cpp
@@ -27,10 +27,8 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
     // First get 2*3 derivative from Unit3::FromPoint3
     Matrix23 D_direction_1T2;
     Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
-    H->block<3, 3>(0, 0) << I_3x3; // upper left
-    H->block<2, 3>(3, 0).setZero(); // lower left
-    H->block<3, 3>(0, 3) << Z_3x3; // upper right
-    H->block<2, 3>(3, 3) = D_direction_1T2 * _1R2_.matrix(); // lower right
+    *H <<  I_3x3, Z_3x3, //
+           Z_2x3, D_direction_1T2 * _1R2_.matrix();
     return EssentialMatrix(_1R2_, direction);
   }
 }
@@ -69,9 +67,8 @@ Point3 EssentialMatrix::transform_to(const Point3& p,
     // 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
-    Matrix35 H;
-    H.block<3,3>(0,0) = DE_.block<3, 3>(0, 0);
-    H.block<3,2>(0,3)  = -aRb_.transpose() * aTb_.basis();
+    Matrix35 H; 
+    H << DE_.block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
     *DE = H;
   }
   return q;
@@ -93,11 +90,8 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
     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->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 (HE) *HE << cRb.matrix(), Z_3x2, //
+             Z_2x3, D_c1Tc2_aTb;
     if (HR) {
       throw runtime_error(
           "EssentialMatrix::rotate: derivative HR not implemented yet");
@@ -118,8 +112,7 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
     Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
     Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
         * aTb_.basis();
-    H->block<1,3>(0,0) = HR;
-    H->block<1,2>(0,3) = HD;
+    *H << HR, HD;
   }
   return dot(vA, E_ * vB);
 }
diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h
index 2491d92d1..8c8e03db4 100644
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@@ -139,7 +139,7 @@ public:
   inline Point2 between(const Point2& q,
       OptionalJacobian<2,2> H1=boost::none,
       OptionalJacobian<2,2> H2=boost::none) const {
-    if(H1) *H1 = _I_2x2;
+    if(H1) *H1 = -I_2x2;
     if(H2) *H2 = I_2x2;
     return q - (*this);
   }
diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp
index 152b5566b..0d84f32fe 100644
--- a/gtsam/geometry/Point3.cpp
+++ b/gtsam/geometry/Point3.cpp
@@ -66,9 +66,9 @@ Point3 Point3::operator/(double s) const {
 Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
     OptionalJacobian<3,3> H2) const {
   if (H1)
-    *H1= I_3x3;
+    *H1 << I_3x3;
   if (H2)
-    *H2= I_3x3;
+    *H2 << I_3x3;
   return *this + q;
 }
 
@@ -76,9 +76,9 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
 Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
     OptionalJacobian<3,3> H2) const {
   if (H1)
-    *H1 = I_3x3;
+    *H1 << I_3x3;
   if (H2)
-    *H2 = I_3x3;
+    *H2 << I_3x3;
   return *this - q;
 }
 
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 0cd038d90..24a3486ae 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -95,8 +95,8 @@ namespace gtsam {
     inline Point3 compose(const Point3& p2,
         OptionalJacobian<3,3> H1=boost::none,
         OptionalJacobian<3,3> H2=boost::none) const {
-      if (H1) H1->setIdentity();
-      if (H2) H2->setIdentity();
+      if (H1) *H1 << I_3x3;
+      if (H2) *H2 << I_3x3;
       return *this + p2;
     }
 
@@ -107,8 +107,8 @@ namespace gtsam {
     inline Point3 between(const Point3& p2,
         OptionalJacobian<3,3> H1=boost::none,
         OptionalJacobian<3,3> H2=boost::none) const {
-      if(H1) *H1 = _I_3x3;
-      if(H2) H2->setIdentity();
+      if(H1) *H1 << -I_3x3;
+      if(H2) *H2 << I_3x3;
       return p2 - *this;
     }
 
@@ -172,7 +172,7 @@ namespace gtsam {
 
       if (H2) {
         *H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z();
-        *H2 = *H2 *(1./d);
+        *H2 << *H2 *(1./d);
       }
       return d;
     }
diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index bf6f02e17..c882d7156 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -125,7 +125,7 @@ Matrix3 Pose2::AdjointMap() const {
 
 /* ************************************************************************* */
 Pose2 Pose2::inverse(OptionalJacobian<3,3> H1) const {
-  if (H1) *H1 = -AdjointMap();
+  if (H1) *H1 << -AdjointMap();
   return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
 }
 
@@ -139,7 +139,7 @@ Point2 Pose2::transform_to(const Point2& point,
   if (H1) *H1 <<
       -1.0, 0.0,  q.y(),
       0.0, -1.0, -q.x();
-  if (H2) *H2 = r_.transpose();
+  if (H2) *H2 << r_.transpose();
   return q;
 }
 
@@ -148,8 +148,8 @@ Point2 Pose2::transform_to(const Point2& point,
 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 = I_3x3;
+  if(H1) *H1 << p2.inverse().AdjointMap();
+  if(H2) *H2 << I_3x3;
   return (*this)*p2;
 }
 
@@ -162,11 +162,8 @@ Point2 Pose2::transform_from(const Point2& p,
     const Matrix2 R = r_.matrix();
     Matrix21 Drotate1;
     Drotate1 <<  -q.y(), q.x();
-    if (H1) {
-        H1->block<2,2>(0,0) = R; // [R R_{pi/2}q]
-        H1->block<2,1>(0,2) = Drotate1;
-    }
-    if (H2) *H2 = R; // R
+    if (H1) *H1 << R, Drotate1;
+    if (H2) *H2 << R; // R
   }
   return q + t_;
 }
@@ -200,7 +197,7 @@ Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1,
         s,  -c,  dt2,
         0.0, 0.0,-1.0;
   }
-  if (H2) *H2 = I_3x3;
+  if (H2) *H2 << I_3x3;
 
   return Pose2(R,t);
 }
@@ -214,8 +211,8 @@ Rot2 Pose2::bearing(const Point2& point,
   if (!H1 && !H2) return Rot2::relativeBearing(d);
   Matrix12 D_result_d;
   Rot2 result = Rot2::relativeBearing(d, D_result_d);
-  if (H1) *H1 = D_result_d * (D1);
-  if (H2) *H2 = D_result_d * (D2);
+  if (H1) *H1 << D_result_d * (D1);
+  if (H2) *H2 << D_result_d * (D2);
   return result;
 }
 
@@ -226,8 +223,7 @@ Rot2 Pose2::bearing(const Pose2& pose,
   Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0);
   if (H2) {
     Matrix12 H2_ = D2 * pose.r().matrix();
-    H2->setZero();
-    H2->block<1,2>(0,0) = H2_;
+    *H2 << H2_, Z_1x1;
   }
   return result;
 }
@@ -242,9 +238,9 @@ double Pose2::range(const Point2& point,
       Matrix23 H1_;
       H1_ << -r_.c(),  r_.s(),  0.0,
               -r_.s(), -r_.c(),  0.0;
-      *H1 = H * H1_;
+      *H1 << H * H1_;
   }
-  if (H2) *H2 = H;
+  if (H2) *H2 << H;
   return r;
 }
 
@@ -261,14 +257,14 @@ double Pose2::range(const Pose2& pose,
       H1_ <<
       -r_.c(),  r_.s(),  0.0,
       -r_.s(), -r_.c(),  0.0;
-      *H1 = H * H1_;
+      *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_;
+      *H2 << H * H2_;
   }
   return r;
 }
diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 35abf39a2..09ff633d2 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -274,17 +274,14 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
 /* ************************************************************************* */
 Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1,
     OptionalJacobian<6,6> H2) const {
-  if (H1)
-    *H1 = p2.inverse().AdjointMap();
-  if (H2)
-    *H2 = I_6x6;
+  if (H1) *H1 << p2.inverse().AdjointMap();
+  if (H2) *H2 << I_6x6;
   return (*this) * p2;
 }
 
 /* ************************************************************************* */
 Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const {
-  if (H1)
-    *H1 = -AdjointMap();
+  if (H1) *H1 << -AdjointMap();
   Rot3 Rt = R_.inverse();
   return Pose3(Rt, Rt * (-t_));
 }
@@ -294,10 +291,8 @@ Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const {
 Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1,
     OptionalJacobian<6,6> H2) const {
   Pose3 result = inverse() * p2;
-  if (H1)
-    *H1 = -result.inverse().AdjointMap();
-  if (H2)
-    *H2 = I_6x6;
+  if (H1) *H1 << -result.inverse().AdjointMap();
+  if (H2) *H2 << I_6x6;
   return result;
 }
 
@@ -313,10 +308,8 @@ double Pose3::range(const Point3& point, OptionalJacobian<1,6> H1,
       d2);
   Matrix13 D_result_d ;
   D_result_d << x / n, y / n, z / n;
-  if (H1)
-    *H1 = D_result_d * (D1);
-  if (H2)
-    *H2 = D_result_d * (D2);
+  if (H1) *H1 << D_result_d * D1;
+  if (H2) *H2 << D_result_d * D2;
   return n;
 }
 
@@ -327,8 +320,7 @@ double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1,
   double r = range(pose.translation(), H1, H2? &D2 : 0);
   if (H2) {
     Matrix13 H2_ = D2 * pose.rotation().matrix();
-    H2->setZero();
-    H2->block<1,3>(0,3) = H2_;
+    *H2 << Z_1x3, H2_;
   }
   return r;
 }
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index d396fe61f..6a7ed8456 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -70,6 +70,12 @@ public:
       R_(R), t_(t) {
   }
 
+  /** Construct from R,t, where t \in vector3 */
+  Pose3(const Rot3& R, const Vector3& t) :
+      R_(R), t_(t) {
+  }
+
+
   /** Construct from Pose2 */
   explicit Pose3(const Pose2& pose2);
 
diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index 324332c9e..33685bc7c 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -118,8 +118,8 @@ namespace gtsam {
     /** Compose - make a new rotation by adding angles */
     inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 =
         boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
-      if (H1) *H1 << I_1x1; // set to 1x1 identity matrix
-      if (H2) *H2 << I_1x1; // set to 1x1 identity matrix
+      if (H1) *H1 << 1; // set to 1x1 identity matrix
+      if (H2) *H2 << 1; // set to 1x1 identity matrix
       return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
     }
 
@@ -131,8 +131,8 @@ namespace gtsam {
     /** Between using the default implementation */
     inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 =
         boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
-      if (H1) *H1 << _I_1x1; // set to 1x1 identity matrix
-      if (H2) *H2 <<  I_1x1; // set to 1x1 identity matrix
+      if (H1) *H1 << -1; // set to 1x1 identity matrix
+      if (H2) *H2 <<  1; // set to 1x1 identity matrix
       return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
     }
 
diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp
index efc5d6f8b..6171d3347 100644
--- a/gtsam/geometry/Rot3M.cpp
+++ b/gtsam/geometry/Rot3M.cpp
@@ -141,10 +141,8 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) {
 
 /* ************************************************************************* */
 Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
-  if (H1)
-    *H1 = R2.transpose();
-  if (H2)
-    *H2 = I_3x3;
+  if (H1) *H1 << R2.transpose();
+  if (H2) *H2 << I_3x3;
   return *this * R2;
 }
 
@@ -161,15 +159,15 @@ Matrix3 Rot3::transpose() const {
 
 /* ************************************************************************* */
 Rot3 Rot3::inverse(boost::optional<Matrix3 &> H1) const {
-  if (H1) *H1 = -rot_;
+  if (H1) *H1 << -rot_;
   return Rot3(Matrix3(transpose()));
 }
 
 /* ************************************************************************* */
 Rot3 Rot3::between (const Rot3& R2,
     OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
-  if (H1) *H1 = -(R2.transpose()*rot_);
-  if (H2) *H2 = I_3x3;
+  if (H1) *H1 << -(R2.transpose()*rot_);
+  if (H2) *H2 << I_3x3;
   Matrix3 R12 = transpose()*R2.rot_;
   return Rot3(R12);
 }
@@ -178,8 +176,8 @@ Rot3 Rot3::between (const Rot3& R2,
 Point3 Rot3::rotate(const Point3& p,
     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_;
+      if (H1) *H1 << rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
+      if (H2) *H2 << rot_;
     }
   return Point3(rot_ * p.vector());
 }
diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp
index cab871874..d92c5bdd7 100644
--- a/gtsam/geometry/SimpleCamera.cpp
+++ b/gtsam/geometry/SimpleCamera.cpp
@@ -21,27 +21,27 @@
 
 namespace gtsam {
 
-  SimpleCamera simpleCamera(const Matrix& P) {
+  SimpleCamera simpleCamera(const Matrix34& P) {
 
     // P = [A|a] = s K cRw [I|-T], with s the unknown scale
-    Matrix A = P.topLeftCorner(3, 3);
-    Vector a = P.col(3);
+    Matrix3 A = P.topLeftCorner(3, 3);
+    Vector3 a = P.col(3);
 
     // do RQ decomposition to get s*K and cRw angles
-    Matrix sK;
-    Vector xyz;
+    Matrix3 sK;
+    Vector3 xyz;
     boost::tie(sK, xyz) = RQ(A);
 
     // Recover scale factor s and K
     double s = sK(2, 2);
-    Matrix K = sK / s;
+    Matrix3 K = sK / s;
 
     // Recover cRw itself, and its inverse
     Rot3 cRw = Rot3::RzRyRx(xyz);
     Rot3 wRc = cRw.inverse();
 
     // Now, recover T from a = - s K cRw T = - A T
-    Vector T = -(A.inverse() * a);
+    Vector3 T = -(A.inverse() * a);
     return SimpleCamera(Pose3(wRc, T),
         Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)));
   }
diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h
index 557654d2d..c6d33bcb3 100644
--- a/gtsam/geometry/SimpleCamera.h
+++ b/gtsam/geometry/SimpleCamera.h
@@ -27,5 +27,5 @@ namespace gtsam {
   typedef PinholeCamera<Cal3_S2> SimpleCamera;
 
   /// Recover camera from 3*4 camera matrix
-  GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix& P);
+  GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
 }
diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp
index b9e03c01d..f48c188aa 100644
--- a/gtsam/geometry/StereoCamera.cpp
+++ b/gtsam/geometry/StereoCamera.cpp
@@ -30,8 +30,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   StereoPoint2 StereoCamera::project(const Point3& point,
-      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
-      boost::optional<Matrix&> H3) const {
+      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
 
 #ifdef STEREOCAMERA_CHAIN_RULE
     const Point3 q = leftCamPose_.transform_to(point, H1, H2);
diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h
index 60ea7693d..dfefc4bec 100644
--- a/gtsam/geometry/StereoCamera.h
+++ b/gtsam/geometry/StereoCamera.h
@@ -120,8 +120,7 @@ public:
    */
   StereoPoint2 project(const Point3& point,
       boost::optional<Matrix&> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none,
-      boost::optional<Matrix&> H3 = boost::none) const;
+      boost::optional<Matrix&> H2 = boost::none) const;
 
   /**
    *
diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index 6c3f1a7e7..c89d2dacb 100644
--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@@ -49,7 +49,7 @@ TEST( Rot3, chart)
 /* ************************************************************************* */
 TEST( Rot3, constructor)
 {
-  Rot3 expected(I_3x3);
+  Rot3 expected((Matrix)I_3x3);
   Point3 r1(1,0,0), r2(0,1,0), r3(0,0,1);
   Rot3 actual(r1, r2, r3);
   CHECK(assert_equal(actual,expected));