Simplified applyDexp

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;
@@ -83,45 +85,54 @@ SO3 ExpmapFunctor::expmap() const {
     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) {
+  C = (1 - A) / theta2;
+}
+
+Matrix3 DexpFunctor::rightJacobian() const {
   if (nearZero) {
-    rightJacobian_ = I_3x3 - 0.5 * W;  // + one_sixth * WW;
+    return I_3x3 - 0.5 * W;  // + one_sixth * WW;
   } else {
-    C = (1 - A) / theta2;
+    return I_3x3 - B * W + C * WW;
-    rightJacobian_ = I_3x3 - B * W + C * WW;
   }
 }
 
+// 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();
+}
+
+// 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 {
-  if (H1) {
+  // Wv = omega x * v, with Jacobian -V in w
-    const Matrix3 V = skewSymmetric(v);
+  const Vector3 Wv = gtsam::cross(omega, v);
-    if (nearZero) {
+
-      *H1 = 0.5 * V;
+  if (nearZero) {
-    } else {
+    if (H1) *H1 = 0.5 * skewSymmetric(v);
-      // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
+    if (H2) *H2 = I_3x3 - 0.5 * W;
-      const double Da = (A - 2.0 * B) / theta2;
+    return v - 0.5 * Wv;
-      const double Db = (B - 3.0 * C) / theta2;
+  } else {
-      *H1 = (Db * WW - Da * W) * v * omega.transpose() -
+    // WWv = omega x (omega x * v), with Jacobian doubleCrossJacobian
-            C * skewSymmetric(W * v) + B * V - C * W * V;
+    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;
     }
+    if (H2) *H2 = rightJacobian();
+    return v - B * Wv + C * WWv;
   }
-  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 = rightJacobian_.inverse();
+  const Matrix3 invDexp = rightJacobian().inverse();
   const Vector3 c = invDexp * v;
   if (H1) {
     Matrix3 D_dexpv_omega;
@@ -132,6 +143,23 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
   return c;
 }
 
+Matrix3 DexpFunctor::leftJacobian() const {
+  if (nearZero) {
+    return I_3x3 + 0.5 * W;  // + one_sixth * WW;
+  } else {
+    return I_3x3 + B * W + C * WW;
+  }
+}
+
+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;
+}
+
 }  // namespace so3
 
 //******************************************************************************

gtsam/geometry/SO3.h

@@ -26,7 +26,6 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/dllexport.h>
 
-#include <cmath>
 #include <vector>
 
 namespace gtsam {
@@ -160,7 +159,6 @@ class DexpFunctor : public ExpmapFunctor {
   static constexpr double one_sixth = 1.0 / 6.0;
   const Vector3 omega;
   double C;  // Ethan Eade's C constant
-  Matrix3 rightJacobian_;
 
  public:
   /// Constructor with element of Lie algebra so(3)
@@ -172,8 +170,11 @@ class DexpFunctor : public ExpmapFunctor {
   // Information Theory, and Lie Groups", Volume 2, 2008.
   //   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) */
+  // a perturbation on the manifold Expmap(dexp * v)
-  const Matrix3& dexp() const { return rightJacobian_; }
+  GTSAM_EXPORT Matrix3 rightJacobian() const;
+
+  /// differential of expmap == right Jacobian
+  GTSAM_EXPORT Matrix3 dexp() const { return rightJacobian(); }
 
   /// Multiplies with dexp(), with optional derivatives
   GTSAM_EXPORT Vector3 applyDexp(const Vector3& v,
@@ -186,8 +187,12 @@ class DexpFunctor : public ExpmapFunctor {
                                     OptionalJacobian<3, 3> H2 = {}) const;
 
   // Compute the left Jacobian for Exponential map in SO(3)
-  // Note precomputed, as not used as much
+  GTSAM_EXPORT Matrix3 leftJacobian() const;
-  Matrix3 leftJacobian() const;
+
+  /// Multiplies with leftJacobian(), with optional derivatives
+  GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v,
+                                         OptionalJacobian<3, 3> H1 = {},
+                                         OptionalJacobian<3, 3> H2 = {}) const;
 };
 }  //  namespace so3