diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index f867b3088..16ea55665 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -246,33 +246,34 @@ class ExpmapFunctor : public so3::DexpFunctor {
   static constexpr double one_one_hundred_twentieth = 1.0 / 120.0;
 
  public:
-  ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
-      : so3::DexpFunctor(omega, nearZeroApprox) {}
+  ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false,
+                bool includeHigherOrder = false)
+      : so3::DexpFunctor(omega, nearZeroApprox),
+        includeHigherOrder(includeHigherOrder) {}
 
-  // Compute the Jacobian Q with respect to w
   Matrix3 computeQ(const Vector3& v) const {
     const Matrix3 V = skewSymmetric(v);
     const Matrix3 WVW = W * V * W;
 
     if (!nearZero) {
-      const double theta3 = theta2 * theta;
-      const double theta4 = theta2 * theta2;
-      const double theta5 = theta4 * theta;
-      const double s = sin_theta;
-      const double a = one_minus_cos - theta2 / 2;
-
-      // The closed-form formula in Barfoot14tro eq. (102)
-      return -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) +
-             a / theta4 * (WW * V + V * WW - 3 * WVW) -
-             0.5 *
-                 (a / theta4 - 3 * (theta - s - theta3 * one_sixth) / theta5) *
-                 (WVW * W + W * WVW);
+      // Simplified from closed-form formula in Barfoot14tro eq. (102)
+      // Note dexp = I_3x3 - B * W + C * WW and t = dexp * v
+      return -0.5 * V + C * (W * V + V * W - WVW) +
+             (B - 0.5) / theta2 * (WW * V + V * WW - 3 * WVW) -
+             0.5 * (B - 3 * C) / theta2 * (WVW * W + W * WVW);
     } else {
-      return -0.5 * V + one_sixth * (W * V + V * W - WVW) -
-             one_twenty_fourth * (WW * V + V * WW - 3 * WVW) +
-             one_one_hundred_twentieth * (WVW * W + W * WVW);
+      Matrix3 Q = -0.5 * V + one_sixth * (W * V + V * W - WVW);
+      Q -= one_twenty_fourth * (WW * V + V * WW - 3 * WVW);
+
+      if (includeHigherOrder) {
+        Q += one_one_hundred_twentieth * (WVW * W + W * WVW);
+      }
+      return Q;
     }
   }
+
+ private:
+  bool includeHigherOrder;
 };
 }  // namespace pose3
 
diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index d4c30119e..626599abf 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -85,7 +85,7 @@ SO3 ExpmapFunctor::expmap() const {
 
 Matrix3 DexpFunctor::leftJacobian() const {
   if (nearZero) {
-    return I_3x3 + 0.5 * W + one_sixth * WW;
+    return I_3x3 + 0.5 * W;  // + one_sixth * WW;
   } else {
     return I_3x3 + B * W + C * WW;
   }
@@ -94,7 +94,7 @@ Matrix3 DexpFunctor::leftJacobian() const {
 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
   if (nearZero) {
-    rightJacobian_ = I_3x3 - 0.5 * W + one_sixth * WW;
+    rightJacobian_ = I_3x3 - 0.5 * W;  // + one_sixth * WW;
   } else {
     C = (1 - A) / theta2;
     rightJacobian_ = I_3x3 - B * W + C * WW;
@@ -104,18 +104,15 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
 Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                OptionalJacobian<3, 3> H2) const {
   if (H1) {
+    const Matrix3 V = skewSymmetric(v);
     if (nearZero) {
-      *H1 = 0.5 * skewSymmetric(v);
+      *H1 = 0.5 * V;
     } else {
       // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
-      double a = B * theta;
-      double b = C * theta2;
-      const Vector3 Kv = W * v / theta;
-      const double Da = (sin_theta - 2.0 * a) / theta2;
-      const double Db = (one_minus_cos - 3.0 * b) / theta2;
-      *H1 = (Db * W / theta - Da * I_3x3) * Kv * omega.transpose() -
-            skewSymmetric(Kv * b / theta) +
-            (a * I_3x3 - b * W / theta) * skewSymmetric(v / theta);
+      const double Da = (A - 2.0 * B) / theta2;
+      const double Db = (B - 3.0 * C) / theta2;
+      *H1 = (Db * WW - Da * W) * v * omega.transpose() -
+            C * skewSymmetric(W * v) + B * V - C * W * V;
     }
   }
   if (H2) *H2 = rightJacobian_;