From 440c3ea64bbcd750c692bca7d8cd348db43bb7f4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 14 Dec 2024 14:28:44 -0500
Subject: [PATCH 01/14] Simplify functor according to Eade

---
 gtsam/geometry/SO3.cpp | 47 +++++++++++++++++++++++-------------------
 gtsam/geometry/SO3.h   | 27 ++++++++++++++++--------
 2 files changed, 44 insertions(+), 30 deletions(-)

diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index 2585c37be..d4c30119e 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -48,12 +48,15 @@ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9,
 }
 
 void ExpmapFunctor::init(bool nearZeroApprox) {
+  WW = W * W;
   nearZero =
       nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
   if (!nearZero) {
     sin_theta = std::sin(theta);
     const double s2 = std::sin(theta / 2.0);
     one_minus_cos = 2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
+    A = sin_theta / theta;
+    B = one_minus_cos / theta2;
   }
 }
 
@@ -62,39 +65,39 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
   const double wx = omega.x(), wy = omega.y(), wz = omega.z();
   W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
   init(nearZeroApprox);
-  if (!nearZero) {
-    K = W / theta;
-    KK = K * K;
-  }
 }
 
 ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
                              bool nearZeroApprox)
     : theta2(angle * angle), theta(angle) {
   const double ax = axis.x(), ay = axis.y(), az = axis.z();
-  K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
-  W = K * angle;
+  W << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
+  W *= angle;
   init(nearZeroApprox);
-  if (!nearZero) {
-    KK = K * K;
-  }
 }
 
 SO3 ExpmapFunctor::expmap() const {
   if (nearZero)
-    return SO3(I_3x3 + W);
+    return SO3(I_3x3 + W + 0.5 * WW);
   else
-    return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
+    return SO3(I_3x3 + A * W + B * WW);
+}
+
+Matrix3 DexpFunctor::leftJacobian() const {
+  if (nearZero) {
+    return I_3x3 + 0.5 * W + one_sixth * WW;
+  } else {
+    return I_3x3 + B * W + C * WW;
+  }
 }
 
 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
   if (nearZero) {
-    dexp_ = I_3x3 - 0.5 * W;
+    rightJacobian_ = I_3x3 - 0.5 * W + one_sixth * WW;
   } else {
-    a = one_minus_cos / theta;
-    b = 1.0 - sin_theta / theta;
-    dexp_ = I_3x3 - a * K + b * KK;
+    C = (1 - A) / theta2;
+    rightJacobian_ = I_3x3 - B * W + C * WW;
   }
 }
 
@@ -105,21 +108,23 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
       *H1 = 0.5 * skewSymmetric(v);
     } else {
       // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
-      const Vector3 Kv = K * v;
+      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 * K - Da * I_3x3) * Kv * omega.transpose() -
+      *H1 = (Db * W / theta - Da * I_3x3) * Kv * omega.transpose() -
             skewSymmetric(Kv * b / theta) +
-            (a * I_3x3 - b * K) * skewSymmetric(v / theta);
+            (a * I_3x3 - b * W / theta) * skewSymmetric(v / theta);
     }
   }
-  if (H2) *H2 = dexp_;
-  return dexp_ * v;
+  if (H2) *H2 = rightJacobian_;
+  return rightJacobian_ * v;
 }
 
 Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                   OptionalJacobian<3, 3> H2) const {
-  const Matrix3 invDexp = dexp_.inverse();
+  const Matrix3 invDexp = rightJacobian_.inverse();
   const Vector3 c = invDexp * v;
   if (H1) {
     Matrix3 D_dexpv_omega;
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index cd67bfb8c..8d4401c31 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -136,7 +136,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
 class GTSAM_EXPORT ExpmapFunctor {
  protected:
   const double theta2;
-  Matrix3 W, K, KK;
+  Matrix3 W, WW;
+  double A, B; // Ethan Eade's constants
   bool nearZero;
   double theta, sin_theta, one_minus_cos;  // only defined if !nearZero
 
@@ -155,13 +156,16 @@ class GTSAM_EXPORT ExpmapFunctor {
 
 /// Functor that implements Exponential map *and* its derivatives
 class DexpFunctor : public ExpmapFunctor {
+ protected:
+  static constexpr double one_sixth = 1.0 / 6.0;
   const Vector3 omega;
-  double a, b;
-  Matrix3 dexp_;
+  double C;  // Ethan Eade's C constant
+  Matrix3 rightJacobian_;
 
  public:
   /// Constructor with element of Lie algebra so(3)
-  GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
+  GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega,
+                                    bool nearZeroApprox = false);
 
   // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
   // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
@@ -169,16 +173,21 @@ class DexpFunctor : public ExpmapFunctor {
   //   expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
   // This maps a perturbation v in the tangent space to
   // a perturbation on the manifold Expmap(dexp * v) */
-  const Matrix3& dexp() const { return dexp_; }
+  const Matrix3& dexp() const { return rightJacobian_; }
 
   /// Multiplies with dexp(), with optional derivatives
-  GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
-                    OptionalJacobian<3, 3> H2 = {}) const;
+  GTSAM_EXPORT Vector3 applyDexp(const Vector3& v,
+                                 OptionalJacobian<3, 3> H1 = {},
+                                 OptionalJacobian<3, 3> H2 = {}) const;
 
   /// Multiplies with dexp().inverse(), with optional derivatives
   GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v,
-                       OptionalJacobian<3, 3> H1 = {},
-                       OptionalJacobian<3, 3> H2 = {}) const;
+                                    OptionalJacobian<3, 3> H1 = {},
+                                    OptionalJacobian<3, 3> H2 = {}) const;
+
+  // Compute the left Jacobian for Exponential map in SO(3)
+  // Note precomputed, as not used as much
+  Matrix3 leftJacobian() const;
 };
 }  //  namespace so3
 

From 7f1e101c6b1f934e23cbd3914a40396b6d282efb Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 14 Dec 2024 14:28:56 -0500
Subject: [PATCH 02/14] Use functor as in SO3

---
 gtsam/geometry/Pose3.cpp           | 73 +++++++++++++++++-------------
 gtsam/geometry/tests/testPose3.cpp | 23 +++++++++-
 2 files changed, 63 insertions(+), 33 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index bb1483432..f867b3088 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -238,14 +238,50 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
 #endif
 }
 
+/* ************************************************************************* */
+namespace pose3 {
+class ExpmapFunctor : public so3::DexpFunctor {
+ private:
+  static constexpr double one_twenty_fourth = 1.0 / 24.0;
+  static constexpr double one_one_hundred_twentieth = 1.0 / 120.0;
+
+ public:
+  ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
+      : so3::DexpFunctor(omega, nearZeroApprox) {}
+
+  // 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);
+    } 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);
+    }
+  }
+};
+}  // namespace pose3
+
 /* ************************************************************************* */
 Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
                                            double nearZeroThreshold) {
-  Matrix3 Q;
   const auto w = xi.head<3>();
   const auto v = xi.tail<3>();
-  ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
-  return Q;
+  return pose3::ExpmapFunctor(w).computeQ(v);
 }
 
 /* ************************************************************************* */
@@ -256,39 +292,12 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
   const double theta2 = w.dot(w);
   bool nearZero = (theta2 <= nearZeroThreshold);
 
