diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp
index 2048a84c8..f1e1a0010 100644
--- a/examples/CameraResectioning.cpp
+++ b/examples/CameraResectioning.cpp
@@ -48,7 +48,7 @@ public:
   virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
       boost::none) const {
     SimpleCamera camera(pose, *K_);
-    Point2 reprojectionError(camera.project(P_, H) - p_);
+    Point2 reprojectionError(camera.project(P_, H, boost::none, boost::none) - p_);
     return reprojectionError.vector();
   }
 };
diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index 16884f4c1..f3d5be9e8 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -37,10 +37,28 @@ namespace gtsam {
 typedef Eigen::MatrixXd Matrix;
 typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
 
+typedef Eigen::Matrix2d Matrix2;
 typedef Eigen::Matrix3d Matrix3;
 typedef Eigen::Matrix4d Matrix4;
+typedef Eigen::Matrix<double,5,5> Matrix5;
 typedef Eigen::Matrix<double,6,6> Matrix6;
 
+typedef Eigen::Matrix<double,2,3> Matrix23;
+typedef Eigen::Matrix<double,2,4> Matrix24;
+typedef Eigen::Matrix<double,2,5> Matrix25;
+typedef Eigen::Matrix<double,2,6> Matrix26;
+typedef Eigen::Matrix<double,2,7> Matrix27;
+typedef Eigen::Matrix<double,2,8> Matrix28;
+typedef Eigen::Matrix<double,2,9> Matrix29;
+
+typedef Eigen::Matrix<double,3,2> Matrix32;
+typedef Eigen::Matrix<double,3,4> Matrix34;
+typedef Eigen::Matrix<double,3,5> Matrix35;
+typedef Eigen::Matrix<double,3,6> Matrix36;
+typedef Eigen::Matrix<double,3,7> Matrix37;
+typedef Eigen::Matrix<double,3,8> Matrix38;
+typedef Eigen::Matrix<double,3,9> Matrix39;
+
 // Matrix expressions for accessing parts of matrices
 typedef Eigen::Block<Matrix> SubMatrix;
 typedef Eigen::Block<const Matrix> ConstSubMatrix;
diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h
index b35afccdb..bf7d1733a 100644
--- a/gtsam/base/Vector.h
+++ b/gtsam/base/Vector.h
@@ -36,7 +36,12 @@ typedef Eigen::VectorXd Vector;
 // Commonly used fixed size vectors
 typedef Eigen::Vector2d Vector2;
 typedef Eigen::Vector3d Vector3;
+typedef Eigen::Matrix<double, 4, 1> Vector4;
+typedef Eigen::Matrix<double, 5, 1> Vector5;
 typedef Eigen::Matrix<double, 6, 1> Vector6;
+typedef Eigen::Matrix<double, 7, 1> Vector7;
+typedef Eigen::Matrix<double, 8, 1> Vector8;
+typedef Eigen::Matrix<double, 9, 1> Vector9;
 
 typedef Eigen::VectorBlock<Vector> SubVector;
 typedef Eigen::VectorBlock<const Vector> ConstSubVector;
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index fff9a6e6d..e508710cd 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -36,6 +36,8 @@ private:
   double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
 
 public:
+  /// dimension of the variable - used to autodetect sizes
+  static const size_t dimension = 3;
 
   /// @name Standard Constructors
   /// @{
diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp
index c75eff022..fe2acaf29 100644
--- a/gtsam/geometry/Cal3DS2.cpp
+++ b/gtsam/geometry/Cal3DS2.cpp
@@ -53,23 +53,23 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
 }
 
 /* ************************************************************************* */
-static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx,
+static Matrix29 D2dcalibration(double x, double y, double xx,
     double yy, double xy, double rr, double r4, double pnx, double pny,
-    const Eigen::Matrix<double, 2, 2>& DK) {
-  Eigen::Matrix<double, 2, 5> DR1;
+    const Matrix2& DK) {
+  Matrix25 DR1;
   DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
-  Eigen::Matrix<double, 2, 4> DR2;
+  Matrix24 DR2;
   DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
          y * rr, y * r4, rr + 2 * yy, 2 * xy;
-  Eigen::Matrix<double, 2, 9> D;
+  Matrix29 D;
   D << DR1, DK * DR2;
   return D;
 }
 
 /* ************************************************************************* */
-static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
+static Matrix2 D2dintrinsic(double x, double y, double rr,
     double g, double k1, double k2, double p1, double p2,
-    const Eigen::Matrix<double, 2, 2>& DK) {
+    const Matrix2& DK) {
   const double drdx = 2. * x;
   const double drdy = 2. * y;
   const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
@@ -82,7 +82,7 @@ static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
   const double dDydx = 2. * p2 * y + p1 * drdx;
   const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
 
-  Eigen::Matrix<double, 2, 2> DR;
+  Matrix2 DR;
   DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
         y * dgdx + dDydx, g + y * dgdy + dDydy;
 
@@ -110,7 +110,7 @@ Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
   const double pnx = g * x + dx;
   const double pny = g * y + dy;
 
-  Eigen::Matrix<double, 2, 2> DK;
+  Matrix2 DK;
   if (H1 || H2) DK << fx_, s_, 0.0, fy_;
 
   // Derivative for calibration
@@ -161,7 +161,7 @@ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
   const double rr = xx + yy;
   const double r4 = rr * rr;
   const double g = (1 + k1_ * rr + k2_ * r4);
-  Eigen::Matrix<double, 2, 2> DK;
+  Matrix2 DK;
   DK << fx_, s_, 0.0, fy_;
   return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
 }
@@ -176,7 +176,7 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
   const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
   const double pnx = g * x + dx;
   const double pny = g * y + dy;
-  Eigen::Matrix<double, 2, 2> DK;
+  Matrix2 DK;
   DK << fx_, s_, 0.0, fy_;
   return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
 }
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index 51fe958d6..82bfa4c5f 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -46,6 +46,9 @@ protected:
   double p1_, p2_ ; // tangential distortion
 
 public:
+  /// dimension of the variable - used to autodetect sizes
+  static const size_t dimension = 9;
+
   Matrix K() const ;
   Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
   Vector vector() const ;
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index 58e024c27..eacbf7053 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -50,8 +50,9 @@ private:
   double xi_;  // mirror parameter
 
 public:
-  //Matrix K() const ;
-  //Eigen::Vector4d k() const { return Base::k(); }
+  /// dimension of the variable - used to autodetect sizes
+  static const size_t dimension = 10;
+
   Vector vector() const ;
 
   /// @name Standard Constructors
diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp
index c82b36a83..aece1ded1 100644
--- a/gtsam/geometry/Cal3_S2.cpp
+++ b/gtsam/geometry/Cal3_S2.cpp
@@ -86,6 +86,21 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
   return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
 }
 
+/* ************************************************************************* */
+Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
+    boost::optional<Matrix2&> Dp) const {
+  const double x = p.x(), y = p.y();
+  if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
+  if (Dp) *Dp << fx_, s_, 0.0, fy_;
+  return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
+}
+
+/* ************************************************************************* */
+Point2 Cal3_S2::uncalibrate(const Point2& p) const {
+  const double x = p.x(), y = p.y();
+  return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
+}
+
 /* ************************************************************************* */
 Point2 Cal3_S2::calibrate(const Point2& p) const {
   const double u = p.x(), v = p.y();
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index 03c6bff3f..4e17c64f4 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -36,6 +36,8 @@ private:
   double fx_, fy_, s_, u0_, v0_;
 
 public:
+  /// dimension of the variable - used to autodetect sizes
+  static const size_t dimension = 5;
 
   typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
 
@@ -144,12 +146,29 @@ public:
   /**
    * convert intrinsic coordinates xy to image coordinates uv
    * @param p point in intrinsic coordinates
+   * @return point in image coordinates
+   */
+  Point2 uncalibrate(const Point2& p) const;
+
+  /**
+   * convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
+   * @param p point in intrinsic coordinates
    * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in image coordinates
    */
-  Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal =
-      boost::none, boost::optional<Matrix&> Dp = boost::none) const;
+  Point2 uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
+      boost::optional<Matrix2&> Dp) const;
+
+  /**
+   * convert intrinsic coordinates xy to image coordinates uv, dynamic derivaitves
+   * @param p point in intrinsic coordinates
+   * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in image coordinates
+   */
+  Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
+      boost::optional<Matrix&> Dp) const;
 
   /**
    * convert image coordinates uv to intrinsic coordinates xy
@@ -181,12 +200,12 @@ public:
 
   /// return DOF, dimensionality of tangent space
   inline size_t dim() const {
-    return 5;
+    return dimension;
   }
 
   /// return DOF, dimensionality of tangent space
   static size_t Dim() {
-    return 5;
+    return dimension;
   }
 
   /// Given 5-dim tangent vector, create new calibration
diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index 4f81750a5..86e6a9097 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -270,17 +270,15 @@ public:
    * @param Dpoint is the 2*3 Jacobian w.r.t. P
    */
   inline static Point2 project_to_camera(const Point3& P,
-      boost::optional<Matrix&> Dpoint = boost::none) {
+      boost::optional<Matrix23&> Dpoint = boost::none) {
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
     if (P.z() <= 0)
       throw CheiralityException();
 #endif
     double d = 1.0 / P.z();
     const double u = P.x() * d, v = P.y() * d;
-    if (Dpoint) {
-      Dpoint->resize(2,3);
+    if (Dpoint)
       *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
-    }
     return Point2(u, v);
   }
 
