From f6731fe559c01213ad679966ee602f7f8fc2c473 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sat, 1 Feb 2014 00:50:03 -0500
Subject: [PATCH] More efficient derivative

---
 gtsam/navigation/tests/testMagFactor.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp
index 39a7ccc41..ad9fbc896 100644
--- a/gtsam/navigation/tests/testMagFactor.cpp
+++ b/gtsam/navigation/tests/testMagFactor.cpp
@@ -60,8 +60,12 @@ public:
   static Sphere2 unrotate(const Rot2& R, const Sphere2& p,
       boost::optional<Matrix&> HR = boost::none) {
     Sphere2 q = Rot3::yaw(R.theta()) * p;
-    if (HR) // 2*3                     3*1
-      (*HR) = -q.basis().transpose() * q.skew().col(2);
+    if (HR) {
+      HR->resize(2, 1);
+      Point3 Q = q.unitVector();
+      Matrix B = q.basis().transpose();
+      (*HR) = Q.x() * B.col(1) - Q.y() * B.col(0);
+    }
     return q;
   }