-  if (Q) {
-    const Matrix3 V = skewSymmetric(v);
-    const Matrix3 W = skewSymmetric(w);
-    const Matrix3 WVW = W * V * W;
-    const double theta = w.norm();
+  if (Q) *Q = pose3::ExpmapFunctor(w, nearZero).computeQ(v);
 
-    if (nearZero) {
-      static constexpr double one_sixth = 1. / 6.;
-      static constexpr double one_twenty_fourth = 1. / 24.;
-      static constexpr double one_one_hundred_twentieth = 1. / 120.;
-
-      *Q = -0.5 * V + one_sixth * (W * V + V * W - WVW) -
-           one_twenty_fourth * (W * W * V + V * W * W - 3 * WVW) +
-           one_one_hundred_twentieth * (WVW * W + W * WVW);
-    } else {
-      const double s = sin(theta), c = cos(theta);
-      const double theta3 = theta2 * theta, theta4 = theta3 * theta,
-                   theta5 = theta4 * theta;
-
-      // Invert the sign of odd-order terms to have the right Jacobian
-      *Q = -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) +
-           (1 - theta2 / 2 - c) / theta4 * (W * W * V + V * W * W - 3 * WVW) -
-           0.5 *
-               ((1 - theta2 / 2 - c) / theta4 -
-                3 * (theta - s - theta3 / 6.) / theta5) *
-               (WVW * W + W * WVW);
-    }
-  }
-
-  // TODO(Frank): this threshold is *different*. Why?
   if (nearZero) {
     return v + 0.5 * w.cross(v);
   } else {
+    // Geometric intuition and faster than using functor.
     Vector3 t_parallel = w * w.dot(v);  // translation parallel to axis
     Vector3 w_cross_v = w.cross(v);     // translation orthogonal to axis
     Rot3 rotation = R.value_or(Rot3::Expmap(w));
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index c9851f761..4ff4f9a85 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -845,7 +845,28 @@ TEST( Pose3, ExpmapDerivative1) {
 }
 
 /* ************************************************************************* */
-TEST(Pose3, ExpmapDerivative2) {
+TEST( Pose3, ExpmapDerivative2) {
+  Matrix6 actualH;
+  Vector6 w; w << 1.0, -2.0, 3.0, -10.0, -20.0, 30.0;
+  Pose3::Expmap(w,actualH);
+  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
+      OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
+  EXPECT(assert_equal(expectedH, actualH));
+}
+
+/* ************************************************************************* */
+TEST( Pose3, ExpmapDerivative3) {
+  Matrix6 actualH;
+  Vector6 w; w << 0.0, 0.0, 0.0, -10.0, -20.0, 30.0;
+  Pose3::Expmap(w,actualH);
+  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
+      OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
+  // Small angle approximation is not as precise as numerical derivative?
+  EXPECT(assert_equal(expectedH, actualH, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(Pose3, ExpmapDerivative4) {
   // Iserles05an (Lie-group Methods) says:
   // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
   // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)

From d1634c033520920496c117548013339e2c5cddbe Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 14 Dec 2024 15:48:55 -0500
Subject: [PATCH 03/14] Simplified Jacobians with ABC

---
 gtsam/geometry/Pose3.cpp | 37 +++++++++++++++++++------------------
 gtsam/geometry/SO3.cpp   | 19 ++++++++-----------
 2 files changed, 27 insertions(+), 29 deletions(-)

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_;

From 8040a0a31e188d99583a747de4be2dd6887a37f4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 14 Dec 2024 17:22:09 -0500
Subject: [PATCH 04/14] doubleCross, tested

---
 gtsam/geometry/Point3.cpp           | 10 ++++++++++
 gtsam/geometry/Point3.h             |  7 ++++++-
 gtsam/geometry/tests/testPoint3.cpp | 13 +++++++++++++
 3 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp
index ef91108eb..79fd6b9bf 100644
--- a/gtsam/geometry/Point3.cpp
+++ b/gtsam/geometry/Point3.cpp
@@ -69,6 +69,16 @@ Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1,
                 p.x() * q.y() - p.y() * q.x());
 }
 
+Point3 doubleCross(const Point3 &p, const Point3 &q,  //
+                   OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) {
+  if (H1) *H1 = q.dot(p) * I_3x3 + p * q.transpose() - 2 * q * p.transpose();
+  if (H2) {
+    const Matrix3 W = skewSymmetric(p);
+    *H2 = W * W;
+  }
+  return gtsam::cross(p, gtsam::cross(p, q));
+}
+
 double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
            OptionalJacobian<1, 3> H2) {
   if (H1) *H1 << q.x(), q.y(), q.z();
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 8da83817d..0fa53b1b2 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -55,11 +55,16 @@ GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = {});
 /// normalize, with optional Jacobian
 GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = {});
 
-/// cross product @return this x q
+/// cross product @return p x q
 GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
                           OptionalJacobian<3, 3> H_p = {},
                           OptionalJacobian<3, 3> H_q = {});
 
+/// double cross product @return p x (p x q)
+Point3 doubleCross(const Point3& p, const Point3& q,
+                   OptionalJacobian<3, 3> H1 = {},
+                   OptionalJacobian<3, 3> H2 = {});
+
 /// dot product
 GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
                         OptionalJacobian<1, 3> H_p = {},
diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp
index dcfb99105..bb49486a8 100644
--- a/gtsam/geometry/tests/testPoint3.cpp
+++ b/gtsam/geometry/tests/testPoint3.cpp
@@ -18,6 +18,8 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Point3.h>
+#include "gtsam/base/Matrix.h"
+#include "gtsam/base/OptionalJacobian.h"
 
 using namespace std::placeholders;
 using namespace gtsam;
@@ -154,6 +156,17 @@ TEST( Point3, cross2) {
   }
 }
 
+/* ************************************************************************* */
+TEST(Point3, doubleCross) {
+  Matrix aH1, aH2;
+  std::function<Point3(const Point3&, const Point3&)> f =
+      [](const Point3& p, const Point3& q) { return doubleCross(p, q); };
+  const Point3 omega(1, 2, 3), theta(4, 5, 6);
+  doubleCross(omega, theta, aH1, aH2);
+  EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
+  EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
+}
+
 //*************************************************************************
 TEST (Point3, normalize) {
   Matrix actualH;

From 78f17aabc466b81554b513ff22d7d3df42b643ed Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 14 Dec 2024 23:53:51 -0500
Subject: [PATCH 05/14] Simplified applyDexp

---
 gtsam/geometry/SO3.cpp | 82 ++++++++++++++++++++++++++++--------------
 gtsam/geometry/SO3.h   | 17 +++++----
 2 files changed, 66 insertions(+), 33 deletions(-)

diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index 626599abf..bc2e1e6ec 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -18,13 +18,14 @@
  * @date    December 2014
  */
 
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
 #include <gtsam/base/concepts.h>
+#include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/SO3.h>
 
 #include <Eigen/SVD>
-
 #include <cmath>
-#include <iostream>
 #include <limits>
 
 namespace gtsam {
@@ -41,7 +42,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
   return H;
 }
 
-GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) {
+GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
+                             OptionalJacobian<9, 9> H) {
   Matrix3 MR = M * R.matrix();
   if (H) *H = Dcompose(R);
   return MR;
@@ -83,45 +85,54 @@ SO3 ExpmapFunctor::expmap() const {
     return SO3(I_3x3 + A * W + B * WW);
 }
 
-Matrix3 DexpFunctor::leftJacobian() const {
-  if (nearZero) {
-    return I_3x3 + 0.5 * W;  // + one_sixth * WW;
-  } else {
-    return I_3x3 + B * W + C * WW;
-  }
-}
-
 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
+  C = (1 - A) / theta2;
+}
+
+Matrix3 DexpFunctor::rightJacobian() const {
   if (nearZero) {
-    rightJacobian_ = I_3x3 - 0.5 * W;  // + one_sixth * WW;
+    return I_3x3 - 0.5 * W;  // + one_sixth * WW;
   } else {
-    C = (1 - A) / theta2;
-    rightJacobian_ = I_3x3 - B * W + C * WW;
+    return I_3x3 - B * W + C * WW;
   }
 }
 