@@ -291,6 +289,22 @@ public:
     return std::make_pair(K_.uncalibrate(pn), pc.z() > 0);
   }
 
+  /** project a point from world coordinate to the image
+   *  @param pw is a point in world coordinates
+   */
+  inline Point2 project(const Point3& pw) const {
+
+    // Transform to camera coordinates and check cheirality
+    const Point3 pc = pose_.transform_to(pw);
+
+    // Project to normalized image coordinates
+    const Point2 pn = project_to_camera(pc);
+
+    return K_.uncalibrate(pn);
+  }
+
+  typedef Eigen::Matrix<double,2,Calibration::dimension> Matrix2K;
+
   /** project a point from world coordinate to the image
    *  @param pw is a point in world coordinates
    *  @param Dpose is the Jacobian w.r.t. pose3
@@ -299,9 +313,44 @@ public:
    */
   inline Point2 project(
       const Point3& pw, //
-      boost::optional<Matrix&> Dpose = boost::none,
-      boost::optional<Matrix&> Dpoint = boost::none,
-      boost::optional<Matrix&> Dcal = boost::none) const {
+      boost::optional<Matrix26&> Dpose,
+      boost::optional<Matrix23&> Dpoint,
+      boost::optional<Matrix2K&> Dcal) const {
+
+    // Transform to camera coordinates and check cheirality
+    const Point3 pc = pose_.transform_to(pw);
+
+    // Project to normalized image coordinates
+    const Point2 pn = project_to_camera(pc);
+
+    if (Dpose || Dpoint) {
+      const double z = pc.z(), d = 1.0 / z;
+
+      // uncalibration
+      Matrix2 Dpi_pn;
+      const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
+
+      // chain the Jacobian matrices
+      if (Dpose)
+        calculateDpose(pn, d, Dpi_pn, *Dpose);
+      if (Dpoint)
+        calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
+      return pi;
+    } else
+      return K_.uncalibrate(pn, Dcal, boost::none);
+  }
+
+  /** project a point from world coordinate to the image
+   *  @param pw is a point in world coordinates
+   *  @param Dpose is the Jacobian w.r.t. pose3
+   *  @param Dpoint is the Jacobian w.r.t. point3
+   *  @param Dcal is the Jacobian w.r.t. calibration
+   */
+  inline Point2 project(
+      const Point3& pw, //
+      boost::optional<Matrix&> Dpose,
+      boost::optional<Matrix&> Dpoint,
+      boost::optional<Matrix&> Dcal) const {
 
     // Transform to camera coordinates and check cheirality
     const Point3 pc = pose_.transform_to(pw);
@@ -327,7 +376,7 @@ public:
       }
       return pi;
     } else
-      return K_.uncalibrate(pn, Dcal);
+      return K_.uncalibrate(pn, Dcal, boost::none);
   }
 
   /** project a point at infinity from world coordinate to the image
@@ -356,7 +405,7 @@ public:
     Dpc_pose.block(0, 0, 3, 3) = Dpc_rot;
 
     // camera to normalized image coordinate
-    Matrix Dpn_pc; // 2*3
+    Matrix23 Dpn_pc; // 2*3
     const Point2 pn = project_to_camera(pc, Dpn_pc);
 
     // uncalibration
@@ -391,12 +440,12 @@ public:
       const double z = pc.z(), d = 1.0 / z;
 
       // uncalibration
-      Matrix Dcal, Dpi_pn(2, 2);
+      Matrix Dcal, Dpi_pn(2,2);
       const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
 
       if (Dcamera) {
         Dcamera->resize(2, this->dim());
-        calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>());
+        calculateDpose(pn, d, Dpi_pn.block<2,2>(0,0), Dcamera->leftCols<6>());
         Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
       }
       if (Dpoint) {
@@ -518,16 +567,16 @@ private:
    * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
    */
   template<typename Derived>
-  static void calculateDpose(const Point2& pn, double d, const Matrix& Dpi_pn,
+  static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn,
       Eigen::MatrixBase<Derived> const & Dpose) {
     // optimized version of derivatives, see CalibratedCamera.nb
     const double u = pn.x(), v = pn.y();
     double uv = u * v, uu = u * u, vv = v * v;
-    Eigen::Matrix<double, 2, 6> Dpn_pose;
+    Matrix26 Dpn_pose;
     Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
     assert(Dpose.rows()==2 && Dpose.cols()==6);
     const_cast<Eigen::MatrixBase<Derived>&>(Dpose) = //
-        Dpi_pn.block<2, 2>(0, 0) * Dpn_pose;
+        Dpi_pn * Dpn_pose;
   }
 
   /**
@@ -539,18 +588,18 @@ private:
    * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
    */
   template<typename Derived>
-  static void calculateDpoint(const Point2& pn, double d, const Matrix& R,
-      const Matrix& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) {
+  static void calculateDpoint(const Point2& pn, double d, const Matrix3& R,
+      const Matrix2& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) {
     // optimized version of derivatives, see CalibratedCamera.nb
     const double u = pn.x(), v = pn.y();
-    Eigen::Matrix<double, 2, 3> Dpn_point;
+    Matrix23 Dpn_point;
     Dpn_point << //
         R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), //
     /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
     Dpn_point *= d;
     assert(Dpoint.rows()==2 && Dpoint.cols()==3);
     const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) = //
-        Dpi_pn.block<2, 2>(0, 0) * Dpn_point;
+        Dpi_pn * Dpn_point;
   }
 
   /// @}
diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 85307e322..90c3f5f8c 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -123,6 +123,27 @@ Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
   return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
 }
 
