diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp
index 0976a7c1e..379e24bda 100644
--- a/cpp/Pose3.cpp
+++ b/cpp/Pose3.cpp
@@ -142,6 +142,14 @@ namespace gtsam {
     return pose.rotation() * p + pose.translation();
   }
 
+  /* ************************************************************************* */
+  Point3 transform_from(const Pose3& pose, const Point3& p,
+		  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
+	  if (H1) *H1 = Dtransform_from1(pose, p);
+	  if (H2) *H2 = Dtransform_from2(pose);
+	  return transform_from(pose, p);
+  }
+
   /* ************************************************************************* */
   Matrix Dtransform_from1(const Pose3& pose, const Point3& p) {
 #ifdef CORRECT_POSE3_EXMAP
@@ -165,6 +173,14 @@ namespace gtsam {
     return unrotate(pose.rotation(), sub);
   }
 
+  /* ************************************************************************* */
+  Point3 transform_to(const Pose3& pose, const Point3& p,
+    		  	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
+	  if (H1) *H1 = Dtransform_to1(pose, p);
+	  if (H2) *H2 = Dtransform_to2(pose, p);
+	  return transform_to(pose, p);
+  }
+
   /* ************************************************************************* */
   // see math.lyx, derivative of action
   Matrix Dtransform_to1(const Pose3& pose, const Point3& p) {
diff --git a/cpp/Pose3.h b/cpp/Pose3.h
index 006587d71..3b809f54b 100644
--- a/cpp/Pose3.h
+++ b/cpp/Pose3.h
@@ -117,11 +117,17 @@ namespace gtsam {
   /** receives the point in Pose coordinates and transforms it to world coordinates */
   Point3 transform_from(const Pose3& pose, const Point3& p);
   inline Point3 operator*(const Pose3& pose, const Point3& p) { return transform_from(pose, p); }
+  Point3 transform_from(const Pose3& pose, const Point3& p,
+		  	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
+
+  // Older transform functions
   Matrix Dtransform_from1(const Pose3& pose, const Point3& p);
   Matrix Dtransform_from2(const Pose3& pose); // does not depend on p !
 
   /** receives the point in world coordinates and transforms it to Pose coordinates */
   Point3 transform_to(const Pose3& pose, const Point3& p);
+  Point3 transform_to(const Pose3& pose, const Point3& p,
+  		  	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
   Matrix Dtransform_to1(const Pose3& pose, const Point3& p);
   Matrix Dtransform_to2(const Pose3& pose, const Point3& p);
 
diff --git a/cpp/testPose3.cpp b/cpp/testPose3.cpp
index c25a9571d..83f942b12 100644
--- a/cpp/testPose3.cpp
+++ b/cpp/testPose3.cpp
@@ -271,6 +271,28 @@ TEST( Pose3, Dtransform_to2)
   CHECK(assert_equal(numerical,computed,1e-8));
 }
 
+/* ************************************************************************* */
+TEST( Pose3, transform_to_with_derivatives)
+{
+	Matrix actH1, actH2;
+	transform_to(T,P,actH1,actH2);
+	Matrix expH1 = numericalDerivative21(transform_to, T,P),
+		   expH2 = numericalDerivative22(transform_to, T,P);
+	CHECK(assert_equal(expH1, actH1, 1e-8));
+	CHECK(assert_equal(expH2, actH2, 1e-8));
+}
+
+/* ************************************************************************* */
+TEST( Pose3, transform_from_with_derivatives)
+{
+	Matrix actH1, actH2;
+	transform_from(T,P,actH1,actH2);
+	Matrix expH1 = numericalDerivative21(transform_from, T,P),
+		   expH2 = numericalDerivative22(transform_from, T,P);
+	CHECK(assert_equal(expH1, actH1, 1e-8));
+	CHECK(assert_equal(expH2, actH2, 1e-8));
+}
+
 /* ************************************************************************* */
 TEST( Pose3, transform_to_translate)
 {