+// Derivative of w x w x v in w:
+static Matrix3 doubleCrossJacobian(const Vector3& w, const Vector3& v) {
+  return v.dot(w) * I_3x3 + w * v.transpose() - 2 * v * w.transpose();
+}
+
+// Multiplies v with left Jacobian through vector operations only.
+// When Jacobian H1 wrt omega is requested, product rule is invoked twice, once
+// for (B * Wv) and (C * WWv). The Jacobian H2 wrt v is just the right Jacobian.
 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 * V;
-    } else {
-      // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
-      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;
+  // Wv = omega x * v, with Jacobian -V in w
+  const Vector3 Wv = gtsam::cross(omega, v);
+
+  if (nearZero) {
+    if (H1) *H1 = 0.5 * skewSymmetric(v);
+    if (H2) *H2 = I_3x3 - 0.5 * W;
+    return v - 0.5 * Wv;
+  } else {
+    // WWv = omega x (omega x * v), with Jacobian doubleCrossJacobian
+    const Vector3 WWv = gtsam::cross(omega, Wv);
+    if (H1) {
+      // 1x3 Jacobians of B and C with respect to theta
+      const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose();
+      const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose();
+      *H1 = -Wv * HB + B * skewSymmetric(v) +
+            C * doubleCrossJacobian(omega, v) + WWv * HC;
     }
+    if (H2) *H2 = rightJacobian();
+    return v - B * Wv + C * WWv;
   }
-  if (H2) *H2 = rightJacobian_;
-  return rightJacobian_ * v;
 }
 
 Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                   OptionalJacobian<3, 3> H2) const {
-  const Matrix3 invDexp = rightJacobian_.inverse();
+  const Matrix3 invDexp = rightJacobian().inverse();
   const Vector3 c = invDexp * v;
   if (H1) {
     Matrix3 D_dexpv_omega;
@@ -132,6 +143,23 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
   return c;
 }
 
+Matrix3 DexpFunctor::leftJacobian() const {
+  if (nearZero) {
+    return I_3x3 + 0.5 * W;  // + one_sixth * WW;
+  } else {
+    return I_3x3 + B * W + C * WW;
+  }
+}
+
+Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
+                                       OptionalJacobian<3, 3> H1,
+                                       OptionalJacobian<3, 3> H2) const {
+  const Matrix3 Jw = leftJacobian();
+  if (H1) H1->setZero();
+  if (H2) *H2 = Jw;
+  return Jw * v;
+}
+
 }  // namespace so3
 
 //******************************************************************************
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index 8d4401c31..bd54f0cc4 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -26,7 +26,6 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/dllexport.h>
 
-#include <cmath>
 #include <vector>
 
 namespace gtsam {
@@ -160,7 +159,6 @@ class DexpFunctor : public ExpmapFunctor {
   static constexpr double one_sixth = 1.0 / 6.0;
   const Vector3 omega;
   double C;  // Ethan Eade's C constant
-  Matrix3 rightJacobian_;
 
  public:
   /// Constructor with element of Lie algebra so(3)
@@ -172,8 +170,11 @@ class DexpFunctor : public ExpmapFunctor {
   // Information Theory, and Lie Groups", Volume 2, 2008.
   //   expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
   // This maps a perturbation v in the tangent space to
-  // a perturbation on the manifold Expmap(dexp * v) */
-  const Matrix3& dexp() const { return rightJacobian_; }
+  // a perturbation on the manifold Expmap(dexp * v)
+  GTSAM_EXPORT Matrix3 rightJacobian() const;
+
+  /// differential of expmap == right Jacobian
+  GTSAM_EXPORT Matrix3 dexp() const { return rightJacobian(); }
 
   /// Multiplies with dexp(), with optional derivatives
   GTSAM_EXPORT Vector3 applyDexp(const Vector3& v,
@@ -186,8 +187,12 @@ class DexpFunctor : public ExpmapFunctor {
                                     OptionalJacobian<3, 3> H2 = {}) const;
 
   // Compute the left Jacobian for Exponential map in SO(3)
-  // Note precomputed, as not used as much
-  Matrix3 leftJacobian() const;
+  GTSAM_EXPORT Matrix3 leftJacobian() const;
+
+  /// Multiplies with leftJacobian(), with optional derivatives
+  GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v,
+                                         OptionalJacobian<3, 3> H1 = {},
+                                         OptionalJacobian<3, 3> H2 = {}) const;
 };
 }  //  namespace so3
 

From c7f651d98db1f0a7d3bea7b1a047cd1e4f93c7e1 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 00:50:38 -0500
Subject: [PATCH 06/14] applyLeftJacobian

---
 gtsam/geometry/SO3.cpp           | 67 +++++++++++++++++++++-----------
 gtsam/geometry/SO3.h             |  6 +++
 gtsam/geometry/tests/testSO3.cpp | 24 ++++++++++++
 3 files changed, 74 insertions(+), 23 deletions(-)

diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index bc2e1e6ec..13600f884 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -26,6 +26,7 @@
 
 #include <Eigen/SVD>
 #include <cmath>
+#include <iostream>
 #include <limits>
 
 namespace gtsam {
@@ -98,35 +99,47 @@ Matrix3 DexpFunctor::rightJacobian() const {
   }
 }
 
-// Derivative of w x w x v in w:
-static Matrix3 doubleCrossJacobian(const Vector3& w, const Vector3& v) {
-  return v.dot(w) * I_3x3 + w * v.transpose() - 2 * v * w.transpose();
+Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const {
+  // Wv = omega x * v
+  const Vector3 Wv = gtsam::cross(omega, v);
+  if (H) {
+    // 1x3 Jacobian of B with respect to omega
+    const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose();
+    // Apply product rule:
+    *H = Wv * HB - B * skewSymmetric(v);
+  }
+  return B * Wv;
+}
+
+Vector3 DexpFunctor::doubleCross(const Vector3& v,
+                                 OptionalJacobian<3, 3> H) const {
+  // WWv = omega x (omega x * v)
+  Matrix3 doubleCrossJacobian;
+  const Vector3 WWv =
+      gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr);
+  if (H) {
+    // 1x3 Jacobian of C with respect to omega
+    const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose();
+    // Apply product rule:
+    *H = WWv * HC + C * doubleCrossJacobian;
+  }
+  return C * WWv;
 }
 
 // Multiplies v with left Jacobian through vector operations only.
-// When Jacobian H1 wrt omega is requested, product rule is invoked twice, once
-// for (B * Wv) and (C * WWv). The Jacobian H2 wrt v is just the right Jacobian.
 Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                OptionalJacobian<3, 3> H2) const {
-  // Wv = omega x * v, with Jacobian -V in w
-  const Vector3 Wv = gtsam::cross(omega, v);
-
   if (nearZero) {
     if (H1) *H1 = 0.5 * skewSymmetric(v);
     if (H2) *H2 = I_3x3 - 0.5 * W;
-    return v - 0.5 * Wv;
+    return v - 0.5 * gtsam::cross(omega, v);
   } else {
-    // WWv = omega x (omega x * v), with Jacobian doubleCrossJacobian
-    const Vector3 WWv = gtsam::cross(omega, Wv);
-    if (H1) {
-      // 1x3 Jacobians of B and C with respect to theta
-      const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose();
-      const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose();
-      *H1 = -Wv * HB + B * skewSymmetric(v) +
-            C * doubleCrossJacobian(omega, v) + WWv * HC;
-    }
+    Matrix3 D_BWv_omega, D_CWWv_omega;
+    const Vector3 BWv = cross(v, D_BWv_omega);
+    const Vector3 CWWv = doubleCross(v, D_CWWv_omega);
+    if (H1) *H1 = - D_BWv_omega + D_CWWv_omega;
     if (H2) *H2 = rightJacobian();
-    return v - B * Wv + C * WWv;
+    return v - BWv + CWWv;
   }
 }
 