+/* ************************************************************************* */
+// see doc/math.lyx, SE(2) section
+Point2 Pose2::transform_to(const Point2& point) const {
+  Point2 d = point - t_;
+  return r_.unrotate(d);
+}
+
+/* ************************************************************************* */
+// see doc/math.lyx, SE(2) section
+Point2 Pose2::transform_to(const Point2& point,
+    boost::optional<Matrix23&> H1, boost::optional<Matrix2&> H2) const {
+  Point2 d = point - t_;
+  Point2 q = r_.unrotate(d);
+  if (!H1 && !H2) return q;
+  if (H1) *H1 <<
+      -1.0, 0.0,  q.y(),
+      0.0, -1.0, -q.x();
+  if (H2) *H2 = r_.transpose();
+  return q;
+}
+
 /* ************************************************************************* */
 // see doc/math.lyx, SE(2) section
 Point2 Pose2::transform_to(const Point2& point,
@@ -161,6 +182,62 @@ Point2 Pose2::transform_from(const Point2& p,
   return q + t_;
 }
 
+/* ************************************************************************* */
+Pose2 Pose2::between(const Pose2& p2) const {
+  // get cosines and sines from rotation matrices
+  const Rot2& R1 = r_, R2 = p2.r();
+  double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
+
+  // Assert that R1 and R2 are normalized
+  assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
+
+  // Calculate delta rotation = between(R1,R2)
+  double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
+  Rot2 R(Rot2::atan2(s,c)); // normalizes
+
+  // Calculate delta translation = unrotate(R1, dt);
+  Point2 dt = p2.t() - t_;
+  double x = dt.x(), y = dt.y();
+  // t = R1' * (t2-t1)
+  Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
+
+  return Pose2(R,t);
+}
+
+/* ************************************************************************* */
+Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix3&> H1,
+    boost::optional<Matrix3&> H2) const {
+  // get cosines and sines from rotation matrices
+  const Rot2& R1 = r_, R2 = p2.r();
+  double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
+
+  // Assert that R1 and R2 are normalized
+  assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
+
+  // Calculate delta rotation = between(R1,R2)
+  double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
+  Rot2 R(Rot2::atan2(s,c)); // normalizes
+
+  // Calculate delta translation = unrotate(R1, dt);
+  Point2 dt = p2.t() - t_;
+  double x = dt.x(), y = dt.y();
+  // t = R1' * (t2-t1)
+  Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
+
+  // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
+  if (H1) {
+    double dt1 = -s2 * x + c2 * y;
+    double dt2 = -c2 * x - s2 * y;
+    *H1 <<
+        -c,  -s,  dt1,
+        s,  -c,  dt2,
+        0.0, 0.0,-1.0;
+  }
+  if (H2) *H2 = I3;
+
+  return Pose2(R,t);
+}
+
 /* ************************************************************************* */
 Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
     boost::optional<Matrix&> H2) const {
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 26244877b..13773ddb4 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -123,10 +123,19 @@ public:
   /**
    * Return relative pose between p1 and p2, in p1 coordinate frame
    */
-  Pose2 between(const Pose2& p2,
-      boost::optional<Matrix&> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const;
+  Pose2 between(const Pose2& p2) const;
 
+  /**
+   * Return relative pose between p1 and p2, in p1 coordinate frame
+   */
+  Pose2 between(const Pose2& p2, boost::optional<Matrix3&> H1,
+      boost::optional<Matrix3&> H2) const;
+
+  /**
+   * Return relative pose between p1 and p2, in p1 coordinate frame
+   */
+  Pose2 between(const Pose2& p2, boost::optional<Matrix&> H1,
+      boost::optional<Matrix&> H2) const;
 
   /// @}
   /// @name Manifold
@@ -182,10 +191,18 @@ public:
   /// @name Group Action on Point2
   /// @{
 
+  /** Return point coordinates in pose coordinate frame */
+  Point2 transform_to(const Point2& point) const;
+
   /** Return point coordinates in pose coordinate frame */
   Point2 transform_to(const Point2& point,
-      boost::optional<Matrix&> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const;
+      boost::optional<Matrix23&> H1,
+      boost::optional<Matrix2&> H2) const;
+
+  /** Return point coordinates in pose coordinate frame */
+  Point2 transform_to(const Point2& point,
+      boost::optional<Matrix&> H1,
+      boost::optional<Matrix&> H2) const;
 
   /** Return point coordinates in global frame */
   Point2 transform_from(const Point2& point,
diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index ea04c1d44..b7a0c1714 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -254,16 +254,36 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
 }
 
 /* ************************************************************************* */
-Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
-    boost::optional<Matrix&> Dpoint) const {
+Point3 Pose3::transform_to(const Point3& p) const {
+  return R_.unrotate(p - t_);
+}
+
+/* ************************************************************************* */
+Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
+    boost::optional<Matrix3&> Dpoint) const {
   // Only get transpose once, to avoid multiple allocations,
   // as well as multiple conversions in the Quaternion case
   Matrix3 Rt(R_.transpose());
   const Point3 q(Rt*(p - t_).vector());
   if (Dpose) {
-    double wx = q.x();
-    double wy = q.y();
-    double wz = q.z();
+    const double wx = q.x(), wy = q.y(), wz = q.z();
+    (*Dpose) <<
+        0.0, -wz, +wy,-1.0, 0.0, 0.0,
+        +wz, 0.0, -wx, 0.0,-1.0, 0.0,
+        -wy, +wx, 0.0, 0.0, 0.0,-1.0;
+  }
+  if (Dpoint)
+    *Dpoint = Rt;
+  return q;
+}
+
+/* ************************************************************************* */
+Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
+    boost::optional<Matrix&> Dpoint) const {
+  Matrix3 Rt(R_.transpose());
+  const Point3 q(Rt*(p - t_).vector());
+  if (Dpose) {
+    const double wx = q.x(), wy = q.y(), wz = q.z();
     Dpose->resize(3, 6);
     (*Dpose) <<
         0.0, -wz, +wy,-1.0, 0.0, 0.0,
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 825389243..55cda05a8 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -250,8 +250,13 @@ public:
      * @param Dpoint optional 3*3 Jacobian wrpt point
      * @return point in Pose coordinates
      */
+    Point3 transform_to(const Point3& p) const;
+
     Point3 transform_to(const Point3& p,
-        boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const;
+        boost::optional<Matrix36&> Dpose, boost::optional<Matrix3&> Dpoint) const;
+
+    Point3 transform_to(const Point3& p,
+        boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) const;
 
     /// @}
     /// @name Standard Interface
diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index 37aa78a78..daa8eeda1 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -97,13 +97,33 @@ Unit3 Rot3::operator*(const Unit3& p) const {
   return rotate(p);
 }
 
+/* ************************************************************************* */
+// see doc/math.lyx, SO(3) section
+Point3 Rot3::unrotate(const Point3& p, boost::optional<Matrix3&> H1,
+    boost::optional<Matrix3&> H2) 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 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
+  if (H2)
+    *H2 = Rt;
+  return q;
+}
+
 /* ************************************************************************* */
 // see doc/math.lyx, SO(3) section
 Point3 Rot3::unrotate(const Point3& p,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-  Point3 q(transpose()*p.vector()); // q = Rt*p
-  if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
-  if (H2) *H2 = transpose();
+  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;
 }
 
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index c8aeae51b..115f288e3 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -219,8 +219,15 @@ namespace gtsam {
     Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const;
 
     /// Compose two rotations i.e., R= (*this) * R2
-    Rot3 compose(const Rot3& R2,
-    boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
+    Rot3 compose(const Rot3& R2) const;
+
+    /// Compose two rotations i.e., R= (*this) * R2
+    Rot3 compose(const Rot3& R2, boost::optional<Matrix3&> H1,
+        boost::optional<Matrix3&> H2) const;
+
+    /// Compose two rotations i.e., R= (*this) * R2
+    Rot3 compose(const Rot3& R2, boost::optional<Matrix&> H1,
+        boost::optional<Matrix&> H2) const;
 
     /** compose two rotations */
     Rot3 operator*(const Rot3& R2) const;
@@ -328,11 +335,16 @@ namespace gtsam {
     /// 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, boost::optional<Matrix&> H1 = boost::none,
-        boost::optional<Matrix&> H2 = boost::none) 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;
 
     /// @}
     /// @name Group Action on Unit3
diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp
index 118d8546e..96ebcac08 100644
--- a/gtsam/geometry/Rot3M.cpp
+++ b/gtsam/geometry/Rot3M.cpp
@@ -143,6 +143,19 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) {
      -swy + C02,  swx + C12,    c + C22);
 }
 
+/* ************************************************************************* */
+Rot3 Rot3::compose (const Rot3& R2) const {
+  return *this * R2;
+}
+
+/* ************************************************************************* */
+Rot3 Rot3::compose (const Rot3& R2,
+    boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) const {
+  if (H1) *H1 = R2.transpose();
+  if (H2) *H2 = I3;
+  return *this * R2;
+}
+
 /* ************************************************************************* */
 Rot3 Rot3::compose (const Rot3& R2,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@@ -301,6 +314,12 @@ Quaternion Rot3::toQuaternion() const {
   return Quaternion(rot_);
 }
 
+/* ************************************************************************* */
+Point3 Rot3::unrotate(const Point3& p) const {
+  // Eigen expression
+  return Point3(rot_.transpose()*p.vector()); // q = Rt*p
+}
+
 /* ************************************************************************* */
 
 } // namespace gtsam
diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp
index c5990153a..4344a623c 100644
--- a/gtsam/geometry/Rot3Q.cpp
+++ b/gtsam/geometry/Rot3Q.cpp
@@ -82,6 +82,19 @@ namespace gtsam {
   Rot3 Rot3::rodriguez(const Vector& w, double theta) {
     return Quaternion(Eigen::AngleAxisd(theta, w)); }
 
+  /* ************************************************************************* */
+  Rot3 Rot3::compose(const Rot3& R2) const {
+    return Rot3(quaternion_ * R2.quaternion_);
+  }
+
+  /* ************************************************************************* */
+  Rot3 Rot3::compose(const Rot3& R2,
+  boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) const {
+    if (H1) *H1 = R2.transpose();
+    if (H2) *H2 = I3;
+    return Rot3(quaternion_ * R2.quaternion_);
+  }
+
   /* ************************************************************************* */
   Rot3 Rot3::compose(const Rot3& R2,
   boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@@ -158,6 +171,11 @@ namespace gtsam {
   /* ************************************************************************* */
   Quaternion Rot3::toQuaternion() const { return quaternion_; }
 
+  /* ************************************************************************* */
+  Point3 Rot3::unrotate(const Point3& p) const {
+    return Point3(transpose()*p.vector()); // q = Rt*p
+  }
+
  /* ************************************************************************* */
 
 } // namespace gtsam
diff --git a/gtsam/geometry/TriangulationFactor.h b/gtsam/geometry/TriangulationFactor.h
index 24b7918e3..fc8d546d3 100644
--- a/gtsam/geometry/TriangulationFactor.h
+++ b/gtsam/geometry/TriangulationFactor.h
@@ -115,7 +115,7 @@ public:
   Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
       boost::none) const {
     try {
-      Point2 error(camera_.project(point, boost::none, H2) - measured_);
+      Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_);
       return error.vector();
     } catch (CheiralityException& e) {
       if (H2)
@@ -155,7 +155,7 @@ public:
 
     // Would be even better if we could pass blocks to project
     const Point3& point = x.at<Point3>(key());
-    b = -(camera_.project(point, boost::none, A) - measured_).vector();
+    b = -(camera_.project(point, boost::none, A, boost::none) - measured_).vector();
     if (noiseModel_)
       this->noiseModel_->WhitenSystem(A, b);
 
diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp
index 9ced28dca..366d09d49 100644
--- a/gtsam/geometry/tests/testSimpleCamera.cpp
+++ b/gtsam/geometry/tests/testSimpleCamera.cpp
@@ -125,7 +125,7 @@ static Point2 project2(const Pose3& pose, const Point3& point) {
 TEST( SimpleCamera, Dproject_point_pose)
 {
   Matrix Dpose, Dpoint;
-  Point2 result = camera.project(point1, Dpose, Dpoint);
+  Point2 result = camera.project(point1, Dpose, Dpoint, boost::none);
   Matrix numerical_pose  = numericalDerivative21(project2, pose1, point1);
   Matrix numerical_point = numericalDerivative22(project2, pose1, point1);
   CHECK(assert_equal(result, Point2(-100,  100) ));
diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h
index 44f543bc9..bcd33a095 100644
--- a/gtsam/navigation/MagFactor.h
+++ b/gtsam/navigation/MagFactor.h
@@ -53,7 +53,7 @@ public:
 
   static Point3 unrotate(const Rot2& R, const Point3& p,
       boost::optional<Matrix&> HR = boost::none) {
-    Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR);
+    Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR, boost::none);
     if (HR) {
       // assign to temporary first to avoid error in Win-Debug mode
       Matrix H = HR->col(2);
@@ -106,7 +106,7 @@ public:
   Vector evaluateError(const Rot3& nRb,
       boost::optional<Matrix&> H = boost::none) const {
     // measured bM = nRbÕ * nM + b
-    Point3 hx = nRb.unrotate(nM_, H) + bias_;
+    Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
     return (hx - measured_).vector();
   }
 };
diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h
index 9e6f3f8ba..557565a6e 100644
--- a/gtsam/slam/EssentialMatrixFactor.h
+++ b/gtsam/slam/EssentialMatrixFactor.h
@@ -174,12 +174,14 @@ public:
     } else {
 
       // Calculate derivatives. TODO if slow: optimize with Mathematica
-      //     3*2        3*3       3*3        2*3
-      Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2;
+      //     3*2        3*3       3*3
+      Matrix D_1T2_dir, DdP2_rot, DP2_point;
 
       Point3 _1T2 = E.direction().point3(D_1T2_dir);
       Point3 d1T2 = d * _1T2;
       Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
+
+      Matrix23 Dpn_dP2;
       pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
 
       if (DE) {
diff --git a/gtsam/slam/ImplicitSchurFactor.h b/gtsam/slam/ImplicitSchurFactor.h
index c0f339b30..e579d38a1 100644
--- a/gtsam/slam/ImplicitSchurFactor.h
+++ b/gtsam/slam/ImplicitSchurFactor.h
@@ -29,7 +29,6 @@ public:
 protected:
 
   typedef Eigen::Matrix<double, 2, D> Matrix2D; ///< type of an F block
-  typedef Eigen::Matrix<double, 2, 3> Matrix23;
   typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
   typedef std::pair<Key, Matrix2D> KeyMatrix2D; ///< named F block
 
@@ -203,7 +202,7 @@ public:
 
       // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
       //      static const Eigen::Matrix<double, 2, 2> I2 = eye(2);
-      //      Eigen::Matrix<double, 2, 2> Q = //
+      //      Matrix2 Q = //
       //          I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose();
       //      blocks[j] = Fj.transpose() * Q * Fj;
     }
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index 3e093c7c4..db37e6b8d 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -132,17 +132,17 @@ namespace gtsam {
           if(H1) {
             gtsam::Matrix H0;
             PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
-            Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
+            Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
             *H1 = *H1 * H0;
             return reprojectionError.vector();
           } else {
             PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
-            Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
+            Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
             return reprojectionError.vector();
           }
         } else {
           PinholeCamera<CALIBRATION> camera(pose, *K_);
-          Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
+          Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
           return reprojectionError.vector();
         }
       } catch( CheiralityException& e) {
diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h
index ebc430277..52dcea2db 100644
--- a/gtsam_unstable/dynamics/SimpleHelicopter.h
+++ b/gtsam_unstable/dynamics/SimpleHelicopter.h
@@ -125,7 +125,7 @@ public:
     Vector pk_1 = muk_1 - 0.5*Pose3::adjointTranspose(-h_*xik_1, muk_1, D_adjThxik1_muk1);
 
     Matrix D_gravityBody_gk;
-    Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk);
+    Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk, boost::none);
     Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z());
 
     Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext;
diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h
index 4eb7992a2..30f17fb7a 100644
--- a/gtsam_unstable/geometry/InvDepthCamera3.h
+++ b/gtsam_unstable/geometry/InvDepthCamera3.h
@@ -104,7 +104,7 @@ public:
     }
     else {
       gtsam::Matrix J2;
-      gtsam::Point2 uv= camera.project(landmark,H1, J2);
+      gtsam::Point2 uv= camera.project(landmark,H1, J2, boost::none);
       if (H1) {
         *H1 = (*H1) * gtsam::eye(6);
       }
diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h
index 786ee2b21..06405579e 100644
--- a/gtsam_unstable/nonlinear/Expression-inl.h
+++ b/gtsam_unstable/nonlinear/Expression-inl.h
@@ -22,6 +22,7 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/base/Matrix.h>
 #include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
 
 namespace gtsam {
 
@@ -134,16 +135,25 @@ public:
 };
 
 //-----------------------------------------------------------------------------
-template<class T>
 struct JacobianTrace {
-  T t;
-  T value() const {
-    return t;
+  virtual ~JacobianTrace() {
   }
   virtual void reverseAD(JacobianMap& jacobians) const = 0;
   virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0;
+//  template<class JacobianFT>
+//  void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const {
 };
 
+typedef JacobianTrace* TracePtr;
+
+//template <class Derived>
+//struct TypedTrace {
+//  virtual void reverseAD(JacobianMap& jacobians) const = 0;
+//  virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0;
+////  template<class JacobianFT>
+////  void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const {
+//};
+
 //-----------------------------------------------------------------------------
 /**
  * Expression node. The superclass for objects that do the heavy lifting
@@ -175,8 +185,7 @@ public:
   virtual Augmented<T> forward(const Values& values) const = 0;
 
   /// Construct an execution trace for reverse AD
-  virtual boost::shared_ptr<JacobianTrace<T> > traceExecution(
-      const Values& values) const = 0;
+  virtual T traceExecution(const Values& values, TracePtr& p) const = 0;
 };
 
 //-----------------------------------------------------------------------------
@@ -217,7 +226,7 @@ public:
   }
 
   /// Trace structure for reverse AD
-  struct Trace: public JacobianTrace<T> {
+  struct Trace: public JacobianTrace {
     /// If the expression is just a constant, we do nothing
     virtual void reverseAD(JacobianMap& jacobians) const {
     }
@@ -227,11 +236,10 @@ public:
   };
 
   /// Construct an execution trace for reverse AD
-  virtual boost::shared_ptr<JacobianTrace<T> > traceExecution(
-      const Values& values) const {
-    boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
-    trace->t = constant_;
-    return trace;
+  virtual T traceExecution(const Values& values, TracePtr& p) const {
+    Trace* trace = new Trace();
+    p = static_cast<TracePtr>(trace);
+    return constant_;
   }
 };
 
@@ -270,12 +278,11 @@ public:
 
   /// Return value and derivatives
   virtual Augmented<T> forward(const Values& values) const {
-    T t = value(values);
-    return Augmented<T>(t, key_);
+    return Augmented<T>(values.at<T>(key_), key_);
   }
 
   /// Trace structure for reverse AD
-  struct Trace: public JacobianTrace<T> {
+  struct Trace: public JacobianTrace {
     Key key;
     /// If the expression is just a leaf, we just insert an identity matrix
     virtual void reverseAD(JacobianMap& jacobians) const {
@@ -293,12 +300,11 @@ public:
   };
 
   /// Construct an execution trace for reverse AD
-  virtual boost::shared_ptr<JacobianTrace<T> > traceExecution(
-      const Values& values) const {
-    boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
-    trace->t = value(values);
+  virtual T traceExecution(const Values& values, TracePtr& p) const {
+    Trace* trace = new Trace();
+    p = static_cast<TracePtr>(trace);
     trace->key = key_;
-    return trace;
+    return values.at<T>(key_);
   }
 
 };
@@ -310,7 +316,8 @@ class UnaryExpression: public ExpressionNode<T> {
 
 public:
 
-  typedef boost::function<T(const A&, boost::optional<Matrix&>)> Function;
+  typedef Eigen::Matrix<double, T::dimension, A::dimension> JacobianTA;
+  typedef boost::function<T(const A&, boost::optional<JacobianTA&>)> Function;
 
 private:
 
@@ -344,33 +351,35 @@ public:
   virtual Augmented<T> forward(const Values& values) const {
     using boost::none;
     Augmented<A> argument = this->expressionA_->forward(values);
-    Matrix dTdA;
+    JacobianTA dTdA;
     T t = function_(argument.value(),
-        argument.constant() ? none : boost::optional<Matrix&>(dTdA));
+        argument.constant() ? none : boost::optional<JacobianTA&>(dTdA));
     return Augmented<T>(t, dTdA, argument.jacobians());
   }
 
   /// Trace structure for reverse AD
-  struct Trace: public JacobianTrace<T> {
-    boost::shared_ptr<JacobianTrace<A> > trace1;
-    Matrix dTdA;
+  struct Trace: public JacobianTrace {
+    TracePtr trace;
+    JacobianTA dTdA;
+    virtual ~Trace() {
+      delete trace;
+    }
     /// Start the reverse AD process
     virtual void reverseAD(JacobianMap& jacobians) const {
-      trace1->reverseAD(dTdA, jacobians);
+      trace->reverseAD(dTdA, jacobians);
     }
     /// Given df/dT, multiply in dT/dA and continue reverse AD process
     virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const {
-      trace1->reverseAD(dFdT * dTdA, jacobians);
+      trace->reverseAD(dFdT * dTdA, jacobians);
     }
   };
 
   /// Construct an execution trace for reverse AD
-  virtual boost::shared_ptr<JacobianTrace<T> > traceExecution(
-      const Values& values) const {
-    boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
-    trace->trace1 = this->expressionA_->traceExecution(values);
-    trace->t = function_(trace->trace1->value(), trace->dTdA);
-    return trace;
+  virtual T traceExecution(const Values& values, TracePtr& p) const {
+    Trace* trace = new Trace();
+    p = static_cast<TracePtr>(trace);
+    A a = this->expressionA_->traceExecution(values,trace->trace);
+    return function_(a, trace->dTdA);
   }
 };
 
@@ -382,9 +391,11 @@ class BinaryExpression: public ExpressionNode<T> {
 
 public:
 
+  typedef Eigen::Matrix<double, T::dimension, A1::dimension> JacobianTA1;
+  typedef Eigen::Matrix<double, T::dimension, A2::dimension> JacobianTA2;
   typedef boost::function<
-      T(const A1&, const A2&, boost::optional<Matrix&>,
-          boost::optional<Matrix&>)> Function;
+      T(const A1&, const A2&, boost::optional<JacobianTA1&>,
+          boost::optional<JacobianTA2&>)> Function;
 
 private:
 
@@ -426,18 +437,23 @@ public:
     using boost::none;
     Augmented<A1> a1 = this->expressionA1_->forward(values);
     Augmented<A2> a2 = this->expressionA2_->forward(values);
-    Matrix dTdA1, dTdA2;
+    JacobianTA1 dTdA1;
+    JacobianTA2 dTdA2;
     T t = function_(a1.value(), a2.value(),
-        a1.constant() ? none : boost::optional<Matrix&>(dTdA1),
-        a2.constant() ? none : boost::optional<Matrix&>(dTdA2));
+        a1.constant() ? none : boost::optional<JacobianTA1&>(dTdA1),
+        a2.constant() ? none : boost::optional<JacobianTA2&>(dTdA2));
     return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians());
   }
 
   /// Trace structure for reverse AD
-  struct Trace: public JacobianTrace<T> {
-    boost::shared_ptr<JacobianTrace<A1> > trace1;
-    boost::shared_ptr<JacobianTrace<A2> > trace2;
-    Matrix dTdA1, dTdA2;
+  struct Trace: public JacobianTrace {
+    TracePtr trace1, trace2;
+    JacobianTA1 dTdA1;
+    JacobianTA2 dTdA2;
+    virtual ~Trace() {
+      delete trace1;
+      delete trace2;
+    }
     /// Start the reverse AD process
     virtual void reverseAD(JacobianMap& jacobians) const {
       trace1->reverseAD(dTdA1, jacobians);
@@ -451,14 +467,12 @@ public:
   };
 
   /// Construct an execution trace for reverse AD
-  virtual boost::shared_ptr<JacobianTrace<T> > traceExecution(
-      const Values& values) const {
-    boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
-    trace->trace1 = this->expressionA1_->traceExecution(values);
-    trace->trace2 = this->expressionA2_->traceExecution(values);
-    trace->t = function_(trace->trace1->value(), trace->trace2->value(),
-        trace->dTdA1, trace->dTdA2);
-    return trace;
+  virtual T traceExecution(const Values& values, TracePtr& p) const {
+    Trace* trace = new Trace();
+    p = static_cast<TracePtr>(trace);
+    A1 a1 = this->expressionA1_->traceExecution(values,trace->trace1);
+    A2 a2 = this->expressionA2_->traceExecution(values,trace->trace2);
+    return function_(a1, a2, trace->dTdA1, trace->dTdA2);
   }
 
 };
@@ -471,9 +485,12 @@ class TernaryExpression: public ExpressionNode<T> {
 
 public:
 
+  typedef Eigen::Matrix<double, T::dimension, A1::dimension> JacobianTA1;
+  typedef Eigen::Matrix<double, T::dimension, A2::dimension> JacobianTA2;
+  typedef Eigen::Matrix<double, T::dimension, A3::dimension> JacobianTA3;
   typedef boost::function<
-      T(const A1&, const A2&, const A3&, boost::optional<Matrix&>,
-          boost::optional<Matrix&>, boost::optional<Matrix&>)> Function;
+      T(const A1&, const A2&, const A3&, boost::optional<JacobianTA1&>,
+          boost::optional<JacobianTA2&>, boost::optional<JacobianTA3&>)> Function;
 
 private:
 
@@ -523,21 +540,28 @@ public:
     Augmented<A1> a1 = this->expressionA1_->forward(values);
     Augmented<A2> a2 = this->expressionA2_->forward(values);
     Augmented<A3> a3 = this->expressionA3_->forward(values);
-    Matrix dTdA1, dTdA2, dTdA3;
+    JacobianTA1 dTdA1;
+    JacobianTA2 dTdA2;
+    JacobianTA3 dTdA3;
     T t = function_(a1.value(), a2.value(), a3.value(),
-        a1.constant() ? none : boost::optional<Matrix&>(dTdA1),
-        a2.constant() ? none : boost::optional<Matrix&>(dTdA2),
-        a3.constant() ? none : boost::optional<Matrix&>(dTdA3));
+        a1.constant() ? none : boost::optional<JacobianTA1&>(dTdA1),
+        a2.constant() ? none : boost::optional<JacobianTA2&>(dTdA2),
+        a3.constant() ? none : boost::optional<JacobianTA3&>(dTdA3));
     return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians(), dTdA3,
         a3.jacobians());
   }
 
   /// Trace structure for reverse AD
-  struct Trace: public JacobianTrace<T> {
-    boost::shared_ptr<JacobianTrace<A1> > trace1;
-    boost::shared_ptr<JacobianTrace<A2> > trace2;
-    boost::shared_ptr<JacobianTrace<A3> > trace3;
-    Matrix dTdA1, dTdA2, dTdA3;
+  struct Trace: public JacobianTrace {
+    TracePtr trace1, trace2, trace3;
+    JacobianTA1 dTdA1;
+    JacobianTA2 dTdA2;
+    JacobianTA3 dTdA3;
+    virtual ~Trace() {
+      delete trace1;
+      delete trace2;
+      delete trace3;
+    }
     /// Start the reverse AD process
     virtual void reverseAD(JacobianMap& jacobians) const {
       trace1->reverseAD(dTdA1, jacobians);
@@ -553,15 +577,13 @@ public:
   };
 
   /// Construct an execution trace for reverse AD
-  virtual boost::shared_ptr<JacobianTrace<T> > traceExecution(
-      const Values& values) const {
-    boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
-    trace->trace1 = this->expressionA1_->traceExecution(values);
-    trace->trace2 = this->expressionA2_->traceExecution(values);
-    trace->trace3 = this->expressionA3_->traceExecution(values);
-    trace->t = function_(trace->trace1->value(), trace->trace2->value(),
-        trace->trace3->value(), trace->dTdA1, trace->dTdA2, trace->dTdA3);
-    return trace;
+  virtual T traceExecution(const Values& values, TracePtr& p) const {
+    Trace* trace = new Trace();
+    p = static_cast<TracePtr>(trace);
+    A1 a1 = this->expressionA1_->traceExecution(values,trace->trace1);
+    A2 a2 = this->expressionA2_->traceExecution(values,trace->trace2);
+    A3 a3 = this->expressionA3_->traceExecution(values,trace->trace3);
+    return function_(a1, a2, a3, trace->dTdA1, trace->dTdA2, trace->dTdA3);
   }
 
 };
diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h
index 18e6c35e1..06265a9fb 100644
--- a/gtsam_unstable/nonlinear/Expression.h
+++ b/gtsam_unstable/nonlinear/Expression.h
@@ -76,8 +76,10 @@ public:
   /// Construct a unary method expression
   template<typename A1, typename A2>
   Expression(const Expression<A1>& expression1,
-      T (A1::*method)(const A2&, boost::optional<Matrix&>,
-          boost::optional<Matrix&>) const, const Expression<A2>& expression2) {
+      T (A1::*method)(const A2&,
+          boost::optional<typename BinaryExpression<T, A1, A2>::JacobianTA1&>,
+          boost::optional<typename BinaryExpression<T, A1, A2>::JacobianTA2&>) const,
+      const Expression<A2>& expression2) {
     root_.reset(
         new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),
             expression1, expression2));
@@ -94,9 +96,11 @@ public:
   /// Construct a ternary function expression
   template<typename A1, typename A2, typename A3>
   Expression(typename TernaryExpression<T, A1, A2, A3>::Function function,
-      const Expression<A1>& expression1, const Expression<A2>& expression2,  const Expression<A3>& expression3) {
+      const Expression<A1>& expression1, const Expression<A2>& expression2,
+      const Expression<A3>& expression3) {
     root_.reset(
-        new TernaryExpression<T, A1, A2, A3>(function, expression1, expression2, expression3));
+        new TernaryExpression<T, A1, A2, A3>(function, expression1, expression2,
+            expression3));
   }
 
   /// Return keys that play in this expression
@@ -113,9 +117,11 @@ public:
   Augmented<T> augmented(const Values& values) const {
 #define REVERSE_AD
 #ifdef REVERSE_AD
-    boost::shared_ptr<JacobianTrace<T> > trace(root_->traceExecution(values));
-    Augmented<T> augmented(trace->value());
+    TracePtr trace;
+    T value = root_->traceExecution(values,trace);
+    Augmented<T> augmented(value);
     trace->reverseAD(augmented.jacobians());
+    delete trace;
     return augmented;
 #else
     return root_->forward(values);
@@ -132,8 +138,9 @@ public:
 template<class T>
 struct apply_compose {
   typedef T result_type;
-  T operator()(const T& x, const T& y, boost::optional<Matrix&> H1,
-      boost::optional<Matrix&> H2) const {
+  typedef Eigen::Matrix<double, T::dimension, T::dimension> Jacobian;
+  T operator()(const T& x, const T& y, boost::optional<Jacobian&> H1,
+      boost::optional<Jacobian&> H2) const {
     return x.compose(y, H1, H2);
   }
 };
diff --git a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp
index 4a22feef2..44a7eab3f 100644
--- a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp
+++ b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp
@@ -58,208 +58,217 @@ TEST(BADFactor, test) {
 }
 
 /* ************************************************************************* */
- // Unary(Binary(Leaf,Leaf))
- TEST(BADFactor, test1) {
+// Unary(Binary(Leaf,Leaf))
+TEST(BADFactor, test1) {
 
- // Create some values
- Values values;
- values.insert(1, Pose3());
- values.insert(2, Point3(0, 0, 1));
+  // Create some values
+  Values values;
+  values.insert(1, Pose3());
+  values.insert(2, Point3(0, 0, 1));
 
- // Create old-style factor to create expected value and derivatives
- GenericProjectionFactor<Pose3, Point3> old(measured, model, 1, 2,
- boost::make_shared<Cal3_S2>());
- double expected_error = old.error(values);
- GaussianFactor::shared_ptr expected = old.linearize(values);
+  // Create old-style factor to create expected value and derivatives
+  GenericProjectionFactor<Pose3, Point3> old(measured, model, 1, 2,
+      boost::make_shared<Cal3_S2>());
+  double expected_error = old.error(values);
+  GaussianFactor::shared_ptr expected = old.linearize(values);
 
- // Create leaves
- Pose3_ x(1);
- Point3_ p(2);
+  // Create leaves
+  Pose3_ x(1);
+  Point3_ p(2);
 
- // Try concise version
- BADFactor<Point2> f2(model, measured, project(transform_to(x, p)));
- EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
- EXPECT_LONGS_EQUAL(2, f2.dim());
- boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
- EXPECT( assert_equal(*expected, *gf2, 1e-9));
- }
+  // Try concise version
+  BADFactor<Point2> f2(model, measured, project(transform_to(x, p)));
+  EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
+  EXPECT_LONGS_EQUAL(2, f2.dim());
+  boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
+  EXPECT( assert_equal(*expected, *gf2, 1e-9));
+}
 
- /* ************************************************************************* */
- // Binary(Leaf,Unary(Binary(Leaf,Leaf)))
- TEST(BADFactor, test2) {
+/* ************************************************************************* */
+// Binary(Leaf,Unary(Binary(Leaf,Leaf)))
+TEST(BADFactor, test2) {
 
- // Create some values
- Values values;
- values.insert(1, Pose3());
- values.insert(2, Point3(0, 0, 1));
- values.insert(3, Cal3_S2());
+  // Create some values
+  Values values;
+  values.insert(1, Pose3());
+  values.insert(2, Point3(0, 0, 1));
+  values.insert(3, Cal3_S2());
 
- // Create old-style factor to create expected value and derivatives
- GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3);
- double expected_error = old.error(values);
- GaussianFactor::shared_ptr expected = old.linearize(values);
+  // Create old-style factor to create expected value and derivatives
+  GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3);
+  double expected_error = old.error(values);
+  GaussianFactor::shared_ptr expected = old.linearize(values);
 
- // Create leaves
- Pose3_ x(1);
- Point3_ p(2);
- Cal3_S2_ K(3);
+  // Create leaves
+  Pose3_ x(1);
+  Point3_ p(2);
+  Cal3_S2_ K(3);
 
- // Create expression tree
- Point3_ p_cam(x, &Pose3::transform_to, p);
- Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
- Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
+  // Create expression tree
+  Point3_ p_cam(x, &Pose3::transform_to, p);
+  Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
+  Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
 
- // Create factor and check value, dimension, linearization
- BADFactor<Point2> f(model, measured, uv_hat);
- EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
- EXPECT_LONGS_EQUAL(2, f.dim());
- boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
- EXPECT( assert_equal(*expected, *gf, 1e-9));
+  // Create factor and check value, dimension, linearization
+  BADFactor<Point2> f(model, measured, uv_hat);
+  EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
+  EXPECT_LONGS_EQUAL(2, f.dim());
+  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
+  EXPECT( assert_equal(*expected, *gf, 1e-9));
 
- // Try concise version
- BADFactor<Point2> f2(model, measured,
- uncalibrate(K, project(transform_to(x, p))));
- EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
- EXPECT_LONGS_EQUAL(2, f2.dim());
- boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
- EXPECT( assert_equal(*expected, *gf2, 1e-9));
- }
+  // Try concise version
+  BADFactor<Point2> f2(model, measured,
+      uncalibrate(K, project(transform_to(x, p))));
+  EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
+  EXPECT_LONGS_EQUAL(2, f2.dim());
+  boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
+  EXPECT( assert_equal(*expected, *gf2, 1e-9));
 
- /* ************************************************************************* */
+  TernaryExpression<Point2, Pose3, Point3, Cal3_S2>::Function fff = project6;
 
- TEST(BADFactor, compose1) {
+  // Try ternary version
+  BADFactor<Point2> f3(model, measured, project3(x, p, K));
+  EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9);
+  EXPECT_LONGS_EQUAL(2, f3.dim());
+  boost::shared_ptr<GaussianFactor> gf3 = f3.linearize(values);
+  EXPECT( assert_equal(*expected, *gf3, 1e-9));
+}
 
