Merge pull request #1930 from borglab/feature/NavStateGroup

Endow NavState with group operations

Author: dellaert

gtsam/geometry/Pose3.cpp

@@ -21,13 +21,10 @@
 
 #include <cmath>
 #include <iostream>
-#include <limits>
 #include <string>
 
 namespace gtsam {
 
-using std::vector;
-
 /** instantiate concept checks */
 GTSAM_CONCEPT_POSE_INST(Pose3)
 
@@ -167,21 +164,23 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
 /* ************************************************************************* */
 /** Modified from Murray94book version (which assumes w and v normalized?) */
 Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
-  if (Hxi) *Hxi = ExpmapDerivative(xi);
-
-  // get angular velocity omega and translational velocity v from twist xi
-  Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
-
-  Rot3 R = Rot3::Expmap(omega);
-  double theta2 = omega.dot(omega);
-  if (theta2 > std::numeric_limits<double>::epsilon()) {
-    Vector3 t_parallel = omega * omega.dot(v);  // translation parallel to axis
-    Vector3 omega_cross_v = omega.cross(v);     // points towards axis
-    Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
-    return Pose3(R, t);
-  } else {
-    return Pose3(R, v);
+  // Get angular velocity omega and translational velocity v from twist xi
+  const Vector3 w = xi.head<3>(), v = xi.tail<3>();
+
+  // Compute rotation using Expmap
+  Rot3 R = Rot3::Expmap(w);
+
+  // Compute translation and optionally its Jacobian in w
+  Matrix3 Q;
+  const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R);
+
+  if (Hxi) {
+    const Matrix3 Jw = Rot3::ExpmapDerivative(w);
+    *Hxi << Jw, Z_3x3,
+             Q, Jw;
   }
+
+  return Pose3(R, t);
 }
 
 /* ************************************************************************* */
@@ -240,55 +239,68 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
 }
 
 /* ************************************************************************* */
-Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) {
+Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
+                                           double nearZeroThreshold) {
+  Matrix3 Q;
   const auto w = xi.head<3>();
   const auto v = xi.tail<3>();
-  const Matrix3 V = skewSymmetric(v);
-  const Matrix3 W = skewSymmetric(w);
-
-  Matrix3 Q;
+  ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
+  return Q;
+}
 
-#ifdef NUMERICAL_EXPMAP_DERIV
-  Matrix3 Qj = Z_3x3;
-  double invFac = 1.0;
-  Q = Z_3x3;
-  Matrix3 Wj = I_3x3;
-  for (size_t j=1; j<10; ++j) {
-    Qj = Qj*W + Wj*V;
-    invFac = -invFac/(j+1);
-    Q = Q + invFac*Qj;
-    Wj = Wj*W;
+/* ************************************************************************* */
+Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
+                                 OptionalJacobian<3, 3> Q,
+                                 const std::optional<Rot3>& R,
+                                 double nearZeroThreshold) {
+  const double theta2 = w.dot(w);
+  bool nearZero = (theta2 <= nearZeroThreshold);
+
+  if (Q) {
+    const Matrix3 V = skewSymmetric(v);
+    const Matrix3 W = skewSymmetric(w);
+    const Matrix3 WVW = W * V * W;
+    const double theta = w.norm();
+
+    if (nearZero) {
+      static constexpr double one_sixth = 1. / 6.;
+      static constexpr double one_twenty_fourth = 1. / 24.;
+      static constexpr double one_one_hundred_twentieth = 1. / 120.;
+
+      *Q = -0.5 * V + one_sixth * (W * V + V * W - WVW) -
+           one_twenty_fourth * (W * W * V + V * W * W - 3 * WVW) +
+           one_one_hundred_twentieth * (WVW * W + W * WVW);
+    } else {
+      const double s = sin(theta), c = cos(theta);
+      const double theta3 = theta2 * theta, theta4 = theta3 * theta,
+                   theta5 = theta4 * theta;
+
+      // Invert the sign of odd-order terms to have the right Jacobian
+      *Q = -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) +
+           (1 - theta2 / 2 - c) / theta4 * (W * W * V + V * W * W - 3 * WVW) -
+           0.5 *
+               ((1 - theta2 / 2 - c) / theta4 -
+                3 * (theta - s - theta3 / 6.) / theta5) *
+               (WVW * W + W * WVW);
+    }
   }