@@ -154,10 +167,18 @@ Matrix3 DexpFunctor::leftJacobian() const {
 Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
                                        OptionalJacobian<3, 3> H1,
                                        OptionalJacobian<3, 3> H2) const {
-  const Matrix3 Jw = leftJacobian();
-  if (H1) H1->setZero();
-  if (H2) *H2 = Jw;
-  return Jw * v;
+  if (nearZero) {
+    if (H1) *H1 = - 0.5 * skewSymmetric(v);
+    if (H2) *H2 = I_3x3 + 0.5 * W;
+    return v + 0.5 * gtsam::cross(omega, v);
+  } else {
+    Matrix3 D_BWv_omega, D_CWWv_omega;
+    const Vector3 BWv = cross(v, D_BWv_omega);
+    const Vector3 CWWv = doubleCross(v, D_CWWv_omega);
+    if (H1) *H1 = D_BWv_omega + D_CWWv_omega;
+    if (H2) *H2 = leftJacobian();
+    return v + BWv + CWWv;
+  }
 }
 
 }  // namespace so3
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index bd54f0cc4..8766fecbe 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -160,6 +160,12 @@ class DexpFunctor : public ExpmapFunctor {
   const Vector3 omega;
   double C;  // Ethan Eade's C constant
 
+  /// Computes B * (omega x v).
+  Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
+
+  /// Computes C * (omega x (omega x v)).
+  Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
+
  public:
   /// Constructor with element of Lie algebra so(3)
   GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega,
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index 27c143d31..8773e287a 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -19,6 +19,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/testLie.h>
 #include <gtsam/geometry/SO3.h>
+#include <iostream>
 
 using namespace std::placeholders;
 using namespace std;
@@ -326,6 +327,29 @@ TEST(SO3, ApplyDexp) {
   }
 }
 
+//******************************************************************************
+TEST(SO3, ApplyLeftJacobian) {
+  Matrix aH1, aH2;
+  for (bool nearZeroApprox : {false, true}) {
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
+        [=](const Vector3& omega, const Vector3& v) {
+          return so3::DexpFunctor(omega, nearZeroApprox).applyLeftJacobian(v);
+        };
+    for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
+                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
+      so3::DexpFunctor local(omega, nearZeroApprox);
+      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
+                        Vector3(0.4, 0.3, 0.2)}) {
+        CHECK(assert_equal(Vector3(local.leftJacobian() * v),
+                            local.applyLeftJacobian(v, aH1, aH2)));
+        CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1));
+        CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2));
+        CHECK(assert_equal(local.leftJacobian(), aH2));
+      }
+    }
+  }
+}
+
 //******************************************************************************
 TEST(SO3, ApplyInvDexp) {
   Matrix aH1, aH2;

From 82e13806035327385d66b13bbbe36bb1d91b87dd Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 01:53:02 -0500
Subject: [PATCH 07/14] Cleanup

---
 gtsam/geometry/Pose3.cpp            | 30 ++++++++++++-----------------
 gtsam/geometry/tests/testPoint3.cpp |  2 --
 gtsam/geometry/tests/testSO3.cpp    |  1 -
 3 files changed, 12 insertions(+), 21 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 16ea55665..d87e3164c 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -168,14 +168,14 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
   const Vector3 w = xi.head<3>(), v = xi.tail<3>();
 
   // Compute rotation using Expmap
-  Rot3 R = Rot3::Expmap(w);
+  Matrix3 Jw;
+  Rot3 R = Rot3::Expmap(w, Hxi ? &Jw : nullptr);
 
   // Compute translation and optionally its Jacobian in w
   Matrix3 Q;
   const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R);
 
   if (Hxi) {
-    const Matrix3 Jw = Rot3::ExpmapDerivative(w);
     *Hxi << Jw, Z_3x3,
              Q, Jw;
   }
@@ -246,34 +246,27 @@ class ExpmapFunctor : public so3::DexpFunctor {
   static constexpr double one_one_hundred_twentieth = 1.0 / 120.0;
 
  public:
-  ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false,
-                bool includeHigherOrder = false)
-      : so3::DexpFunctor(omega, nearZeroApprox),
-        includeHigherOrder(includeHigherOrder) {}
+  ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
+      : so3::DexpFunctor(omega, nearZeroApprox) {}
 
+  // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative
+  // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand
+  // how to compute mess below from its derivatives in w and v.
   Matrix3 computeQ(const Vector3& v) const {
     const Matrix3 V = skewSymmetric(v);
     const Matrix3 WVW = W * V * W;
 
     if (!nearZero) {
       // 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 {
-      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;
+      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);
     }
   }
-
- private:
-  bool includeHigherOrder;
 };
 }  // namespace pose3
 
@@ -298,7 +291,8 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
   if (nearZero) {
     return v + 0.5 * w.cross(v);
   } else {
-    // Geometric intuition and faster than using functor.
+    // NOTE(Frank): t can also be computed by calling applyLeftJacobian(v), but
+    // formulas below convey geometric insight and creating functor is not free.
     Vector3 t_parallel = w * w.dot(v);  // translation parallel to axis
     Vector3 w_cross_v = w.cross(v);     // translation orthogonal to axis
     Rot3 rotation = R.value_or(Rot3::Expmap(w));
diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp
index bb49486a8..b3b751ea1 100644
--- a/gtsam/geometry/tests/testPoint3.cpp
+++ b/gtsam/geometry/tests/testPoint3.cpp
@@ -18,8 +18,6 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Point3.h>
-#include "gtsam/base/Matrix.h"
-#include "gtsam/base/OptionalJacobian.h"
 
 using namespace std::placeholders;
 using namespace gtsam;
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index 8773e287a..1e105ceca 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -19,7 +19,6 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/testLie.h>
 #include <gtsam/geometry/SO3.h>
-#include <iostream>
 
 using namespace std::placeholders;
 using namespace std;

From fa1249bf14dc815402061bc826b7adbfc2db620f Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 17:53:42 -0500
Subject: [PATCH 08/14] Add export

---
 gtsam/geometry/Point3.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 0fa53b1b2..d5c32816e 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -61,9 +61,9 @@ GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
                           OptionalJacobian<3, 3> H_q = {});
 
 /// double cross product @return p x (p x q)
-Point3 doubleCross(const Point3& p, const Point3& q,
-                   OptionalJacobian<3, 3> H1 = {},
-                   OptionalJacobian<3, 3> H2 = {});
+GTSAM_EXPORT Point3 doubleCross(const Point3& p, const Point3& q,
+                                OptionalJacobian<3, 3> H1 = {},
+                                OptionalJacobian<3, 3> H2 = {});
 
 /// dot product
 GTSAM_EXPORT double dot(const Point3& p, const Point3& q,

From e96d8487e493d9cce4f60a09ab7ec20c2412210e Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 17:54:30 -0500
Subject: [PATCH 09/14] New constants for cross and doubleCross

---
 gtsam/geometry/SO3.cpp           | 65 ++++++++++++------------
 gtsam/geometry/SO3.h             | 22 ++++++---
 gtsam/geometry/tests/testSO3.cpp | 84 +++++++++++++++++++++++---------
 3 files changed, 108 insertions(+), 63 deletions(-)

diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index 13600f884..e341911b9 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -51,49 +51,51 @@ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
 }
 
 void ExpmapFunctor::init(bool nearZeroApprox) {
-  WW = W * W;
   nearZero =
       nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
   if (!nearZero) {
-    sin_theta = std::sin(theta);
-    const double s2 = std::sin(theta / 2.0);
-    one_minus_cos = 2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
+    const double sin_theta = std::sin(theta);
     A = sin_theta / theta;
+    const double s2 = std::sin(theta / 2.0);
+    const double one_minus_cos =
+        2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
     B = one_minus_cos / theta2;
+  } else {
+    // Limits as theta -> 0:
+    A = 1.0;
+    B = 0.5;
   }
 }
 
 ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