- // Create expression
- Rot3_ R1(1), R2(2);
- Rot3_ R3 = R1 * R2;
+/* ************************************************************************* */
 
- // Create factor
- BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
+TEST(BADFactor, compose1) {
 
- // Create some values
- Values values;
- values.insert(1, Rot3());
- values.insert(2, Rot3());
+  // Create expression
+  Rot3_ R1(1), R2(2);
+  Rot3_ R3 = R1 * R2;
 
- // Check unwhitenedError
- std::vector<Matrix> H(2);
- Vector actual = f.unwhitenedError(values, H);
- EXPECT( assert_equal(eye(3), H[0],1e-9));
- EXPECT( assert_equal(eye(3), H[1],1e-9));
+  // Create factor
+  BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
 
- // Check linearization
- JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
- boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
- boost::shared_ptr<JacobianFactor> jf = //
- boost::dynamic_pointer_cast<JacobianFactor>(gf);
- EXPECT( assert_equal(expected, *jf,1e-9));
- }
+  // Create some values
+  Values values;
+  values.insert(1, Rot3());
+  values.insert(2, Rot3());
 
- /* ************************************************************************* */
- // Test compose with arguments referring to the same rotation
- TEST(BADFactor, compose2) {
+  // Check unwhitenedError
+  std::vector<Matrix> H(2);
+  Vector actual = f.unwhitenedError(values, H);
+  EXPECT( assert_equal(eye(3), H[0],1e-9));
+  EXPECT( assert_equal(eye(3), H[1],1e-9));
 
- // Create expression
- Rot3_ R1(1), R2(1);
- Rot3_ R3 = R1 * R2;
+  // Check linearization
+  JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
+  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
+  boost::shared_ptr<JacobianFactor> jf = //
+      boost::dynamic_pointer_cast<JacobianFactor>(gf);
+  EXPECT( assert_equal(expected, *jf,1e-9));
+}
 
