inverse Jacobians

gtsam/geometry/SO3.cpp

@@ -87,19 +87,29 @@ 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 = nearZero ? one_sixth : (1 - A) / theta2;
-  D = nearZero ? _one_twelfth : (A - 2.0 * B) / theta2;
-  E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2;
+  if (!nearZero) {
+    C = (1 - A) / theta2;
+    D = (1.0 - A / (2.0 * B)) / theta2;
+    E = (2.0 * B - A) / theta2;
+    F = (3.0 * C - B) / theta2;
+  } else {
+    // Limit as theta -> 0
+    // TODO(Frank): flipping signs here does not trigger any tests: harden!
+    C = one_sixth;
+    D = one_twelfth;
+    E = one_twelfth;
+    F = one_sixtieth;
+  }
 }
 
 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);
+    // Apply product rule to (B Wv)
+    // - E * omega.transpose() is 1x3 Jacobian of B with respect to omega
+    // - skewSymmetric(v) is 3x3 Jacobian of Wv = gtsam::cross(omega, v)
+    *H = - Wv * E * omega.transpose() - B * skewSymmetric(v);
   }
   return B * Wv;
 }
@@ -111,10 +121,10 @@ Vector3 DexpFunctor::doubleCrossC(const Vector3& v,
   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;
+    // Apply product rule to (C WWv)
+    // - F * omega.transpose() is 1x3 Jacobian of C with respect to omega
+    // doubleCrossJacobian is 3x3 Jacobian of WWv = gtsam::doubleCross(omega, v)
+    *H = - WWv * F * omega.transpose() + C * doubleCrossJacobian;
   }
   return C * WWv;
 }

gtsam/geometry/SO3.h

@@ -157,10 +157,16 @@ struct GTSAM_EXPORT ExpmapFunctor {
 /// Functor that implements Exponential map *and* its derivatives
 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
+
+  // Ethan's C constant used in Jacobians
+  double C;  // (1 - A) / theta^2 or 1/6 for theta->0
+
+  // Constant used in inverse Jacobians
+  double D;  // 1.0 - A / (2.0 * B)) / theta2 or 1/12 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
+  double E;  // (A - 2.0 * B) / theta2 or -1/12 for theta->0
+  double F;  // (B - 3.0 * C) / theta2 or -1/60 for theta->0
 
   /// Constructor with element of Lie algebra so(3)
   explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
@@ -179,6 +185,15 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   /// Differential of expmap == right Jacobian
   inline Matrix3 dexp() const { return rightJacobian(); }
 
+  /// Inverse of right Jacobian
+  Matrix3 rightJacobianInverse() const { return I_3x3 + 0.5 * W + D * WW; }
+
+  // Inverse of left Jacobian
+  Matrix3 leftJacobianInverse() const { return I_3x3 - 0.5 * W + D * WW; }
+
+  /// Synonym for rightJacobianInverse
+  inline Matrix3 invDexp() const { return rightJacobianInverse(); }
+
   /// Computes B * (omega x v).
   Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
 
@@ -198,8 +213,8 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
                             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;
+  static constexpr double one_twelfth = 1.0 / 12.0;
+  static constexpr double one_sixtieth = 1.0 / 60.0;
 };
 }  //  namespace so3
 

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;
@@ -304,13 +305,29 @@ TEST(SO3, JacobianLogmap) {
 }
 
 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> small{{0, 0, 0},                                 //
+                           {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5},  //,
+                           {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}};
 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, JacobianInverses) {
+  Matrix HR, HL;
+  for (bool nearZero : {true, false}) {
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
+      so3::DexpFunctor local(omega, nearZero);
+      EXPECT(assert_equal<Matrix3>(local.rightJacobian().inverse(),
+                                   local.rightJacobianInverse()));
+      EXPECT(assert_equal<Matrix3>(local.leftJacobian().inverse(),
+                                   local.leftJacobianInverse()));
+    }
+  }
+}
+
 //******************************************************************************
 TEST(SO3, CrossB) {
   Matrix aH1;