Merge pull request #1979 from borglab/fix/faster_expmaps

Faster SE(3) and SE_2(3) exponential maps

gtsam/geometry/Pose3.cpp

@@ -183,18 +183,37 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
   // Get angular velocity omega and translational velocity v from twist xi
   const Vector3 w = xi.head<3>(), v = xi.tail<3>();
 
+  // Instantiate functor for Dexp-related operations:
+  const bool nearZero = (w.dot(w) <= 1e-5);
+  const so3::DexpFunctor local(w, nearZero);
+
   // Compute rotation using Expmap
-  Matrix3 Jr;
-  Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
+#ifdef GTSAM_USE_QUATERNIONS
+  const Rot3 R = traits<gtsam::Quaternion>::Expmap(w);
+#else
+  const Rot3 R(local.expmap());
+#endif
 
-  // Compute translation and optionally its Jacobian Q in w
-  // The Jacobian in v is the right Jacobian Jr of SO(3), which we already have.
-  Matrix3 Q;
-  const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr);
+  // The translation t = local.leftJacobian() * v.
+  // Here we call applyLeftJacobian, which is faster if you don't need
+  // Jacobians, and returns Jacobian of t with respect to w if asked.
+  // NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas
+  //   t_parallel = w * w.dot(v);  // translation parallel to axis
+  //   w_cross_v = w.cross(v);     // translation orthogonal to axis
+  //   t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2;
+  // but functor does not need R, deals automatically with the case where theta2
+  // is near zero, and also gives us the machinery for the Jacobians.
+  Matrix3 H;
+  const Vector3 t = local.applyLeftJacobian(v, Hxi ? &H : nullptr);
 
   if (Hxi) {
-    *Hxi << Jr, Z_3x3,  //
-        Q, Jr;
+    // The Jacobian of expmap is given by the right Jacobian of SO(3):
+    const Matrix3 Jr = local.rightJacobian();
+    // We are creating a Pose3, so we still need to chain H with R^T, the
+    // Jacobian of Pose3::Create with respect to t.
+    const Matrix3 Q = R.matrix().transpose() * H;
+    *Hxi << Jr, Z_3x3,  // Jr here *is* the Jacobian of expmap
+        Q, Jr;  // Here Jr = R^T * J_l, with J_l the Jacobian of t in v.
   }
 
   return Pose3(R, t);
@@ -260,45 +279,18 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
                                            double nearZeroThreshold) {
   const auto w = xi.head<3>();
   const auto v = xi.tail<3>();
-  Matrix3 Q;
-  ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
-  return Q;
-}
-
-/* ************************************************************************* */
-// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas
-//   t_parallel = w * w.dot(v);  // translation parallel to axis
-//   w_cross_v = w.cross(v);     // translation orthogonal to axis
-//   t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2;
-// but functor does not need R, deals automatically with the case where theta2
-// is near zero, and also gives us the machinery for the Jacobians.
-Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
-                                 OptionalJacobian<3, 3> Q,
-                                 OptionalJacobian<3, 3> J,
-                                 double nearZeroThreshold) {
-  const double theta2 = w.dot(w);
-  bool nearZero = (theta2 <= nearZeroThreshold);
 
   // Instantiate functor for Dexp-related operations:
+  bool nearZero = (w.dot(w) <= nearZeroThreshold);
   so3::DexpFunctor local(w, nearZero);
 
-  // Call applyLeftJacobian which is faster than local.leftJacobian() * v if you
-  // don't need Jacobians, and returns Jacobian of t with respect to w if asked.
+  // Call applyLeftJacobian to get its Jacobian
   Matrix3 H;
-  Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr);
-
-  // We return Jacobians for use in Expmap, so we multiply with X, that
-  // translates from left to right for our right expmap convention:
-  if (Q) {
-    Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
-    *Q = X * H;
-  }
-
-  if (J) {
-    *J = local.rightJacobian();  // = X * local.leftJacobian();
-  }
+  local.applyLeftJacobian(v, H);
 