- // Create factor
- BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
+/* ************************************************************************* */
+// Test compose with arguments referring to the same rotation
+TEST(BADFactor, compose2) {
 
- // Create some values
- Values values;
- values.insert(1, Rot3());
+  // Create expression
+  Rot3_ R1(1), R2(1);
+  Rot3_ R3 = R1 * R2;
 
- // Check unwhitenedError
- std::vector<Matrix> H(1);
- Vector actual = f.unwhitenedError(values, H);
- EXPECT_LONGS_EQUAL(1, H.size());
- EXPECT( assert_equal(2*eye(3), H[0],1e-9));
+  // Create factor
+  BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
 
- // Check linearization
- JacobianFactor expected(1, 2 * eye(3), zero(3));
- boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
- boost::shared_ptr<JacobianFactor> jf = //
- boost::dynamic_pointer_cast<JacobianFactor>(gf);
- EXPECT( assert_equal(expected, *jf,1e-9));
- }
+  // Create some values
+  Values values;
+  values.insert(1, Rot3());
 
- /* ************************************************************************* */
- // Test compose with one arguments referring to a constant same rotation
- TEST(BADFactor, compose3) {
+  // Check unwhitenedError
+  std::vector<Matrix> H(1);
+  Vector actual = f.unwhitenedError(values, H);
+  EXPECT_LONGS_EQUAL(1, H.size());
+  EXPECT( assert_equal(2*eye(3), H[0],1e-9));
 
- // Create expression
- Rot3_ R1(Rot3::identity()), R2(3);
- Rot3_ R3 = R1 * R2;
+  // Check linearization
+  JacobianFactor expected(1, 2 * eye(3), zero(3));
+  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
+  boost::shared_ptr<JacobianFactor> jf = //
+      boost::dynamic_pointer_cast<JacobianFactor>(gf);
+  EXPECT( assert_equal(expected, *jf,1e-9));
+}
 