-    : theta2(omega.dot(omega)), theta(std::sqrt(theta2)) {
-  const double wx = omega.x(), wy = omega.y(), wz = omega.z();
-  W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
+    : theta2(omega.dot(omega)),
+      theta(std::sqrt(theta2)),
+      W(skewSymmetric(omega)),
+      WW(W * W) {
   init(nearZeroApprox);
 }
 
 ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
                              bool nearZeroApprox)
-    : theta2(angle * angle), theta(angle) {
-  const double ax = axis.x(), ay = axis.y(), az = axis.z();
-  W << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
-  W *= angle;
+    : theta2(angle * angle),
+      theta(angle),
+      W(skewSymmetric(axis * angle)),
+      WW(W * W) {
   init(nearZeroApprox);
 }
 
-SO3 ExpmapFunctor::expmap() const {
-  if (nearZero)
-    return SO3(I_3x3 + W + 0.5 * WW);
-  else
-    return SO3(I_3x3 + A * W + B * WW);
-}
+SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }
 
 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
-  C = (1 - A) / theta2;
+  C = nearZero ? one_sixth : (1 - A) / theta2;
+  D = nearZero ? _one_twelfth : (A - 2.0 * B) / theta2;
+  E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2;
 }
 
 Matrix3 DexpFunctor::rightJacobian() const {
   if (nearZero) {
-    return I_3x3 - 0.5 * W;  // + one_sixth * WW;
+    return I_3x3 - B * W;  // + C * WW;
   } else {
     return I_3x3 - B * W + C * WW;
   }
@@ -103,10 +105,10 @@ Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const {
   // Wv = omega x * v
   const Vector3 Wv = gtsam::cross(omega, v);
   if (H) {
-    // 1x3 Jacobian of B with respect to omega
-    const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose();
     // Apply product rule:
-    *H = Wv * HB - B * skewSymmetric(v);
+    // D * omega.transpose() is 1x3 Jacobian of B with respect to omega
+    // - skewSymmetric(v) is 3x3 Jacobian of B gtsam::cross(omega, v)
+    *H = Wv * D * omega.transpose() - B * skewSymmetric(v);
   }
   return B * Wv;
 }
@@ -118,10 +120,10 @@ Vector3 DexpFunctor::doubleCross(const Vector3& v,
   const Vector3 WWv =
       gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr);
   if (H) {
-    // 1x3 Jacobian of C with respect to omega
-    const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose();
     // Apply product rule:
-    *H = WWv * HC + C * doubleCrossJacobian;
+    // E * omega.transpose() is 1x3 Jacobian of C with respect to omega
+    // doubleCrossJacobian is 3x3 Jacobian of C gtsam::doubleCross(omega, v)
+    *H = WWv * E * omega.transpose() + C * doubleCrossJacobian;
   }
   return C * WWv;
 }
@@ -134,12 +136,12 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
     if (H2) *H2 = I_3x3 - 0.5 * W;
     return v - 0.5 * gtsam::cross(omega, v);
   } else {
-    Matrix3 D_BWv_omega, D_CWWv_omega;
+  Matrix3 D_BWv_omega, D_CWWv_omega;
     const Vector3 BWv = cross(v, D_BWv_omega);
     const Vector3 CWWv = doubleCross(v, D_CWWv_omega);
     if (H1) *H1 = - D_BWv_omega + D_CWWv_omega;
-    if (H2) *H2 = rightJacobian();
-    return v - BWv + CWWv;
+  if (H2) *H2 = rightJacobian();
+  return v - BWv + CWWv;
   }
 }
 
@@ -219,12 +221,7 @@ SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
 template <>
 GTSAM_EXPORT
 Matrix3 SO3::Hat(const Vector3& xi) {
-  // skew symmetric matrix X = xi^
-  Matrix3 Y = Z_3x3;
-  Y(0, 1) = -xi(2);
-  Y(0, 2) = +xi(1);
-  Y(1, 2) = -xi(0);
-  return Y - Y.transpose();
+  return skewSymmetric(xi);
 }
 
 //******************************************************************************
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index 8766fecbe..6403c3ae0 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -134,11 +134,12 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
 /// Functor implementing Exponential map
 class GTSAM_EXPORT ExpmapFunctor {
  protected:
-  const double theta2;
-  Matrix3 W, WW;
-  double A, B; // Ethan Eade's constants
+  const double theta2, theta;
+  const Matrix3 W, WW;
   bool nearZero;
-  double theta, sin_theta, one_minus_cos;  // only defined if !nearZero
+  // Ethan Eade's constants:
+  double A; // A = sin(theta) / theta or 1 for theta->0
+  double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0
 
   void init(bool nearZeroApprox = false);
 
@@ -156,17 +157,19 @@ class GTSAM_EXPORT ExpmapFunctor {
 /// Functor that implements Exponential map *and* its derivatives
 class DexpFunctor : public ExpmapFunctor {
  protected:
-  static constexpr double one_sixth = 1.0 / 6.0;
   const Vector3 omega;
-  double C;  // Ethan Eade's C constant
+  double C;  // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0
+  // Constants used in cross and doubleCross
+  double D;  // (A - 2.0 * B) / theta2 or -1/12 for theta->0
+  double E;  // (B - 3.0 * C) / theta2 or -1/60 for theta->0
 
+ public:
   /// Computes B * (omega x v).
   Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
 
   /// Computes C * (omega x (omega x v)).
   Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
 
- public:
   /// Constructor with element of Lie algebra so(3)
   GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega,
                                     bool nearZeroApprox = false);
@@ -199,6 +202,11 @@ class DexpFunctor : public ExpmapFunctor {
   GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v,
                                          OptionalJacobian<3, 3> H1 = {},
                                          OptionalJacobian<3, 3> H2 = {}) const;
+
+ protected:
+  static constexpr double one_sixth = 1.0 / 6.0;
+  static constexpr double _one_twelfth = -1.0 / 12.0;
+  static constexpr double _one_sixtieth = -1.0 / 60.0;
 };
 }  //  namespace so3
 
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index 1e105ceca..e8de2e93d 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -303,19 +303,63 @@ TEST(SO3, JacobianLogmap) {
   EXPECT(assert_equal(Jexpected, Jactual));
 }
 