-  return t;
+  // Multiply with R^T to account for the Pose3::Create Jacobian.
+  const Matrix3 R = local.expmap();
+  return R.transpose() * H;
 }
 
 /* ************************************************************************* */

gtsam/geometry/Pose3.h

@@ -220,27 +220,10 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
   *  (see Chirikjian11book2, pg 44, eq 10.95.
   *  The closed-form formula is identical to formula 102 in Barfoot14tro where
   *  Q_l of the SE3 Expmap left derivative matrix is given.
-  *  This is the Jacobian of ExpmapTranslation and computed there.
   */
   static Matrix3 ComputeQforExpmapDerivative(
       const Vector6& xi, double nearZeroThreshold = 1e-5);
 
-  /**
-   * Compute the translation part of the exponential map, with Jacobians.
-   * @param w 3D angular velocity
-   * @param v 3D velocity
-   * @param Q Optionally, compute 3x3 Jacobian wrpt w
-   * @param J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3)
-   * @param nearZeroThreshold threshold for small values
-   * @note This function returns Jacobians Q and J corresponding to the bottom
-   * blocks of the SE(3) exponential, and translated from left to right from the
-   * applyLeftJacobian Jacobians.
-   */
-  static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v,
-                                   OptionalJacobian<3, 3> Q = {},
-                                   OptionalJacobian<3, 3> J = {},
-                                   double nearZeroThreshold = 1e-5);
-
   using LieGroup<Pose3, 6>::inverse; // version with derivative
 
   /**

gtsam/geometry/SO3.cpp

@@ -92,7 +92,7 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
   init(nearZeroApprox);
 }
 
-SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }
+Matrix3 ExpmapFunctor::expmap() const { return I_3x3 + A * W + B * WW; }
 
 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
@@ -193,7 +193,7 @@ Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
 template <>
 GTSAM_EXPORT
 SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
-  return so3::ExpmapFunctor(axis, theta).expmap();
+  return SO3(so3::ExpmapFunctor(axis, theta).expmap());
 }
 
 //******************************************************************************
@@ -251,11 +251,11 @@ template <>
 GTSAM_EXPORT
 SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
   if (H) {
-    so3::DexpFunctor impl(omega);
-    *H = impl.dexp();
-    return impl.expmap();
+    so3::DexpFunctor local(omega);
+    *H = local.dexp();
+    return SO3(local.expmap());
   } else {
-    return so3::ExpmapFunctor(omega).expmap();
+    return SO3(so3::ExpmapFunctor(omega).expmap());
   }
 }
 

gtsam/geometry/SO3.h

@@ -150,7 +150,7 @@ struct GTSAM_EXPORT ExpmapFunctor {
   ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
 
   /// Rodrigues formula
-  SO3 expmap() const;
+  Matrix3 expmap() const;
 
  protected:
   void init(bool nearZeroApprox = false);

gtsam/geometry/tests/testSO3.cpp

@@ -163,22 +163,22 @@ TEST(SO3, ExpmapFunctor) {
 
   // axis angle version
   so3::ExpmapFunctor f1(axis, angle);
-  SO3 actual1 = f1.expmap();
+  SO3 actual1(f1.expmap());
   CHECK(assert_equal(expected, actual1.matrix(), 1e-5));
 
   // axis angle version, negative angle
   so3::ExpmapFunctor f2(axis, angle - 2*M_PI);
-  SO3 actual2 = f2.expmap();
+  SO3 actual2(f2.expmap());
   CHECK(assert_equal(expected, actual2.matrix(), 1e-5));
 
   // omega version
   so3::ExpmapFunctor f3(axis * angle);
-  SO3 actual3 = f3.expmap();
+  SO3 actual3(f3.expmap());
   CHECK(assert_equal(expected, actual3.matrix(), 1e-5));
 
   // omega version, negative angle
   so3::ExpmapFunctor f4(axis * (angle - 2*M_PI));
-  SO3 actual4 = f4.expmap();
+  SO3 actual4(f4.expmap());
   CHECK(assert_equal(expected, actual4.matrix(), 1e-5));
 }
 

gtsam/navigation/NavState.cpp

@@ -117,20 +117,32 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
   // Get angular velocity w and components rho (for t) and nu (for v) from xi
   Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>();
 
+  // Instantiate functor for Dexp-related operations:
+  const bool nearZero = (w.dot(w) <= 1e-5);
+  const so3::DexpFunctor local(w, nearZero);
+
   // Compute rotation using Expmap
-  Matrix3 Jr;
-  Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
+#ifdef GTSAM_USE_QUATERNIONS
+  const Rot3 R = traits<gtsam::Quaternion>::Expmap(w);
+#else
+  const Rot3 R(local.expmap());
+#endif
 
-  // Compute translations and optionally their Jacobians Q in w
-  // The Jacobians with respect to rho and nu are equal to Jr
-  Matrix3 Qt, Qv;
-  Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr);
-  Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr);
+  // Compute translation and velocity. See Pose3::Expmap
+  Matrix3 H_t_w, H_v_w;
+  const Vector3 t = local.applyLeftJacobian(rho, Hxi ? &H_t_w : nullptr);
+  const Vector3 v = local.applyLeftJacobian(nu, Hxi ? &H_v_w : nullptr);
 
   if (Hxi) {
-    *Hxi << Jr, Z_3x3, Z_3x3,  //
-        Qt, Jr, Z_3x3,         //
-        Qv, Z_3x3, Jr;
+    const Matrix3 Jr = local.rightJacobian();
+    // We are creating a NavState, so we still need to chain H_t_w and H_v_w
+    // with R^T, the Jacobian of Navstate::Create with respect to both t and v.
+    const Matrix3 M = R.matrix();
+    *Hxi << Jr, Z_3x3, Z_3x3,   // Jr here *is* the Jacobian of expmap
+        M.transpose() * H_t_w, Jr, Z_3x3,  //
+        M.transpose() * H_v_w, Z_3x3, Jr;
+        // In the last two rows, Jr = R^T * J_l, see Barfoot eq. (8.83).
+        // J_l is the Jacobian of applyLeftJacobian in the second argument.
   }
 
   return NavState(R, t, v);
@@ -231,11 +243,22 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
   const Vector3 w = xi.head<3>();
   Vector3 rho = xi.segment<3>(3);
   Vector3 nu = xi.tail<3>();
-
-  Matrix3 Qt, Qv;
-  const Rot3 R = Rot3::Expmap(w);
-  Pose3::ExpmapTranslation(w, rho, Qt);
-  Pose3::ExpmapTranslation(w,  nu, Qv);
+
+  // Instantiate functor for Dexp-related operations:
+  const bool nearZero = (w.dot(w) <= 1e-5);
+  const so3::DexpFunctor local(w, nearZero);
+
+  // Call applyLeftJacobian to get its Jacobians
+  Matrix3 H_t_w, H_v_w;
+  local.applyLeftJacobian(rho, H_t_w);
+  local.applyLeftJacobian(nu, H_v_w);
+
+  // Multiply with R^T to account for NavState::Create Jacobian.
+  const Matrix3 R = local.expmap();
+  const Matrix3 Qt = R.transpose() * H_t_w;
+  const Matrix3 Qv = R.transpose() * H_v_w;
+
+  // Now compute the blocks of the LogmapDerivative Jacobian
   const Matrix3 Jw = Rot3::LogmapDerivative(w);
   const Matrix3 Qt2 = -Jw * Qt * Jw;
   const Matrix3 Qv2 = -Jw * Qv * Jw;
@@ -247,7 +270,6 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
   return J;
 }
 
-
 //------------------------------------------------------------------------------
 NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
                                           ChartJacobian Hxi) {