fix degenerated cases in Pose3's ExpmapDerivative. testPose3 passed.

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,
+  double phi = w.norm();
-      phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
+  if (fabs(phi)>1e-5) {
-  // Invert the sign of odd-order terms to have the right Jacobian
+    double sinPhi = sin(phi), cosPhi = cos(phi);
-  Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W)
+    double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
-          + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W)
+    // Invert the sign of odd-order terms to have the right Jacobian
-          - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6)/phi5)*(W*V*W*W + W*W*V*W);
+    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;

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);
 }
 
 /* ************************************************************************* */