+namespace test_cases {
+std::vector<Vector3> nearZeros{
+    {0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}};
+std::vector<Vector3> omegas{
+    {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}};
+std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}};
+}  // namespace test_cases
+
+//******************************************************************************
+TEST(SO3, Cross) {
+  Matrix aH1;
+  for (bool nearZero : {true, false}) {
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
+        [=](const Vector3& omega, const Vector3& v) {
+          return so3::DexpFunctor(omega, nearZero).cross(v);
+        };
+    const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas;
+    for (Vector3 omega : omegas) {
+      so3::DexpFunctor local(omega, nearZero);
+      for (Vector3 v : test_cases::vs) {
+        local.cross(v, aH1);
+        EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
+      }
+    }
+  }
+}
+
+//******************************************************************************
+TEST(SO3, DoubleCross) {
+  Matrix aH1;
+  for (bool nearZero : {true, false}) {
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
+        [=](const Vector3& omega, const Vector3& v) {
+          return so3::DexpFunctor(omega, nearZero).doubleCross(v);
+        };
+    const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas;
+    for (Vector3 omega : omegas) {
+      so3::DexpFunctor local(omega, nearZero);
+      for (Vector3 v : test_cases::vs) {
+        local.doubleCross(v, aH1);
+        EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
+      }
+    }
+  }
+}
+
 //******************************************************************************
 TEST(SO3, ApplyDexp) {
   Matrix aH1, aH2;
-  for (bool nearZeroApprox : {true, false}) {
+  for (bool nearZero : {true, false}) {
     std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
+          return so3::DexpFunctor(omega, nearZero).applyDexp(v);
         };
-    for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
-                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
-      so3::DexpFunctor local(omega, nearZeroApprox);
-      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
-                        Vector3(0.4, 0.3, 0.2)}) {
+    for (Vector3 omega : test_cases::omegas) {
+      so3::DexpFunctor local(omega, nearZero);
+      for (Vector3 v : test_cases::vs) {
         EXPECT(assert_equal(Vector3(local.dexp() * v),
                             local.applyDexp(v, aH1, aH2)));
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
@@ -329,18 +373,16 @@ TEST(SO3, ApplyDexp) {
 //******************************************************************************
 TEST(SO3, ApplyLeftJacobian) {
   Matrix aH1, aH2;
-  for (bool nearZeroApprox : {false, true}) {
+  for (bool nearZero : {true, false}) {
     std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZeroApprox).applyLeftJacobian(v);
+          return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v);
         };
-    for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
-                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
-      so3::DexpFunctor local(omega, nearZeroApprox);
-      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
-                        Vector3(0.4, 0.3, 0.2)}) {
+    for (Vector3 omega : test_cases::omegas) {
+      so3::DexpFunctor local(omega, nearZero);
+      for (Vector3 v : test_cases::vs) {
         CHECK(assert_equal(Vector3(local.leftJacobian() * v),
-                            local.applyLeftJacobian(v, aH1, aH2)));
+                           local.applyLeftJacobian(v, aH1, aH2)));
         CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1));
         CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2));
         CHECK(assert_equal(local.leftJacobian(), aH2));
@@ -352,17 +394,15 @@ TEST(SO3, ApplyLeftJacobian) {
 //******************************************************************************
 TEST(SO3, ApplyInvDexp) {
   Matrix aH1, aH2;
-  for (bool nearZeroApprox : {true, false}) {
+  for (bool nearZero : {true, false}) {
     std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
+          return so3::DexpFunctor(omega, nearZero).applyInvDexp(v);
         };
-    for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
-                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
-      so3::DexpFunctor local(omega, nearZeroApprox);
+    for (Vector3 omega : test_cases::omegas) {
+      so3::DexpFunctor local(omega, nearZero);
       Matrix invDexp = local.dexp().inverse();
-      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
-                        Vector3(0.4, 0.3, 0.2)}) {
+      for (Vector3 v : test_cases::vs) {
         EXPECT(assert_equal(Vector3(invDexp * v),
                             local.applyInvDexp(v, aH1, aH2)));
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));

From 9b0f3c3b38f9eaaa2cc2e9be24a0fa94a3e5c04b Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 17:55:22 -0500
Subject: [PATCH 10/14] Incredible simplification using E and F

---
 gtsam/geometry/Pose3.cpp | 21 ++++++++-------------
 1 file changed, 8 insertions(+), 13 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index d87e3164c..7faf433bb 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -243,11 +243,15 @@ namespace pose3 {
 class ExpmapFunctor : public so3::DexpFunctor {
  private:
   static constexpr double one_twenty_fourth = 1.0 / 24.0;
-  static constexpr double one_one_hundred_twentieth = 1.0 / 120.0;
+
+  // Constant used in computeQ
+  double F;  // (B - 0.5) / theta2 or -1/24 for theta->0
 
  public:
   ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
-      : so3::DexpFunctor(omega, nearZeroApprox) {}
+      : so3::DexpFunctor(omega, nearZeroApprox) {
+        F = nearZero ? - one_twenty_fourth : (B - 0.5) / theta2;
+      }
 
   // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative
   // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand
@@ -255,17 +259,8 @@ class ExpmapFunctor : public so3::DexpFunctor {
   Matrix3 computeQ(const Vector3& v) const {
     const Matrix3 V = skewSymmetric(v);
     const Matrix3 WVW = W * V * W;
-
-    if (!nearZero) {
-      // Simplified from closed-form formula in Barfoot14tro eq. (102)
-      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);
-    }
+    return -0.5 * V + C * (W * V + V * W - WVW) +
+           F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW);
   }
 };
 }  // namespace pose3

From a32bb7bf096d6ccd7432563de28b2fdfd96deb39 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 23:39:45 -0500
Subject: [PATCH 11/14] Export

---
 gtsam/geometry/Pose3.cpp | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 7faf433bb..f317cd4e8 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -240,18 +240,16 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
 
 /* ************************************************************************* */
 namespace pose3 {
-class ExpmapFunctor : public so3::DexpFunctor {
- private:
-  static constexpr double one_twenty_fourth = 1.0 / 24.0;
-
+class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
+ protected:
   // Constant used in computeQ
   double F;  // (B - 0.5) / theta2 or -1/24 for theta->0
 
  public:
   ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
       : so3::DexpFunctor(omega, nearZeroApprox) {
-        F = nearZero ? - one_twenty_fourth : (B - 0.5) / theta2;
-      }
+    F = nearZero ? _one_twenty_fourth : (B - 0.5) / theta2;
+  }
 
   // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative
   // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand
@@ -262,6 +260,9 @@ class ExpmapFunctor : public so3::DexpFunctor {
     return -0.5 * V + C * (W * V + V * W - WVW) +
            F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW);
   }
+
+ protected:
+  static constexpr double _one_twenty_fourth = - 1.0 / 24.0;
 };
 }  // namespace pose3
 

From bcfb7d8444ae4fd1f8883c27e7f81a6b559c2a27 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 23:42:59 -0500
Subject: [PATCH 12/14] Fix test cases

---
 gtsam/geometry/tests/testSO3.cpp | 50 +++++++++++++++-----------------
 1 file changed, 24 insertions(+), 26 deletions(-)

diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index e8de2e93d..3ecabe84b 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -304,26 +304,25 @@ TEST(SO3, JacobianLogmap) {
 }
 
 namespace test_cases {
-std::vector<Vector3> nearZeros{
-    {0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}};
-std::vector<Vector3> omegas{
+std::vector<Vector3> small{{0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}};
+std::vector<Vector3> large{
     {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}};
+auto omegas = [](bool nearZero) { return nearZero ? small : large; };
 std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}};
 }  // namespace test_cases
 
 //******************************************************************************