- // Create factor
- BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
+/* ************************************************************************* */
+// Test compose with one arguments referring to a constant same rotation
+TEST(BADFactor, compose3) {
 
- // Create some values
- Values values;
- values.insert(3, Rot3());
+  // Create expression
+  Rot3_ R1(Rot3::identity()), R2(3);
+  Rot3_ R3 = R1 * R2;
 
- // Check unwhitenedError
- std::vector<Matrix> H(1);
- Vector actual = f.unwhitenedError(values, H);
- EXPECT_LONGS_EQUAL(1, H.size());
- EXPECT( assert_equal(eye(3), H[0],1e-9));
+  // Create factor
+  BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
 
- // Check linearization
- JacobianFactor expected(3, eye(3), zero(3));
- boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
- boost::shared_ptr<JacobianFactor> jf = //
- boost::dynamic_pointer_cast<JacobianFactor>(gf);
- EXPECT( assert_equal(expected, *jf,1e-9));
- }
+  // Create some values
+  Values values;
+  values.insert(3, Rot3());
 
- /* ************************************************************************* */
- // Test compose with three arguments
- Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
-     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
-     boost::optional<Matrix&> H3) {
-   // return dummy derivatives (not correct, but that's ok for testing here)
-   if (H1)
-     *H1 = eye(3);
-   if (H2)
-     *H2 = eye(3);
-   if (H3)
-     *H3 = eye(3);
-   return R1 * (R2 * R3);
- }
+  // Check unwhitenedError
+  std::vector<Matrix> H(1);
+  Vector actual = f.unwhitenedError(values, H);
+  EXPECT_LONGS_EQUAL(1, H.size());
+  EXPECT( assert_equal(eye(3), H[0],1e-9));
 
