Merge pull request #1932 from borglab/feature/leftRightJacobians

Left and Right SO(3) Jacobians

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();

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)
+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,
                         OptionalJacobian<1, 3> H_p = {},

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;
   }
@@ -238,14 +238,37 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
 #endif
 }
 
+/* ************************************************************************* */
+namespace pose3 {
+struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
+  // Constant used in computeQ
+  double F;  // (B - 0.5) / theta2 or -1/24 for theta->0
+
+  ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
+      : 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
+  // 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;
+    return -0.5 * V + C * (W * V + V * W - WVW) +
+           F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW);
+  }
+
+  static constexpr double _one_twenty_fourth = - 1.0 / 24.0;
+};
+}  // 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 pose3::ExpmapFunctor(w).computeQ(v);
-  return Q;
 }
 
 /* ************************************************************************* */
@@ -256,39 +279,13 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
   const double theta2 = w.dot(w);
   bool nearZero = (theta2 <= nearZeroThreshold);
 
-  if (Q) {
+  if (Q) *Q = pose3::ExpmapFunctor(w, nearZero).computeQ(v);
-    const Matrix3 V = skewSymmetric(v);
-    const Matrix3 W = skewSymmetric(w);
-    const Matrix3 WVW = W * V * W;
-    const double theta = w.norm();
 
-    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 {
+    // 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));

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;
@@ -51,85 +53,107 @@ void ExpmapFunctor::init(bool nearZeroApprox) {
   nearZero =
       nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
   if (!nearZero) {
-    sin_theta = std::sin(theta);
+    const double sin_theta = std::sin(theta);
+    A = sin_theta / theta;
     const double s2 = std::sin(theta / 2.0);
-    one_minus_cos = 2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
+    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)) {
+    : theta2(omega.dot(omega)),
-  const double wx = omega.x(), wy = omega.y(), wz = omega.z();
+      theta(std::sqrt(theta2)),
-  W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
+      W(skewSymmetric(omega)),
+      WW(W * W) {
   init(nearZeroApprox);
-  if (!nearZero) {
-    K = W / theta;
-    KK = K * K;
-  }
 }
 
 ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
                              bool nearZeroApprox)
-    : theta2(angle * angle), theta(angle) {
+    : theta2(angle * angle),
-  const double ax = axis.x(), ay = axis.y(), az = axis.z();
+      theta(angle),
-  K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
+      W(skewSymmetric(axis * angle)),
-  W = K * angle;
+      WW(W * W) {
   init(nearZeroApprox);
-  if (!nearZero) {
-    KK = K * K;
-  }
 }
 
-SO3 ExpmapFunctor::expmap() const {
+SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }
-  if (nearZero)
-    return SO3(I_3x3 + W);
-  else
-    return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
-}
 
 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
-  if (nearZero) {
+  C = nearZero ? one_sixth : (1 - A) / theta2;
-    dexp_ = I_3x3 - 0.5 * W;
+  D = nearZero ? _one_twelfth : (A - 2.0 * B) / theta2;
-  } else {
+  E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2;
-    a = one_minus_cos / theta;
-    b = 1.0 - sin_theta / theta;
-    dexp_ = I_3x3 - a * K + b * KK;
-  }
 }
 
+Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const {
+  // Wv = omega x v
+  const Vector3 Wv = gtsam::cross(omega, v);
+  if (H) {
+    // Apply product rule:
+    // 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;
+}
+
+Vector3 DexpFunctor::doubleCrossC(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) {
+    // Apply product rule:
+    // 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;
+}
+
+// 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 (H1) {
+  Matrix3 D_BWv_w, D_CWWv_w;
-    if (nearZero) {
+  const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
-      *H1 = 0.5 * skewSymmetric(v);
+  const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr);
-    } else {
+  if (H1) *H1 = -D_BWv_w + D_CWWv_w;
-      // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
+  if (H2) *H2 = rightJacobian();
-      const Vector3 Kv = K * v;
+  return v - BWv + CWWv;
-      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() -
-            skewSymmetric(Kv * b / theta) +
-            (a * I_3x3 - b * K) * skewSymmetric(v / theta);
-    }
-  }
-  if (H2) *H2 = dexp_;
-  return dexp_ * 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;
+    Matrix3 D_dexpv_w;
-    applyDexp(c, D_dexpv_omega);  // get derivative H of forward mapping
+    applyDexp(c, D_dexpv_w);  // get derivative H of forward mapping
-    *H1 = -invDexp * D_dexpv_omega;
+    *H1 = -invDexp * D_dexpv_w;
   }
   if (H2) *H2 = invDexp;
   return c;
 }
 
+Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
+                                       OptionalJacobian<3, 3> H1,
+                                       OptionalJacobian<3, 3> H2) const {
+  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;
+}
+
 }  // namespace so3
 
 //******************************************************************************