-TEST(SO3, Cross) {
+TEST(SO3, CrossB) {
   Matrix aH1;
   for (bool nearZero : {true, false}) {
     std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZero).cross(v);
+          return so3::DexpFunctor(omega, nearZero).crossB(v);
         };
-    const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas;
-    for (Vector3 omega : omegas) {
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
       so3::DexpFunctor local(omega, nearZero);
-      for (Vector3 v : test_cases::vs) {
-        local.cross(v, aH1);
+      for (const Vector3& v : test_cases::vs) {
+        local.crossB(v, aH1);
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
       }
     }
@@ -331,18 +330,17 @@ TEST(SO3, Cross) {
 }
 
 //******************************************************************************
-TEST(SO3, DoubleCross) {
+TEST(SO3, DoubleCrossC) {
   Matrix aH1;
   for (bool nearZero : {true, false}) {
     std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZero).doubleCross(v);
+          return so3::DexpFunctor(omega, nearZero).doubleCrossC(v);
         };
-    const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas;
-    for (Vector3 omega : omegas) {
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
       so3::DexpFunctor local(omega, nearZero);
-      for (Vector3 v : test_cases::vs) {
-        local.doubleCross(v, aH1);
+      for (const Vector3& v : test_cases::vs) {
+        local.doubleCrossC(v, aH1);
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
       }
     }
@@ -357,9 +355,9 @@ TEST(SO3, ApplyDexp) {
         [=](const Vector3& omega, const Vector3& v) {
           return so3::DexpFunctor(omega, nearZero).applyDexp(v);
         };
-    for (Vector3 omega : test_cases::omegas) {
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
       so3::DexpFunctor local(omega, nearZero);
-      for (Vector3 v : test_cases::vs) {
+      for (const Vector3& v : test_cases::vs) {
         EXPECT(assert_equal(Vector3(local.dexp() * v),
                             local.applyDexp(v, aH1, aH2)));
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
@@ -378,14 +376,14 @@ TEST(SO3, ApplyLeftJacobian) {
         [=](const Vector3& omega, const Vector3& v) {
           return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v);
         };
-    for (Vector3 omega : test_cases::omegas) {
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
       so3::DexpFunctor local(omega, nearZero);
-      for (Vector3 v : test_cases::vs) {
-        CHECK(assert_equal(Vector3(local.leftJacobian() * v),
-                           local.applyLeftJacobian(v, aH1, aH2)));
-        CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1));
-        CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2));
-        CHECK(assert_equal(local.leftJacobian(), aH2));
+      for (const Vector3& v : test_cases::vs) {
+        EXPECT(assert_equal(Vector3(local.leftJacobian() * v),
+                            local.applyLeftJacobian(v, aH1, aH2)));
+        EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
+        EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
+        EXPECT(assert_equal(local.leftJacobian(), aH2));
       }
     }
   }
@@ -399,10 +397,10 @@ TEST(SO3, ApplyInvDexp) {
         [=](const Vector3& omega, const Vector3& v) {
           return so3::DexpFunctor(omega, nearZero).applyInvDexp(v);
         };
-    for (Vector3 omega : test_cases::omegas) {
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
       so3::DexpFunctor local(omega, nearZero);
       Matrix invDexp = local.dexp().inverse();
-      for (Vector3 v : test_cases::vs) {
+      for (const Vector3& v : test_cases::vs) {
         EXPECT(assert_equal(Vector3(invDexp * v),
                             local.applyInvDexp(v, aH1, aH2)));
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));

From d547fe2ec137ce3f6727d84bd9f1e5b9d7508867 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 15 Dec 2024 23:43:59 -0500
Subject: [PATCH 13/14] Remove all nearZero paths

---
 gtsam/geometry/SO3.cpp | 52 ++++++++++--------------------------------
 gtsam/geometry/SO3.h   | 50 +++++++++++++++++++---------------------
 2 files changed, 35 insertions(+), 67 deletions(-)

diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index e341911b9..ef1fa374b 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -93,15 +93,7 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
   E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2;
 }
 
-Matrix3 DexpFunctor::rightJacobian() const {
-  if (nearZero) {
-    return I_3x3 - B * W;  // + C * WW;
-  } else {
-    return I_3x3 - B * W + C * WW;
-  }
-}
-
-Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const {
+Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const {
   // Wv = omega x * v
   const Vector3 Wv = gtsam::cross(omega, v);
   if (H) {
@@ -113,8 +105,8 @@ Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const {
   return B * Wv;
 }
 
-Vector3 DexpFunctor::doubleCross(const Vector3& v,
-                                 OptionalJacobian<3, 3> H) const {
+Vector3 DexpFunctor::doubleCrossC(const Vector3& v,
+                                  OptionalJacobian<3, 3> H) const {
   // WWv = omega x (omega x * v)
   Matrix3 doubleCrossJacobian;
   const Vector3 WWv =
@@ -131,18 +123,12 @@ Vector3 DexpFunctor::doubleCross(const Vector3& v,
 // Multiplies v with left Jacobian through vector operations only.
 Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                OptionalJacobian<3, 3> H2) const {
-  if (nearZero) {
-    if (H1) *H1 = 0.5 * skewSymmetric(v);
-    if (H2) *H2 = I_3x3 - 0.5 * W;
-    return v - 0.5 * gtsam::cross(omega, v);
-  } else {
   Matrix3 D_BWv_omega, D_CWWv_omega;
-    const Vector3 BWv = cross(v, D_BWv_omega);
-    const Vector3 CWWv = doubleCross(v, D_CWWv_omega);
-    if (H1) *H1 = - D_BWv_omega + D_CWWv_omega;
+  const Vector3 BWv = crossB(v, D_BWv_omega);
+  const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega);
+  if (H1) *H1 = -D_BWv_omega + D_CWWv_omega;
   if (H2) *H2 = rightJacobian();
   return v - BWv + CWWv;
-  }
 }
 
 Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
@@ -158,29 +144,15 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
   return c;
 }
 
-Matrix3 DexpFunctor::leftJacobian() const {
-  if (nearZero) {
-    return I_3x3 + 0.5 * W;  // + one_sixth * WW;
-  } else {
-    return I_3x3 + B * W + C * WW;
-  }
-}
-
 Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
                                        OptionalJacobian<3, 3> H1,
                                        OptionalJacobian<3, 3> H2) const {
-  if (nearZero) {
-    if (H1) *H1 = - 0.5 * skewSymmetric(v);
-    if (H2) *H2 = I_3x3 + 0.5 * W;
-    return v + 0.5 * gtsam::cross(omega, v);
-  } else {
-    Matrix3 D_BWv_omega, D_CWWv_omega;
-    const Vector3 BWv = cross(v, D_BWv_omega);
-    const Vector3 CWWv = doubleCross(v, D_CWWv_omega);
-    if (H1) *H1 = D_BWv_omega + D_CWWv_omega;
-    if (H2) *H2 = leftJacobian();
-    return v + BWv + CWWv;
-  }
+  Matrix3 D_BWv_omega, D_CWWv_omega;
+  const Vector3 BWv = crossB(v, D_BWv_omega);
+  const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega);
+  if (H1) *H1 = D_BWv_omega + D_CWWv_omega;
+  if (H2) *H2 = leftJacobian();
+  return v + BWv + CWWv;
 }
 
 }  // namespace so3
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index 6403c3ae0..efb947e73 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -155,7 +155,7 @@ class GTSAM_EXPORT ExpmapFunctor {
 };
 
 /// Functor that implements Exponential map *and* its derivatives
-class DexpFunctor : public ExpmapFunctor {
+class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
  protected:
   const Vector3 omega;
   double C;  // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0
@@ -164,15 +164,8 @@ class DexpFunctor : public ExpmapFunctor {
   double E;  // (B - 3.0 * C) / theta2 or -1/60 for theta->0
 
  public:
-  /// Computes B * (omega x v).
-  Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
-
-  /// Computes C * (omega x (omega x v)).
-  Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
-
   /// Constructor with element of Lie algebra so(3)
-  GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega,
-                                    bool nearZeroApprox = false);
+  explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
 
   // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
   // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
@@ -180,28 +173,31 @@ class DexpFunctor : public ExpmapFunctor {
   //   expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
   // This maps a perturbation v in the tangent space to
   // a perturbation on the manifold Expmap(dexp * v)
-  GTSAM_EXPORT Matrix3 rightJacobian() const;
-
-  /// differential of expmap == right Jacobian
-  GTSAM_EXPORT Matrix3 dexp() const { return rightJacobian(); }
-
-  /// Multiplies with dexp(), with optional derivatives
-  GTSAM_EXPORT Vector3 applyDexp(const Vector3& v,
-                                 OptionalJacobian<3, 3> H1 = {},
-                                 OptionalJacobian<3, 3> H2 = {}) const;
-
-  /// Multiplies with dexp().inverse(), with optional derivatives
-  GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v,
-                                    OptionalJacobian<3, 3> H1 = {},
-                                    OptionalJacobian<3, 3> H2 = {}) const;
+  Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; }
 
   // Compute the left Jacobian for Exponential map in SO(3)
-  GTSAM_EXPORT Matrix3 leftJacobian() const;
+  Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; }
+
+  /// differential of expmap == right Jacobian
+  inline Matrix3 dexp() const { return rightJacobian(); }
+
+  /// Computes B * (omega x v).
+  Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
+
+  /// Computes C * (omega x (omega x v)).
+  Vector3 doubleCrossC(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
+
+  /// Multiplies with dexp(), with optional derivatives
+  Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
+                    OptionalJacobian<3, 3> H2 = {}) const;
+
+  /// Multiplies with dexp().inverse(), with optional derivatives
+  Vector3 applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
+                       OptionalJacobian<3, 3> H2 = {}) const;
 
   /// Multiplies with leftJacobian(), with optional derivatives
-  GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v,
-                                         OptionalJacobian<3, 3> H1 = {},
-                                         OptionalJacobian<3, 3> H2 = {}) const;
+  Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
+                            OptionalJacobian<3, 3> H2 = {}) const;
 
  protected:
   static constexpr double one_sixth = 1.0 / 6.0;

From 0c1f087dba7374e13440461e260ee47086d39c31 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 16 Dec 2024 00:10:26 -0500
Subject: [PATCH 14/14] Final touches

---
 gtsam/geometry/Pose3.cpp |  7 ++-----
 gtsam/geometry/SO3.cpp   | 25 ++++++++++++-------------
 gtsam/geometry/SO3.h     | 27 ++++++++++++---------------
 3 files changed, 26 insertions(+), 33 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index f317cd4e8..5f0e8e3ce 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -240,12 +240,10 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
 
 /* ************************************************************************* */
 namespace pose3 {
-class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
- protected:
+struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
   // Constant used in computeQ
   double F;  // (B - 0.5) / theta2 or -1/24 for theta->0
 
- public:
   ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
       : so3::DexpFunctor(omega, nearZeroApprox) {
     F = nearZero ? _one_twenty_fourth : (B - 0.5) / theta2;
@@ -253,7 +251,7 @@ class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
 
   // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative
   // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand
-  // how to compute mess below from its derivatives in w and v.
+  // how to compute mess below from applyLeftJacobian derivatives in w and v.
   Matrix3 computeQ(const Vector3& v) const {
     const Matrix3 V = skewSymmetric(v);
     const Matrix3 WVW = W * V * W;
@@ -261,7 +259,6 @@ class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
            F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW);
   }
 
- protected:
   static constexpr double _one_twenty_fourth = - 1.0 / 24.0;
 };
 }  // namespace pose3
diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index ef1fa374b..c54306ae5 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -26,7 +26,6 @@
 
 #include <Eigen/SVD>
 #include <cmath>
-#include <iostream>
 #include <limits>
 
 namespace gtsam {
@@ -94,7 +93,7 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
 }
 
 Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const {
-  // Wv = omega x * v
+  // Wv = omega x v
   const Vector3 Wv = gtsam::cross(omega, v);
   if (H) {
     // Apply product rule:
@@ -123,10 +122,10 @@ Vector3 DexpFunctor::doubleCrossC(const Vector3& v,
 // Multiplies v with left Jacobian through vector operations only.
 Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                OptionalJacobian<3, 3> H2) const {
-  Matrix3 D_BWv_omega, D_CWWv_omega;
-  const Vector3 BWv = crossB(v, D_BWv_omega);
-  const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega);
-  if (H1) *H1 = -D_BWv_omega + D_CWWv_omega;
+  Matrix3 D_BWv_w, D_CWWv_w;
+  const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
+  const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr);
+  if (H1) *H1 = -D_BWv_w + D_CWWv_w;
   if (H2) *H2 = rightJacobian();
   return v - BWv + CWWv;
 }
@@ -136,9 +135,9 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
   const Matrix3 invDexp = rightJacobian().inverse();
   const Vector3 c = invDexp * v;
   if (H1) {
-    Matrix3 D_dexpv_omega;
-    applyDexp(c, D_dexpv_omega);  // get derivative H of forward mapping
-    *H1 = -invDexp * D_dexpv_omega;
+    Matrix3 D_dexpv_w;
+    applyDexp(c, D_dexpv_w);  // get derivative H of forward mapping
+    *H1 = -invDexp * D_dexpv_w;
   }
   if (H2) *H2 = invDexp;
   return c;
@@ -147,10 +146,10 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
 Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
                                        OptionalJacobian<3, 3> H1,
                                        OptionalJacobian<3, 3> H2) const {
-  Matrix3 D_BWv_omega, D_CWWv_omega;
-  const Vector3 BWv = crossB(v, D_BWv_omega);
-  const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega);
-  if (H1) *H1 = D_BWv_omega + D_CWWv_omega;
+  Matrix3 D_BWv_w, D_CWWv_w;
+  const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
+  const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr);
+  if (H1) *H1 = D_BWv_w + D_CWWv_w;
   if (H2) *H2 = leftJacobian();
   return v + BWv + CWWv;
 }
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index efb947e73..f831aa426 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -132,18 +132,15 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
 // functor also implements dedicated methods to apply dexp and/or inv(dexp).
 
 /// Functor implementing Exponential map
-class GTSAM_EXPORT ExpmapFunctor {
- protected:
+struct GTSAM_EXPORT ExpmapFunctor {
   const double theta2, theta;
   const Matrix3 W, WW;
   bool nearZero;
+
   // Ethan Eade's constants:
-  double A; // A = sin(theta) / theta or 1 for theta->0
-  double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0
+  double A;  // A = sin(theta) / theta or 1 for theta->0
+  double B;  // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0
 
-  void init(bool nearZeroApprox = false);
-
- public:
   /// Constructor with element of Lie algebra so(3)
   explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
 
@@ -152,33 +149,34 @@ class GTSAM_EXPORT ExpmapFunctor {
 
   /// Rodrigues formula
   SO3 expmap() const;
+
+ protected:
+  void init(bool nearZeroApprox = false);
 };
 
 /// Functor that implements Exponential map *and* its derivatives
-class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
- protected:
+struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   const Vector3 omega;
   double C;  // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0
   // Constants used in cross and doubleCross
   double D;  // (A - 2.0 * B) / theta2 or -1/12 for theta->0
   double E;  // (B - 3.0 * C) / theta2 or -1/60 for theta->0
 
- public:
   /// Constructor with element of Lie algebra so(3)
   explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
 
   // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
   // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
   // Information Theory, and Lie Groups", Volume 2, 2008.
-  //   expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
-  // This maps a perturbation v in the tangent space to
-  // a perturbation on the manifold Expmap(dexp * v)
+  //   Expmap(xi + dxi) \approx Expmap(xi) * Expmap(dexp * dxi)
+  // This maps a perturbation dxi=(w,v) in the tangent space to
+  // a perturbation on the manifold Expmap(dexp * xi)
   Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; }
 
   // Compute the left Jacobian for Exponential map in SO(3)
   Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; }
 
-  /// differential of expmap == right Jacobian
+  /// Differential of expmap == right Jacobian
   inline Matrix3 dexp() const { return rightJacobian(); }
 
   /// Computes B * (omega x v).
@@ -199,7 +197,6 @@ class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
                             OptionalJacobian<3, 3> H2 = {}) const;
 
- protected:
   static constexpr double one_sixth = 1.0 / 6.0;
   static constexpr double _one_twelfth = -1.0 / 12.0;
   static constexpr double _one_sixtieth = -1.0 / 60.0;