applyLeftJacobianInverse

gtsam/geometry/SO3.cpp

@@ -142,14 +142,14 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
 
 Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                   OptionalJacobian<3, 3> H2) const {
-  const Matrix3 invDexp = rightJacobian().inverse();
-  const Vector3 c = invDexp * v;
+  const Matrix3 invJr = rightJacobianInverse();
+  const Vector3 c = invJr * v;
   if (H1) {
-    Matrix3 D_dexpv_w;
-    applyDexp(c, D_dexpv_w);  // get derivative H of forward mapping
-    *H1 = -invDexp * D_dexpv_w;
+    Matrix3 H;
+    applyDexp(c, H);  // get derivative H of forward mapping
+    *H1 = -invJr * H;
   }
-  if (H2) *H2 = invDexp;
+  if (H2) *H2 = invJr;
   return c;
 }
 
@@ -164,6 +164,20 @@ Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
   return v + BWv + CWWv;
 }
 
+Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
+                                              OptionalJacobian<3, 3> H1,
+                                              OptionalJacobian<3, 3> H2) const {
+  const Matrix3 invJl = leftJacobianInverse();
+  const Vector3 c = invJl * v;
+  if (H1) {
+    Matrix3 H;
+    applyLeftJacobian(c, H);  // get derivative H of forward mapping
+    *H1 = -invJl * H;
+  }
+  if (H2) *H2 = invJl;
+  return c;
+}
+
 }  // namespace so3
 
 //******************************************************************************

gtsam/geometry/SO3.h

@@ -162,7 +162,7 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   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
+  double D;  // (1 - A/2B) / theta2 or 1/12 for theta->0
 
   // Constants used in cross and doubleCross
   double E;  // (A - 2.0 * B) / theta2 or -1/12 for theta->0
@@ -212,6 +212,11 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
                             OptionalJacobian<3, 3> H2 = {}) const;
 
+  /// Multiplies with leftJacobianInverse(), with optional derivatives
+  Vector3 applyLeftJacobianInverse(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;

gtsam/geometry/tests/testSO3.cpp

@@ -385,6 +385,28 @@ TEST(SO3, ApplyDexp) {
   }
 }
 
+//******************************************************************************
+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 invJr = local.rightJacobianInverse();
+      for (const Vector3& v : test_cases::vs) {
+        EXPECT(
+            assert_equal(Vector3(invJr * v), local.applyInvDexp(v, aH1, aH2)));
+        EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
+        EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
+        EXPECT(assert_equal(invJr, aH2));
+      }
+    }
+  }
+}
+
 //******************************************************************************
 TEST(SO3, ApplyLeftJacobian) {
   Matrix aH1, aH2;
@@ -407,22 +429,22 @@ TEST(SO3, ApplyLeftJacobian) {
 }
 
 //******************************************************************************
-TEST(SO3, ApplyInvDexp) {
+TEST(SO3, ApplyLeftJacobianInverse) {
   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);
+          return so3::DexpFunctor(omega, nearZero).applyLeftJacobianInverse(v);
         };
     for (const Vector3& omega : test_cases::omegas(nearZero)) {
       so3::DexpFunctor local(omega, nearZero);
-      Matrix invDexp = local.dexp().inverse();
+      Matrix invJl = local.leftJacobianInverse();
       for (const Vector3& v : test_cases::vs) {
-        EXPECT(assert_equal(Vector3(invDexp * v),
-                            local.applyInvDexp(v, aH1, aH2)));
+        EXPECT(assert_equal(Vector3(invJl * v),
+                            local.applyLeftJacobianInverse(v, aH1, aH2)));
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
         EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
-        EXPECT(assert_equal(invDexp, aH2));
+        EXPECT(assert_equal(invJl, aH2));
       }
     }
   }