@@ -168,12 +192,7 @@ SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
 template <>
 GTSAM_EXPORT
 Matrix3 SO3::Hat(const Vector3& xi) {
-  // skew symmetric matrix X = xi^
+  return skewSymmetric(xi);
-  Matrix3 Y = Z_3x3;
-  Y(0, 1) = -xi(2);
-  Y(0, 2) = +xi(1);
-  Y(1, 2) = -xi(0);
-  return Y - Y.transpose();
 }
 
 //******************************************************************************

gtsam/geometry/SO3.h

@@ -26,7 +26,6 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/dllexport.h>
 
-#include <cmath>
 #include <vector>
 
 namespace gtsam {
@@ -133,16 +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 {
+struct GTSAM_EXPORT ExpmapFunctor {
- protected:
+  const double theta2, theta;
-  const double theta2;
+  const Matrix3 W, WW;
-  Matrix3 W, K, KK;
   bool nearZero;
-  double theta, sin_theta, one_minus_cos;  // only defined if !nearZero
 
-  void init(bool nearZeroApprox = false);
+  // 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
 
- public:
   /// Constructor with element of Lie algebra so(3)
   explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
 
@@ -151,34 +149,57 @@ class GTSAM_EXPORT ExpmapFunctor {
 
   /// Rodrigues formula
   SO3 expmap() const;
+
+ protected:
+  void init(bool nearZeroApprox = false);
 };
 
 /// Functor that implements Exponential map *and* its derivatives
-class DexpFunctor : public ExpmapFunctor {
+struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   const Vector3 omega;
-  double a, b;
+  double C;  // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0
-  Matrix3 dexp_;
+  // 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)
-  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,
   // Information Theory, and Lie Groups", Volume 2, 2008.
-  //   expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
+  //   Expmap(xi + dxi) \approx Expmap(xi) * Expmap(dexp * dxi)
-  // This maps a perturbation v in the tangent space to
+  // This maps a perturbation dxi=(w,v) in the tangent space to
-  // a perturbation on the manifold Expmap(dexp * v) */
+  // a perturbation on the manifold Expmap(dexp * xi)
-  const Matrix3& dexp() const { return dexp_; }
+  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
+  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
-  GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
+  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,
+  Vector3 applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
-                       OptionalJacobian<3, 3> H1 = {},
                        OptionalJacobian<3, 3> H2 = {}) const;
+
+  /// Multiplies with leftJacobian(), with optional derivatives
+  Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
+                            OptionalJacobian<3, 3> H2 = {}) const;
+
+  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
 

gtsam/geometry/tests/testPoint3.cpp

@@ -154,6 +154,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;

gtsam/geometry/tests/testPose3.cpp

@@ -846,6 +846,27 @@ TEST( Pose3, ExpmapDerivative1) {
 
 /* ************************************************************************* */
 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)

gtsam/geometry/tests/testSO3.cpp

@@ -303,19 +303,61 @@ TEST(SO3, JacobianLogmap) {
   EXPECT(assert_equal(Jexpected, Jactual));
 }
 
+namespace test_cases {
+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, 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).crossB(v);
+        };
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
+      so3::DexpFunctor local(omega, nearZero);
+      for (const Vector3& v : test_cases::vs) {
+        local.crossB(v, aH1);
+        EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
+      }
+    }
+  }
+}
+
+//******************************************************************************
+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).doubleCrossC(v);
+        };
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
+      so3::DexpFunctor local(omega, nearZero);
+      for (const Vector3& v : test_cases::vs) {
+        local.doubleCrossC(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),
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
-                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
+      so3::DexpFunctor local(omega, nearZero);
-      so3::DexpFunctor local(omega, nearZeroApprox);
+      for (const Vector3& v : test_cases::vs) {
-      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
-                        Vector3(0.4, 0.3, 0.2)}) {
         EXPECT(assert_equal(Vector3(local.dexp() * v),
                             local.applyDexp(v, aH1, aH2)));
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
@@ -327,19 +369,38 @@ TEST(SO3, ApplyDexp) {
 }
 
 //******************************************************************************
-TEST(SO3, ApplyInvDexp) {
+TEST(SO3, ApplyLeftJacobian) {
   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).applyLeftJacobian(v);
         };
-    for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
-                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
+      so3::DexpFunctor local(omega, nearZero);
-      so3::DexpFunctor local(omega, nearZeroApprox);
+      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));
+      }
+    }
+  }
+}
+
+//******************************************************************************
+TEST(SO3, ApplyInvDexp) {
+  Matrix aH1, aH2;
+  for (bool nearZero : {true, false}) {
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
+        [=](const Vector3& omega, const Vector3& v) {
+          return so3::DexpFunctor(omega, nearZero).applyInvDexp(v);
+        };
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
+      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),
+      for (const Vector3& v : test_cases::vs) {
-                        Vector3(0.4, 0.3, 0.2)}) {
         EXPECT(assert_equal(Vector3(invDexp * v),
                             local.applyInvDexp(v, aH1, aH2)));
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));