applyLeftJacobian

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

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,

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;