diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp
index 06cc70e5d..859a36543 100644
--- a/cpp/Rot3.cpp
+++ b/cpp/Rot3.cpp
@@ -182,24 +182,19 @@ namespace gtsam {
 
   /* ************************************************************************* */
   Point3 unrotate(const Rot3& R, const Point3& p) {
-    return Point3(
-        R.r1().x() * p.x() + R.r1().y() * p.y() + R.r1().z() * p.z(),
-        R.r2().x() * p.x() + R.r2().y() * p.y() + R.r2().z() * p.z(),
-        R.r3().x() * p.x() + R.r3().y() * p.y() + R.r3().z() * p.z()
-    );
+    const Matrix Rt(R.transpose());
+    return Rt*p.vector(); // q = Rt*p
   }
 
   /* ************************************************************************* */
-  /** see libraries/caml/geometry/math.lyx, derivative of unrotate              */
-  /* ************************************************************************* */
-  Matrix Dunrotate1(const Rot3 & R, const Point3 & p) {
-    Point3 q = unrotate(R,p);
-    return skewSymmetric(q.x(), q.y(), q.z());
-  }
-
-  /* ************************************************************************* */
-  Matrix Dunrotate2(const Rot3 & R) {
-    return R.transpose();
+  // see doc/math.lyx, SO(3) section
+  Point3 unrotate(const Rot3& R, const Point3& p,
+  		boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
+    const Matrix Rt(R.transpose());
+    Point3 q(Rt*p.vector()); // q = Rt*p
+    if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
+    if (H2) *H2 = Rt;
+    return q;
   }
 
   /* ************************************************************************* */
diff --git a/cpp/Rot3.h b/cpp/Rot3.h
index b2635277b..f41bc7816 100644
--- a/cpp/Rot3.h
+++ b/cpp/Rot3.h
@@ -188,8 +188,8 @@ namespace gtsam {
    * frame = R'*p
    */
   Point3 unrotate(const Rot3& R, const Point3& p);
-  Matrix Dunrotate1(const Rot3& R, const Point3& p);
-  Matrix Dunrotate2(const Rot3& R); // does not depend on p !
+  Point3 unrotate(const Rot3& R, const Point3& p,
+  	boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
 
   /**
    * compose two rotations i.e., R=R1*R2
diff --git a/cpp/testRot3.cpp b/cpp/testRot3.cpp
index ba588c84a..e686d47bf 100644
--- a/cpp/testRot3.cpp
+++ b/cpp/testRot3.cpp
@@ -213,47 +213,38 @@ TEST(Rot3, BCH)
 
 TEST( Rot3, Drotate1)
 {
-	Matrix computed = Drotate1(R, P);
+	Matrix actualDrotate1 = Drotate1(R, P);
 	Matrix numerical = numericalDerivative21(rotate, R, P);
-	CHECK(assert_equal(numerical,computed,error));
+	CHECK(assert_equal(numerical,actualDrotate1,error));
 }
 
 TEST( Rot3, Drotate1_)
 {
-	Matrix computed = Drotate1(inverse(R), P);
+	Matrix actualDrotate1 = Drotate1(inverse(R), P);
 	Matrix numerical = numericalDerivative21(rotate, inverse(R), P);
-	CHECK(assert_equal(numerical,computed,error));
+	CHECK(assert_equal(numerical,actualDrotate1,error));
 }
 
 TEST( Rot3, Drotate2_DNrotate2)
 {
-	Matrix computed = Drotate2(R);
+	Matrix actualDrotate2 = Drotate2(R);
 	Matrix numerical = numericalDerivative22(rotate, R, P);
-	CHECK(assert_equal(numerical,computed,error));
+	CHECK(assert_equal(numerical,actualDrotate2,error));
 }
 
 /* ************************************************************************* */
 TEST( Rot3, unrotate)
 {
 	Point3 w = R * P;
-	CHECK(assert_equal(unrotate(R,w),P));
-}
+	Matrix H1,H2;
+	Point3 actual = unrotate(R,w,H1,H2);
+	CHECK(assert_equal(P,actual));
 
-/* ************************************************************************* */
-// unrotate derivatives
+	Matrix numerical1 = numericalDerivative21(unrotate, R, w);
+	CHECK(assert_equal(numerical1,H1,error));
 
-TEST( Rot3, Dunrotate1)
-{
-	Matrix computed = Dunrotate1(R, P);
-	Matrix numerical = numericalDerivative21(unrotate, R, P);
-	CHECK(assert_equal(numerical,computed,error));
-}
-
-TEST( Rot3, Dunrotate2_DNunrotate2)
-{
-	Matrix computed = Dunrotate2(R);
-	Matrix numerical = numericalDerivative22(unrotate, R, P);
-	CHECK(assert_equal(numerical,computed,error));
+	Matrix numerical2 = numericalDerivative22(unrotate, R, w);
+	CHECK(assert_equal(numerical2,H2,error));
 }
 
 /* ************************************************************************* */