- TEST(BADFactor, composeTernary) {
+  // Check linearization
+  JacobianFactor expected(3, eye(3), zero(3));
+  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
+  boost::shared_ptr<JacobianFactor> jf = //
+      boost::dynamic_pointer_cast<JacobianFactor>(gf);
+  EXPECT( assert_equal(expected, *jf,1e-9));
+}
 
- // Create expression
- Rot3_ A(1), B(2), C(3);
- Rot3_ ABC(composeThree, A, B, C);
+/* ************************************************************************* */
+// Test compose with three arguments
+Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
+    boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2,
+    boost::optional<Matrix3&> H3) {
+  // return dummy derivatives (not correct, but that's ok for testing here)
+  if (H1)
+    *H1 = eye(3);
+  if (H2)
+    *H2 = eye(3);
+  if (H3)
+    *H3 = eye(3);
+  return R1 * (R2 * R3);
+}
 
- // Create factor
- BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC);
+TEST(BADFactor, composeTernary) {
 
- // Create some values
- Values values;
- values.insert(1, Rot3());
- values.insert(2, Rot3());
- values.insert(3, Rot3());
+  // Create expression
+  Rot3_ A(1), B(2), C(3);
+  Rot3_ ABC(composeThree, A, B, C);
 
- // Check unwhitenedError
- std::vector<Matrix> H(3);
- Vector actual = f.unwhitenedError(values, H);
- EXPECT_LONGS_EQUAL(3, H.size());
- EXPECT( assert_equal(eye(3), H[0],1e-9));
- EXPECT( assert_equal(eye(3), H[1],1e-9));
- EXPECT( assert_equal(eye(3), H[2],1e-9));
+  // Create factor
+  BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC);
 
- // Check linearization
- JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3));
- boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
- boost::shared_ptr<JacobianFactor> jf = //
- boost::dynamic_pointer_cast<JacobianFactor>(gf);
- EXPECT( assert_equal(expected, *jf,1e-9));
- }
+  // Create some values
+  Values values;
+  values.insert(1, Rot3());
+  values.insert(2, Rot3());
+  values.insert(3, Rot3());
 
