Simplify functor according to Eade

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;

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 = {},
+  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;
+
+  // Compute the left Jacobian for Exponential map in SO(3)
+  // Note precomputed, as not used as much
+  Matrix3 leftJacobian() const;
 };
 }  //  namespace so3