-#else
-  // The closed-form formula in Barfoot14tro eq. (102)
-  double phi = w.norm();
-  const Matrix3 WVW = W * V * W;
-  if (std::abs(phi) > nearZeroThreshold) {
-    const double s = sin(phi), c = cos(phi);
-    const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi,
-                 phi5 = phi4 * phi;
-    // Invert the sign of odd-order terms to have the right Jacobian
-    Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) +
-        (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) -
-        0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) *
-            (WVW * W + W * WVW);
+
+  // TODO(Frank): this threshold is *different*. Why?
+  if (nearZero) {
+    return v + 0.5 * w.cross(v);
   } else {
-    Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) -
-        1. / 24. * (W * W * V + V * W * W - 3 * WVW) +
-        1. / 120. * (WVW * W + W * WVW);
+    Vector3 t_parallel = w * w.dot(v);  // translation parallel to axis
+    Vector3 w_cross_v = w.cross(v);     // translation orthogonal to axis
+    Rot3 rotation = R.value_or(Rot3::Expmap(w));
+    Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / theta2;
+    return t;
   }
-#endif
-
-  return Q;
 }
 
 /* ************************************************************************* */
 Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
-  const Vector3 w = xi.head<3>();
-  const Matrix3 Jw = Rot3::ExpmapDerivative(w);
-  const Matrix3 Q = ComputeQforExpmapDerivative(xi);
   Matrix6 J;
-  J << Jw, Z_3x3, Q, Jw;
+  Expmap(xi, J);
   return J;
 }
 

gtsam/geometry/Pose3.h