- /* ************************************************************************* */
+  // Check unwhitenedError
+  std::vector<Matrix> H(3);
+  Vector actual = f.unwhitenedError(values, H);
+  EXPECT_LONGS_EQUAL(3, H.size());
+  EXPECT( assert_equal(eye(3), H[0],1e-9));
+  EXPECT( assert_equal(eye(3), H[1],1e-9));
+  EXPECT( assert_equal(eye(3), H[2],1e-9));
+
+  // Check linearization
+  JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3));
+  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
+  boost::shared_ptr<JacobianFactor> jf = //
+      boost::dynamic_pointer_cast<JacobianFactor>(gf);
+  EXPECT( assert_equal(expected, *jf,1e-9));
+}
+
+/* ************************************************************************* */
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);
diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp
index 51a7ecbc7..188394e0a 100644
--- a/gtsam_unstable/nonlinear/tests/testExpression.cpp
+++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp
@@ -32,12 +32,12 @@ using namespace gtsam;
 /* ************************************************************************* */
 
 template<class CAL>
-Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional<Matrix&> Dcal,
-    boost::optional<Matrix&> Dp) {
+Point2 uncalibrate(const CAL& K, const Point2& p,
+    boost::optional<Matrix25&> Dcal, boost::optional<Matrix2&> Dp) {
   return K.uncalibrate(p, Dcal, Dp);
 }
 
-static const Rot3 someR = Rot3::RzRyRx(1,2,3);
+static const Rot3 someR = Rot3::RzRyRx(1, 2, 3);
 
 /* ************************************************************************* */
 
@@ -55,7 +55,7 @@ TEST(Expression, constant) {
 TEST(Expression, leaf) {
   Expression<Rot3> R(100);
   Values values;
-  values.insert(100,someR);
+  values.insert(100, someR);
   Augmented<Rot3> a = R.augmented(values);
   EXPECT(assert_equal(someR, a.value()));
   JacobianMap expected;
@@ -76,7 +76,6 @@ TEST(Expression, leaf) {
 //  expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50));
 //  EXPECT(assert_equal(expected.at(67),a.jacobians().at(67)));
 //}
-
 /* ************************************************************************* */
 
 TEST(Expression, test) {
@@ -149,8 +148,8 @@ TEST(Expression, compose3) {
 /* ************************************************************************* */
 // Test with ternary function
 Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
-    boost::optional<Matrix&> H3) {
+    boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2,
+    boost::optional<Matrix3&> H3) {
   // return dummy derivatives (not correct, but that's ok for testing here)
   if (H1)
     *H1 = eye(3);
diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h
index e7060dcd4..b69f852b4 100644
--- a/gtsam_unstable/slam/ProjectionFactorPPP.h
+++ b/gtsam_unstable/slam/ProjectionFactorPPP.h
@@ -127,13 +127,13 @@ namespace gtsam {
           if(H1 || H2 || H3) {
             gtsam::Matrix H0, H02;
             PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_);
-            Point2 reprojectionError(camera.project(point, H1, H3) - measured_);
+            Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
             *H2 = *H1 * H02;
             *H1 = *H1 * H0;
             return reprojectionError.vector();
           } else {
             PinholeCamera<CALIBRATION> camera(pose.compose(transform), *K_);
-            Point2 reprojectionError(camera.project(point, H1, H3) - measured_);
+            Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
             return reprojectionError.vector();
           }
       } catch( CheiralityException& e) {
diff --git a/gtsam_unstable/slam/expressions.h b/gtsam_unstable/slam/expressions.h
index 1acde67d3..d9cbd5716 100644
--- a/gtsam_unstable/slam/expressions.h
+++ b/gtsam_unstable/slam/expressions.h
@@ -10,6 +10,7 @@
 #include <gtsam_unstable/nonlinear/Expression.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/PinholeCamera.h>
+#include <boost/bind.hpp>
 
 namespace gtsam {
 
@@ -48,6 +49,16 @@ Point2_ project(const Point3_& p_cam) {
   return Point2_(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
 }
 
+Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
+    boost::optional<Matrix26&> Dpose, boost::optional<Matrix23&> Dpoint,
+    boost::optional<Matrix25&> Dcal) {
+  return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
+}
+
+Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) {
+  return Point2_(project6, x, p, K);
+}
+
 template<class CAL>
 Point2_ uncalibrate(const Expression<CAL>& K, const Point2_& xy_hat) {
   return Point2_(K, &CAL::uncalibrate, xy_hat);
diff --git a/gtsam_unstable/timing/timeCameraExpression.cpp b/gtsam_unstable/timing/timeCameraExpression.cpp
index baa515029..3b5d85b72 100644
--- a/gtsam_unstable/timing/timeCameraExpression.cpp
+++ b/gtsam_unstable/timing/timeCameraExpression.cpp
@@ -26,7 +26,6 @@
 #include <time.h>
 #include <iostream>
 #include <iomanip>      // std::setprecision
-
 using namespace std;
 using namespace gtsam;
 
@@ -47,9 +46,9 @@ void time(const NonlinearFactor& f, const Values& values) {
 boost::shared_ptr<Cal3_S2> fixedK(new Cal3_S2());
 
 Point2 myProject(const Pose3& pose, const Point3& point,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
+    boost::optional<Matrix26&> H1, boost::optional<Matrix23&> H2) {
   PinholeCamera<Cal3_S2> camera(pose, *fixedK);
-  return camera.project(point, H1, H2);
+  return camera.project(point, H1, H2, boost::none);
 }
 
 int main() {
@@ -74,37 +73,43 @@ int main() {
   // Dedicated factor
   // Oct 3, 2014, Macbook Air
   // 4.2 musecs/call
-  GeneralSFMFactor2<Cal3_S2> oldFactor2(z, model, 1, 2, 3);
-  time(oldFactor2, values);
+  GeneralSFMFactor2<Cal3_S2> f1(z, model, 1, 2, 3);
+  time(f1, values);
 
   // BADFactor
   // Oct 3, 2014, Macbook Air
   // 20.3 musecs/call
-  BADFactor<Point2> newFactor2(model, z,
+  BADFactor<Point2> f2(model, z,
       uncalibrate(K, project(transform_to(x, p))));
-  time(newFactor2, values);
+  time(f2, values);
+
+  // BADFactor ternary
+  // Oct 3, 2014, Macbook Air
+  // 20.3 musecs/call
+  BADFactor<Point2> f3(model, z, project3(x, p, K));
+  time(f3, values);
 
   // CALIBRATED
 
   // Dedicated factor
   // Oct 3, 2014, Macbook Air
   // 3.4 musecs/call
-  GenericProjectionFactor<Pose3, Point3> oldFactor1(z, model, 1, 2, fixedK);
-  time(oldFactor1, values);
+  GenericProjectionFactor<Pose3, Point3> g1(z, model, 1, 2, fixedK);
+  time(g1, values);
 
   // BADFactor
   // Oct 3, 2014, Macbook Air
   // 16.0 musecs/call
-  BADFactor<Point2> newFactor1(model, z,
+  BADFactor<Point2> g2(model, z,
       uncalibrate(Cal3_S2_(*fixedK), project(transform_to(x, p))));
-  time(newFactor1, values);
+  time(g2, values);
 
   // BADFactor, optimized
   // Oct 3, 2014, Macbook Air
   // 9.0 musecs/call
   typedef PinholeCamera<Cal3_S2> Camera;
   typedef Expression<Camera> Camera_;
-  BADFactor<Point2> newFactor3(model, z, Point2_(myProject, x, p));
-  time(newFactor3, values);
+  BADFactor<Point2> g3(model, z, Point2_(myProject, x, p));
+  time(g3, values);
   return 0;
 }
diff --git a/gtsam_unstable/timing/timeOneCameraExpression.cpp b/gtsam_unstable/timing/timeOneCameraExpression.cpp
index a76200812..11bf65709 100644
--- a/gtsam_unstable/timing/timeOneCameraExpression.cpp
+++ b/gtsam_unstable/timing/timeOneCameraExpression.cpp
@@ -25,7 +25,7 @@
 using namespace std;
 using namespace gtsam;
 
-static const int n = 500000;
+static const int n = 1000000;
 
 void time(const NonlinearFactor& f, const Values& values) {
   long timeLog = clock();
@@ -58,9 +58,13 @@ int main() {
   // BADFactor
   // Oct 3, 2014, Macbook Air
   // 20.3 musecs/call
-  BADFactor<Point2> newFactor2(model, z,
-      uncalibrate(K, project(transform_to(x, p))));
-  time(newFactor2, values);
+#define TERNARY
+#ifdef TERNARY
+  BADFactor<Point2> f(model, z, project3(x, p, K));
+#else
+  BADFactor<Point2> f(model, z, uncalibrate(K, project(transform_to(x, p))));
+#endif
+  time(f, values);
 
   return 0;
 }