diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp
index 1469e265d..e2713e9e7 100644
--- a/gtsam/base/Matrix.cpp
+++ b/gtsam/base/Matrix.cpp
@@ -238,11 +238,6 @@ istream& operator>>(istream& inputStream, Matrix& destinationMatrix) {
   return inputStream;
 }
 
-/* ************************************************************************* */
-void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j) {
-  fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix;
-}
-
 /* ************************************************************************* */
 Matrix diag(const std::vector<Matrix>& Hs) {
   size_t rows = 0, cols = 0;
diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index 565960660..4fac6d3e0 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -238,7 +238,10 @@ Eigen::Block<const MATRIX> sub(const MATRIX& A, size_t i1, size_t i2, size_t j1,
  * @param i is the row of the upper left corner insert location
  * @param j is the column of the upper left corner insert location
  */
-GTSAM_EXPORT void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j);
+template <typename Derived1, typename Derived2>
+GTSAM_EXPORT void insertSub(Eigen::MatrixBase<Derived1>& fullMatrix, const Eigen::MatrixBase<Derived2>& subMatrix, size_t i, size_t j) {
+  fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix;
+}
 
 /**
  * Create a matrix with submatrices along its diagonal
diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp
index 06aa4ac24..97fa36d61 100644
--- a/gtsam_unstable/dynamics/PoseRTV.cpp
+++ b/gtsam_unstable/dynamics/PoseRTV.cpp
@@ -71,7 +71,15 @@ Vector9 PoseRTV::Logmap(const PoseRTV& p) {
 }
 
 /* ************************************************************************* */
-PoseRTV PoseRTV::retract(const Vector& v) const {
+PoseRTV PoseRTV::retract(const Vector& v,
+                         OptionalJacobian<dimension, dimension> Horigin,
+                         OptionalJacobian<dimension, dimension> Hv) const {
+  if (Horigin) {
+    CONCEPT_NOT_IMPLEMENTED;
+  }
+  if (Hv) {
+    CONCEPT_NOT_IMPLEMENTED;
+  }
   assert(v.size() == 9);
   // First order approximation
   Pose3 newPose = Rt_.retract(sub(v, 0, 6));
@@ -80,7 +88,15 @@ PoseRTV PoseRTV::retract(const Vector& v) const {
 }
 
 /* ************************************************************************* */
-Vector PoseRTV::localCoordinates(const PoseRTV& p1) const {
+Vector PoseRTV::localCoordinates(const PoseRTV& p1,
+                                 OptionalJacobian<dimension, dimension> Horigin,
+                                 OptionalJacobian<dimension, dimension> Hp) const {
+  if (Horigin) {
+    CONCEPT_NOT_IMPLEMENTED;
+  }
+  if (Hp) {
+    CONCEPT_NOT_IMPLEMENTED;
+  }
   const Pose3& x0 = pose(), &x1 = p1.pose();
   // First order approximation
   Vector6 poseLogmap = x0.localCoordinates(x1);
@@ -90,7 +106,7 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1) const {
 
 /* ************************************************************************* */
 PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); }
-PoseRTV PoseRTV::inverse(boost::optional<Matrix&> H1) const {
+PoseRTV PoseRTV::inverse(OptionalJacobian<dimension,dimension> H1) const {
   if (H1) *H1 = numericalDerivative11<PoseRTV,PoseRTV>(inverse_, *this, 1e-5);
   return PoseRTV(Rt_.inverse(), v_.inverse());
 }
@@ -98,8 +114,8 @@ PoseRTV PoseRTV::inverse(boost::optional<Matrix&> H1) const {
 /* ************************************************************************* */
 PoseRTV compose_(const PoseRTV& p1, const PoseRTV& p2) { return p1.compose(p2); }
 PoseRTV PoseRTV::compose(const PoseRTV& p,
-    boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H2) const {
+    OptionalJacobian<dimension,dimension> H1,
+    OptionalJacobian<dimension,dimension> H2) const {
   if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5);
   if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5);
   return PoseRTV(Rt_.compose(p.Rt_), v_.compose(p.v_));
@@ -108,8 +124,8 @@ PoseRTV PoseRTV::compose(const PoseRTV& p,
 /* ************************************************************************* */
 PoseRTV between_(const PoseRTV& p1, const PoseRTV& p2) { return p1.between(p2); }
 PoseRTV PoseRTV::between(const PoseRTV& p,
-    boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H2) const {
+    OptionalJacobian<dimension,dimension> H1,
+    OptionalJacobian<dimension,dimension> H2) const {
   if (H1) *H1 = numericalDerivative21(between_, *this, p, 1e-5);
   if (H2) *H2 = numericalDerivative22(between_, *this, p, 1e-5);
   return inverse().compose(p);
@@ -227,7 +243,7 @@ Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, doub
 /* ************************************************************************* */
 double range_(const PoseRTV& A, const PoseRTV& B) { return A.range(B); }
 double PoseRTV::range(const PoseRTV& other,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+    OptionalJacobian<1,dimension> H1, OptionalJacobian<1,dimension> H2) const {
   if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5);
   if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5);
   return t().distance(other.t());
@@ -239,8 +255,8 @@ PoseRTV transformed_from_(const PoseRTV& global, const Pose3& transform) {
 }
 
 PoseRTV PoseRTV::transformed_from(const Pose3& trans,
-    boost::optional<Matrix&> Dglobal,
-    boost::optional<Matrix&> Dtrans) const {
+    OptionalJacobian<dimension,dimension> Dglobal,
+    OptionalJacobian<dimension,traits_x<Pose3>::dimension> Dtrans) const {
   // Note that we rotate the velocity
   Matrix DVr, DTt;
   Velocity3 newvel = trans.rotation().rotate(v_, DVr, DTt);
diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h
index 4c2c1d39b..40b666361 100644
--- a/gtsam_unstable/dynamics/PoseRTV.h
+++ b/gtsam_unstable/dynamics/PoseRTV.h
@@ -80,8 +80,8 @@ public:
    *    - v(3-5): Point3
    *    - v(6-8): Translational velocity
    */
-  PoseRTV retract(const Vector& v) const;
-  Vector localCoordinates(const PoseRTV& p) const;
+  PoseRTV retract(const Vector& v, OptionalJacobian<dimension, dimension> Horigin=boost::none, OptionalJacobian<dimension, dimension> Hv=boost::none) const;
+  Vector localCoordinates(const PoseRTV& p, OptionalJacobian<dimension, dimension> Horigin=boost::none,OptionalJacobian<dimension, dimension> Hp=boost::none) const;
 
   // Lie
   /**
@@ -94,24 +94,25 @@ public:
   static PoseRTV identity() { return PoseRTV(); }
 
   /** Derivatives calculated numerically */
-  PoseRTV inverse(boost::optional<Matrix&> H1=boost::none) const;
+  PoseRTV inverse(OptionalJacobian<dimension,dimension> H1=boost::none) const;
 
   /** Derivatives calculated numerically */
   PoseRTV compose(const PoseRTV& p,
-      boost::optional<Matrix&> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const;
+      OptionalJacobian<dimension,dimension> H1=boost::none,
+      OptionalJacobian<dimension,dimension> H2=boost::none) const;
 
+  PoseRTV operator*(const PoseRTV& p) { return compose(p); }
 
   /** Derivatives calculated numerically */
   PoseRTV between(const PoseRTV& p,
-      boost::optional<Matrix&> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const;
+                  OptionalJacobian<dimension,dimension> H1=boost::none,
+                  OptionalJacobian<dimension,dimension> H2=boost::none) const;
 
   // measurement functions
   /** Derivatives calculated numerically */
   double range(const PoseRTV& other,
-      boost::optional<Matrix&> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const;
+               OptionalJacobian<1,dimension> H1=boost::none,
+               OptionalJacobian<1,dimension> H2=boost::none) const;
 
   // IMU-specific
 
@@ -158,8 +159,8 @@ public:
    * Note: the transform jacobian convention is flipped
    */
   PoseRTV transformed_from(const Pose3& trans,
-      boost::optional<Matrix&> Dglobal=boost::none,
-      boost::optional<Matrix&> Dtrans=boost::none) const;
+                           OptionalJacobian<dimension,dimension> Dglobal=boost::none,
+                           OptionalJacobian<dimension,traits_x<Pose3>::dimension> Dtrans=boost::none) const;
 
   // Utility functions
 
@@ -187,6 +188,6 @@ private:
 
 
 template<>
-struct traits_x<PoseRTV> : public internal::LieGroup<PoseRTV> {};
+struct traits_x<PoseRTV> : public internal::LieGroup<PoseRTV, multiplicative_group_tag> {};
 
 } // \namespace gtsam