speed up ManifoldPreintegration update Jacobian computation

Author: JzHuai0108

gtsam/navigation/ImuFactor.cpp

@@ -69,20 +69,6 @@ void PreintegratedImuMeasurementsT<PreintegrationType>::integrateMeasurement(
   Matrix93 B, C;  // Jacobian of state wrpt accel bias and omega bias respectively.
   PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
 
-  // For TangentPreintegration, the perturbation on deltaXij is dX2, the 3D perturbation of the rotation tangent space + 
-  // 3D perturbation of the translation part + 3D perturbation of the velocity part,
-  // as validated by the matrix A_k in the ImuFactor.pdf and the unit test on preintegrated_H_biasAcc()
-
-  // For ManifoldPreintegration, for covariance propagation, the perturbation on deltaXij is dX1, 
-  // the right perturbation of the NavState manifold, as validated by the unit tests on NavState::retract);
-  // for jacobian computation, the perturbation on deltaXij is dX2 defined above, as validated by the unit test on delPdelBiasAcc().
-
-  // For ImuFactor or CombinedImuFactor, the residual is defined by PreintegrationBase::computeError and 
-  // its perturbation is coincident to dX2 (up to a right Jacobian of a small angle ~ identity).
-  // So for TangentPreintegration, the covariance of the residual equals to the preintMeasCov of TangentPreintegration.
-  // But for ManifoldPreintegration, we need to rotate the position and velocity blocks of the preintMeasCov to get the residual cov.
-  // Otherwise, we can update the cov propagation of manifold preintegration to make it use dX2.
-
   // first order covariance propagation:
   // as in [2] we consider a first order propagation that can be seen as a
   // prediction phase in EKF

gtsam/navigation/ImuFactor.h

@@ -173,6 +173,19 @@ class GTSAM_EXPORT PreintegratedImuMeasurementsT: public PreintegrationType {
 // controls which class PreintegratedImuMeasurements uses):
 using PreintegratedImuMeasurements = PreintegratedImuMeasurementsT<DefaultPreintegrationType>;
 
+// For TangentPreintegration, the perturbation on deltaXij is dXt, the 3D perturbation of the rotation tangent space + 
+// 3D perturbation of the translation part + 3D perturbation of the velocity part,
+// as validated by the matrix A_k in the ImuFactor.pdf and the unit test on preintegrated_H_biasAcc()
+
+// For ManifoldPreintegration, for covariance propagation, the perturbation on deltaXij is dXn, 
+// the right perturbation of the NavState manifold, as validated by the unit tests on NavState::retract);
+// for jacobian computation, the perturbation on deltaXij is dXt defined above, as validated by the unit test on delPdelBiasAcc().
+
+// For ImuFactor or CombinedImuFactor, the residual is defined by PreintegrationBase::computeError and 
+// its perturbation is coincident to dXt (up to a right Jacobian of a small angle ~ identity).
+// So for TangentPreintegration, the covariance of the residual equals to the preintMeasCov of TangentPreintegration.
+// But for ManifoldPreintegration, we need to rotate the position and velocity blocks of the preintMeasCov to get the residual cov.
+// Otherwise, we can update the cov propagation of manifold preintegration to make it use dXt.
 template <class PIM>
 inline gtsam::Matrix9 imuFactorResidualCov(const PIM& pim) {
   return pim.preintMeasCov();

gtsam/navigation/ManifoldPreintegration.cpp

@@ -56,6 +56,120 @@ bool ManifoldPreintegration::equals(const ManifoldPreintegration& other,
       && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
 }
 
+NavState ManifoldPreintegration::UpdatePreintegrated(
+    const Eigen::Vector3d& a_body,
+    const Eigen::Vector3d& w_body,
+    double dt,
+    const NavState& X,                       // (R,p,v)
+    gtsam::OptionalJacobian<9,9> A_t,        // dXt_{k+1} / dXt_k
+    gtsam::OptionalJacobian<9,3> B_t,        // dXt_{k+1} / d a_body
+    gtsam::OptionalJacobian<9,3> C_t) {      // dXt_{k+1} / d w_body
+
+  // Let's denote the right perturbation dXn on the NavState X_ij's manifold, i.e.,
+  // dXn = [\delta \phi_ij, \delta p_ij, \delta v_ij] where R_ij = \hat{R}_ij Exp(\delta \phi_ij)
+  // p_ij = \hat{p}_ij + R_ij \delta p_{ij} and v_ij = \hat{v}_ij + R_ij \delta v_{ij}.
+  // The error state dXt of the preint X_ij is actually defined as
+  // dXt = [\delta \phi_ij, \delta p_ij, \delta v_ij] where R_ij = \hat{R}_ij Exp(\delta \phi_ij)
+  // p_ij = \hat{p}_ij + \delta p_{ij} and v_ij = \hat{v}_ij + \delta v_{ij}.
+  // So to propagate the X_ij's covariance, we need to transform the transition matrix A.
+  // from NavState.update(), An = \frac{\delta X^n_j}{\delta X^n_{j-1}}, and transform it to
+  // At = \frac{\delta X^t_j}{\delta X^t_{j-1}} = \frac{\delta X^t_j}{\delta X^n_j} * An * \frac{\delta X^n_{j-1}}{\delta X^t_{j-1}}
+  
+  const double dt2  = dt * dt;
+  const double dt22 = 0.5 * dt2;
+
+  const gtsam::Rot3& R = X.rotation();
+  const Eigen::Matrix3d Rm = R.matrix();
+
+  // Rotation increment
+  const Eigen::Vector3d phi = w_body * dt;
+
+  Eigen::Matrix3d Dexp;  // not used directly here, but cheap to compute
+  const gtsam::Rot3 dR = gtsam::Rot3::Expmap(phi, (A_t || C_t) ? &Dexp : nullptr);
+  const gtsam::Rot3 Rn = R.compose(dR);
+
+  // a_nav uses OLD R (matching typical preintegration / NavState.update structure)
+  const Eigen::Vector3d a_nav = R.rotate(a_body);
+
+  const gtsam::Point3  pn = X.position() + X.v() * dt + a_nav * dt22;
+  const Eigen::Vector3d vn = X.v() + a_nav * dt;
+
+  NavState Xn(Rn, pn, vn);
+
+  if (A_t) {
+    A_t->setZero();
+
+    // dphi+ = dR^T * dphi  (right-perturbation transport)
+    A_t->block<3,3>(0,0) = dR.transpose().matrix();
+
+    // dp+ = dp + dt dv + (d/dphi)(0.5*R*a*dt^2)*dphi
+    // dv+ = dv            + (d/dphi)(R*a*dt)*dphi
+    //
+    // Right perturbation: R Exp(dphi) a ≈ R (I + [dphi]x) a
+    // so δ(R a) = R (dphi x a) = - R [a]x dphi
+    const Eigen::Matrix3d a_skew = skewSymmetric(a_body);
+    A_t->block<3,3>(3,0) = -Rm * a_skew * dt22;
+    A_t->block<3,3>(6,0) = -Rm * a_skew * dt;
+
+    // world-additive p,v parts
+    A_t->block<3,3>(3,3) = Eigen::Matrix3d::Identity();
+    A_t->block<3,3>(3,6) = Eigen::Matrix3d::Identity() * dt;
+    A_t->block<3,3>(6,6) = Eigen::Matrix3d::Identity();
+  }
+
+  if (B_t) {
+    B_t->setZero();
+    // dp+ / da = 0.5 * R * dt^2, dv+ / da = R * dt
+    B_t->block<3,3>(3,0) = Rm * dt22;
+    B_t->block<3,3>(6,0) = Rm * dt;
+  }
+
+  if (C_t) {
+    C_t->setZero();
+    // dphi+ / dw = invJr(phi) * dt
+    so3::DexpFunctor local(phi);
+    const Eigen::Matrix3d invJr = local.InvJacobian().right();
+    C_t->block<3,3>(0,0) = invJr * dt;
+  }
+
+  return Xn;
+}
+
+NavState ManifoldPreintegration::UpdatePreintegratedSlow(
+    const Eigen::Vector3d& a_body,
+    const Eigen::Vector3d& w_body,
+    double dt,
+    const NavState& X,
+    gtsam::OptionalJacobian<9,9> A,
+    gtsam::OptionalJacobian<9,3> B,
+    gtsam::OptionalJacobian<9,3> C) {
+  const Eigen::Matrix3d Rm = X.rotation().matrix();
+
+  Matrix9 D_dXn_dXt_jm1 = Matrix9::Identity();
+  D_dXn_dXt_jm1.block<3,3>(3,3) = Rm.transpose();
+  D_dXn_dXt_jm1.block<3,3>(6,6) = Rm.transpose();
+
+  Matrix9  An;
+  Matrix93 Bn, Cn;
+
+  NavState Xn = X.update(a_body, w_body, dt,
+                         A ? &An : nullptr,
+                         B ? &Bn : nullptr,
+                         C ? &Cn : nullptr);
+
+  const Eigen::Matrix3d Rnm = Xn.rotation().matrix();
+
+  Matrix9 D_dXt_dXn_j = Matrix9::Identity();
+  D_dXt_dXn_j.block<3,3>(3,3) = Rnm;
+  D_dXt_dXn_j.block<3,3>(6,6) = Rnm;
+
+  if (A) *A = D_dXt_dXn_j * An * D_dXn_dXt_jm1;
+  if (B) *B = D_dXt_dXn_j * Bn;
+  if (C) *C = D_dXt_dXn_j * Cn;
+
+  return Xn;
+}
+
 //------------------------------------------------------------------------------
 void ManifoldPreintegration::update(const Vector3& measuredAcc,
     const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
@@ -78,27 +192,7 @@ void ManifoldPreintegration::update(const Vector3& measuredAcc,
 
   // Do update
   deltaTij_ += dt;
-
-  // Let's denote the right perturbation dXn on the NavState X_ij's manifold, i.e.,
-  // dXn = [\delta \phi_ij, \delta p_ij, \delta v_ij] where R_ij = \hat{R}_ij Exp(\delta \phi_ij)
-  // p_ij = \hat{p}_ij + R_ij \delta p_{ij} and v_ij = \hat{v}_ij + R_ij \delta v_{ij}.
-  // The error state dXt of the preint X_ij is actually defined as
-  // dXt = [\delta \phi_ij, \delta p_ij, \delta v_ij] where R_ij = \hat{R}_ij Exp(\delta \phi_ij)
-  // p_ij = \hat{p}_ij + \delta p_{ij} and v_ij = \hat{v}_ij + \delta v_{ij}.
-  // So to propagate the X_ij's covariance, we need to transform the transition matrix A.
-  // from NavState.update(), An = \frac{\delta X^n_j}{\delta X^n_{j-1}}, and transform it to
-  // At = \frac{\delta X^t_j}{\delta X^t_{j-1}} = \frac{\delta X^t_j}{\delta X^n_j} * An * \frac{\delta X^n_{j-1}}{\delta X^t_{j-1}}
-  Matrix9 D_dXn_dXt_jm1 = Matrix9::Identity();
-  D_dXn_dXt_jm1.block<3, 3>(3, 3).noalias() = deltaXij_.rotation().matrix().transpose();
-  D_dXn_dXt_jm1.block<3, 3>(6, 6) = D_dXn_dXt_jm1.block<3, 3>(3, 3);
-  Matrix9 An;
-  deltaXij_ = deltaXij_.update(acc, omega, dt, &An, B, C); // functional
-  Matrix9 D_dXt_dXn_j = Matrix9::Identity();
-  D_dXt_dXn_j.block<3, 3>(3, 3).noalias() = deltaXij_.rotation().matrix();
-  D_dXt_dXn_j.block<3, 3>(6, 6) = D_dXt_dXn_j.block<3, 3>(3, 3);
-  *A = D_dXt_dXn_j * An * D_dXn_dXt_jm1;
-  *B = D_dXt_dXn_j * *B;
-  *C = D_dXt_dXn_j * *C;
+  deltaXij_ = UpdatePreintegrated(acc, omega, dt, deltaXij_, A, B, C);
 
   if (p().body_P_sensor) {
     // More complicated derivatives in case of non-trivial sensor pose

gtsam/navigation/ManifoldPreintegration.h

@@ -92,6 +92,28 @@ class GTSAM_EXPORT ManifoldPreintegration : public PreintegrationBase {
   /// @name Main functionality
   /// @{
 
+  // Error chart dXt:
+  //   R = Rhat * Exp(dphi)     (right perturbation)
+  //   p = phat + dp            (world additive)
+  //   v = vhat + dv            (world additive)
+  static NavState UpdatePreintegrated(
+      const Eigen::Vector3d& a_body,
+      const Eigen::Vector3d& w_body,
+      double dt,
+      const NavState& X,                          // (R,p,v)
+      gtsam::OptionalJacobian<9,9> A = {},        // dXt_{k+1} / dXt_k
+      gtsam::OptionalJacobian<9,3> B = {},        // dXt_{k+1} / d a_body
+      gtsam::OptionalJacobian<9,3> C = {});       // dXt_{k+1} / d w_body
+
+  static NavState UpdatePreintegratedSlow(
+      const Eigen::Vector3d& a_body,
+      const Eigen::Vector3d& w_body,
+      double dt,
+      const NavState& X,                          // (R,p,v)
+      gtsam::OptionalJacobian<9,9> A = {},        // dXt_{k+1} / dXt_k
+      gtsam::OptionalJacobian<9,3> B = {},        // dXt_{k+1} / d a_body
+      gtsam::OptionalJacobian<9,3> C = {});       // dXt_{k+1} / d w_body
+
   /// Update preintegrated measurements and get derivatives
   /// It takes measured quantities in the j frame
   /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -334,7 +334,6 @@ MakeParamsU_Combined(const ImuSimConfig& cfg) {
   // Turn off bias random walk so we can compare to 9D EKF (no bias state)
   p->biasOmegaCovariance      = gtsam::Matrix3::Zero();
   p->biasAccCovariance        = gtsam::Matrix3::Zero();
-  p->biasAccOmegaInt.setZero();
 
   p->use2ndOrderCoriolis      = false;
   return p;

gtsam/navigation/tests/testImuFactor.cpp

@@ -948,7 +948,7 @@ static inline Maps9 BuildMaps9_Tangent(
   // phi = Log(dR)
   const gtsam::Vector3 phi = gtsam::Rot3::Logmap(gtsam::Rot3(dR));
   const Eigen::Matrix3d Jr     = gtsam::so3::DexpFunctor(phi).rightJacobian();
-  const Eigen::Matrix3d Jr_inv = gtsam::so3::DexpFunctor(phi).rightJacobianInverse();
+  const Eigen::Matrix3d Jr_inv = gtsam::so3::DexpFunctor(phi).InvJacobian().right();
 
   m.F.block<3,3>(0,0) = A;
   m.F.block<3,3>(3,0) = -A * gtsam::skewSymmetric(dP);

gtsam/navigation/tests/testManifoldPreintegration.cpp

@@ -155,6 +155,63 @@ TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) {
                               H2.rightCols<3>()));                      
 }
 
+TEST(ManifoldPreintegration, UpdatePreintegratedFastMatchesSlow) {
+  using namespace gtsam;
+
+  const unsigned int seed_cfg  = 0;
+  const unsigned int seed_meas = 1;
+
+  ImuSimConfig cfg(seed_cfg);
+  auto params = MakeParamsU(cfg);
+
+  const double T = 5.0; // seconds
+  const auto meas = MakeRandomImuMeasurements(cfg, T, seed_meas);
+
+  NavState X_fast;
+  NavState X_slow;
+
+  Matrix9 P_fast = Matrix9::Zero();
+  Matrix9 P_slow = Matrix9::Zero();
+
+  const Matrix3 aCov = params->accelerometerCovariance;
+  const Matrix3 wCov = params->gyroscopeCovariance;
+  const Matrix3 iCov = params->integrationCovariance;
+
+  ManifoldPreintegration mip(params, cfg.bias);
+
+  for (const auto& s : meas) {
+    Matrix9  A_f;
+    Matrix93 B_f, C_f;
+    NavState Xn_fast = mip.UpdatePreintegrated(
+        s.measuredAcc, s.measuredOmega, s.dt, X_fast, &A_f, &B_f, &C_f);
+
+    Matrix9  A_s;
+    Matrix93 B_s, C_s;
+    NavState Xn_slow = mip.UpdatePreintegratedSlow(
+        s.measuredAcc, s.measuredOmega, s.dt, X_slow, &A_s, &B_s, &C_s);
+
+    EXPECT(assert_equal(Xn_fast, Xn_slow, 1e-9));
+
+    EXPECT_MAT_NEAR(A_f, A_s, 1e-9, 1e-6);
+    EXPECT_MAT_NEAR(B_f, B_s, 1e-9, 1e-6);
+    EXPECT_MAT_NEAR(C_f, C_s, 1e-6, 1e-4);
+
+    P_fast = A_f * P_fast * A_f.transpose();
+    P_fast.noalias() += B_f * (aCov / s.dt) * B_f.transpose();
+    P_fast.noalias() += C_f * (wCov / s.dt) * C_f.transpose();
+    P_fast.block<3,3>(3,3).noalias() += iCov * s.dt;
+
+    P_slow = A_s * P_slow * A_s.transpose();
+    P_slow.noalias() += B_s * (aCov / s.dt) * B_s.transpose();
+    P_slow.noalias() += C_s * (wCov / s.dt) * C_s.transpose();
+    P_slow.block<3,3>(3,3).noalias() += iCov * s.dt;
+
+    EXPECT_MAT_NEAR(P_fast, P_slow, 1e-5, 5e-3);
+    X_fast = Xn_fast;
+    X_slow = Xn_slow;
+  }
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;