From 2aaa7e7564da5640ec101bd4d447fab98b7d1592 Mon Sep 17 00:00:00 2001
From: thduynguyen <thduynguyen@gmail.com>
Date: Thu, 25 Dec 2014 16:59:11 -0500
Subject: [PATCH] fix degenerated cases in Pose3's ExpmapDerivative. testPose3
 passed.

---
 gtsam/geometry/Pose3.cpp          | 20 ++++++++++++++------
 gtsam/geometry/tests/testRot3.cpp |  4 +++-
 2 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 81a20445a..03777e405 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -207,12 +207,20 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
   }
 #else
   // The closed-form formula in Barfoot14tro eq. (102)
-  double phi = w.norm(), sinPhi = sin(phi), cosPhi = cos(phi), phi2 = phi * phi,
-      phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
-  // Invert the sign of odd-order terms to have the right Jacobian
-  Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W)
-          + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W)
-          - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6)/phi5)*(W*V*W*W + W*W*V*W);
+  double phi = w.norm();
+  if (fabs(phi)>1e-5) {
+    double sinPhi = sin(phi), cosPhi = cos(phi);
+    double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
+    // Invert the sign of odd-order terms to have the right Jacobian
+    Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W)
+            + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W)
+            - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*V*W*W + W*W*V*W);
+  }
+  else {
+    Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W)
+        + 1./24.*(W*W*V + V*W*W - 3*W*V*W)
+        - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W);
+  }
 #endif
 
   return Q;
diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index b232aa92c..e910d38c1 100644
--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@@ -18,10 +18,10 @@
 
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Rot3.h>
-
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/lieProxies.h>
+#include <gtsam/base/testLie.h>
 //#include <gtsam/base/chartTesting.h>
 
 #include <boost/math/constants/constants.hpp>
@@ -665,6 +665,8 @@ TEST(Rot3 , Traits) {
 
   numericalH1 = numericalDerivative11(traits_x<Rot3>::Inverse, R1);
   EXPECT(assert_equal(numericalH1,actualH1));
+
+  CHECK_LIE_GROUP_DERIVATIVES(R1,R2);
 }
 
 /* ************************************************************************* */