@@ -217,10 +217,25 @@ 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 derivative.
+   * @param w 3D angular velocity
+   * @param v 3D velocity
+   * @param Q Optionally, compute 3x3 Jacobian wrpt w
+   * @param R Optionally, precomputed as Rot3::Expmap(w)
+   * @param nearZeroThreshold threshold for small values
+   * Note Q is 3x3 bottom-left block of SE3 Expmap right derivative matrix
+   */
+  static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v,
+                                   OptionalJacobian<3, 3> Q = {},
+                                   const std::optional<Rot3>& R = {},
+                                   double nearZeroThreshold = 1e-5);
+
   using LieGroup<Pose3, 6>::inverse; // version with derivative
 
   /**

gtsam/navigation/NavState.cpp

@@ -77,10 +77,13 @@ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const {
 }
 
 //------------------------------------------------------------------------------
-Matrix7 NavState::matrix() const {
+Matrix5 NavState::matrix() const {
   Matrix3 R = this->R();
-  Matrix7 T;
-  T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0;
+
+  Matrix5 T = Matrix5::Identity();
+  T.block<3, 3>(0, 0) = R;
+  T.block<3, 1>(0, 3) = t_;
+  T.block<3, 1>(0, 4) = v_;
   return T;
 }
 
@@ -103,6 +106,161 @@ bool NavState::equals(const NavState& other, double tol) const {
       && equal_with_abs_tol(v_, other.v_, tol);
 }
 
+//------------------------------------------------------------------------------
+NavState NavState::inverse() const {
+  Rot3 Rt = R_.inverse();
+  return NavState(Rt, Rt * (-t_), Rt * -(v_));
+}
+
+//------------------------------------------------------------------------------
+NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
+  // Get angular velocity w, translational velocity v, and acceleration a
+  Vector3 w = xi.head<3>();
+  Vector3 rho = xi.segment<3>(3);
+  Vector3 nu = xi.tail<3>();
+
+  // Compute rotation using Expmap
+  Rot3 R = Rot3::Expmap(w);
+
+  // Compute translations and optionally their Jacobians
+  Matrix3 Qt, Qv;
+  Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr, R);
+  Vector3 v = Pose3::ExpmapTranslation(w,  nu, Hxi ? &Qv : nullptr, R);
+
+  if (Hxi) {
+    const Matrix3 Jw = Rot3::ExpmapDerivative(w);
+    *Hxi << Jw, Z_3x3, Z_3x3,
+            Qt,    Jw, Z_3x3,
+            Qv, Z_3x3,    Jw;
+  }
+
+  return NavState(R, t, v);
+}
+
+//------------------------------------------------------------------------------
+Vector9 NavState::Logmap(const NavState& state, OptionalJacobian<9, 9> Hstate) {
+  if (Hstate) *Hstate = LogmapDerivative(state);
+
+  const Vector3 phi = Rot3::Logmap(state.rotation());
+  const Vector3& p = state.position();
+  const Vector3& v = state.velocity();
+  const double t = phi.norm();
+  if (t < 1e-8) {
+    Vector9 log;
+    log << phi, p, v;
+    return log;
+
+  } else {
+    const Matrix3 W = skewSymmetric(phi / t);
+
+    const double Tan = tan(0.5 * t);
+    const Vector3 Wp = W * p;
+    const Vector3 Wv = W * v;
+    const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp);
+    const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv);
+    Vector9 log;
+    // Order is ω, p, v
+    log << phi, rho, nu;
+    return log;
+  }
+}
+
+//------------------------------------------------------------------------------
+Matrix9 NavState::AdjointMap() const {
+  const Matrix3 R = R_.matrix();
+  Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
+  Matrix3 B = skewSymmetric(v_.x(), v_.y(), v_.z()) * R;
+  // Eqn 2 in Barrau20icra
+  Matrix9 adj;
+  adj << R, Z_3x3, Z_3x3, A, R, Z_3x3, B, Z_3x3, R;
+  return adj;
+}
+
+//------------------------------------------------------------------------------
+Vector9 NavState::Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_state,
+                          OptionalJacobian<9, 9> H_xib) const {
+  const Matrix9 Ad = AdjointMap();
+
+  // Jacobians
+  if (H_state) *H_state = -Ad * adjointMap(xi_b);
+  if (H_xib) *H_xib = Ad;
+
+  return Ad * xi_b;
+}
+
+//------------------------------------------------------------------------------
+Matrix9 NavState::adjointMap(const Vector9& xi) {
+  Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
+  Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
+  Matrix3 a_hat = skewSymmetric(xi(6), xi(7), xi(8));
+  Matrix9 adj;
+  adj << w_hat, Z_3x3, Z_3x3, v_hat, w_hat, Z_3x3, a_hat, Z_3x3, w_hat;
+  return adj;
+}
+
+//------------------------------------------------------------------------------
+Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y,
+                          OptionalJacobian<9, 9> Hxi,
+                          OptionalJacobian<9, 9> H_y) {
+  if (Hxi) {
+    Hxi->setZero();
+    for (int i = 0; i < 9; ++i) {
+      Vector9 dxi;
+      dxi.setZero();
+      dxi(i) = 1.0;
+      Matrix9 Gi = adjointMap(dxi);
+      Hxi->col(i) = Gi * y;
+    }
+  }
+
+  const Matrix9& ad_xi = adjointMap(xi);
+  if (H_y) *H_y = ad_xi;
+
+  return ad_xi * y;
+}
+
+//------------------------------------------------------------------------------
+Matrix9 NavState::ExpmapDerivative(const Vector9& xi) {
+  Matrix9 J;
+  Expmap(xi, J);
+  return J;
+}
+
+//------------------------------------------------------------------------------
+Matrix9 NavState::LogmapDerivative(const NavState& state) {
+  const Vector9 xi = Logmap(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, R);
+  Pose3::ExpmapTranslation(w,  nu, Qv, R);
+  const Matrix3 Jw = Rot3::LogmapDerivative(w);
+  const Matrix3 Qt2 = -Jw * Qt * Jw;
+  const Matrix3 Qv2 = -Jw * Qv * Jw;
+
+  Matrix9 J;
+  J <<  Jw, Z_3x3, Z_3x3, 
+       Qt2,    Jw, Z_3x3,
+       Qv2, Z_3x3,    Jw;
+  return J;
+}
+
+
+//------------------------------------------------------------------------------
+NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
+                                          ChartJacobian Hxi) {
+  return Expmap(xi, Hxi);
+}
+
+//------------------------------------------------------------------------------
+Vector9 NavState::ChartAtOrigin::Local(const NavState& state,
+                                       ChartJacobian Hstate) {
+  return Logmap(state, Hstate);
+}
+
 //------------------------------------------------------------------------------
 NavState NavState::retract(const Vector9& xi, //
     OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
@@ -142,15 +300,16 @@ Vector9 NavState::localCoordinates(const NavState& g, //
   Matrix3 D_xi_R;
   xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV;
   if (H1) {
-    *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
-    D_dt_R, -I_3x3, Z_3x3, //
-    D_dv_R, Z_3x3, -I_3x3;
+    *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3,  //
+        D_dt_R, -I_3x3, Z_3x3,             //
+        D_dv_R, Z_3x3, -I_3x3;
   }
   if (H2) {
-    *H2 << D_xi_R, Z_3x3, Z_3x3, //
-    Z_3x3, dR.matrix(), Z_3x3, //
-    Z_3x3, Z_3x3, dR.matrix();
+    *H2 << D_xi_R, Z_3x3, Z_3x3,    //
+        Z_3x3, dR.matrix(), Z_3x3,  //
+        Z_3x3, Z_3x3, dR.matrix();
   }
+
   return xi;
 }
 
@@ -213,7 +372,8 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
 //------------------------------------------------------------------------------
 Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
     OptionalJacobian<9, 9> H) const {
-  auto [nRb, n_t, n_v] = (*this);
+  Rot3 nRb = R_;
+  Point3 n_t = t_, n_v = v_;
 
   const double dt2 = dt * dt;
   const Vector3 omega_cross_vel = omega.cross(n_v);