make manifold preintegration jac and cov consistent

Author: JzHuai0108

gtsam/navigation/CombinedImuFactor.h

@@ -193,25 +193,6 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurementsT : public PreintegrationTyp
 // For backward compatibility:
 using PreintegratedCombinedMeasurements = PreintegratedCombinedMeasurementsT<DefaultPreintegrationType>;
 
-// Manifold combined preintegration: convert preintMeasCov() (defined in the
-// “transported” coordinate convention) into the residual covariance convention
-// used by the factor, by rotating the dp/dv blocks with dR.
-inline Eigen::Matrix<double, 15, 15> combinedImuFactorResidualCov(
-    const gtsam::PreintegratedCombinedMeasurementsT<gtsam::ManifoldPreintegration>& pim) {
-  const Eigen::Matrix<double, 15, 15> Sigma = pim.preintMeasCov();
-
-  const gtsam::Matrix3 R = pim.deltaRij().matrix();
-
-  Eigen::Matrix<double, 15, 15> J = Eigen::Matrix<double, 15, 15>::Identity();
-  J.block<3,3>(3,3) = R;  // dp
-  J.block<3,3>(6,6) = R;  // dv
-
-  Eigen::Matrix<double, 15, 15> Sigma_out;
-  Sigma_out.noalias() = J * Sigma * J.transpose();
-  return Sigma_out;
-}
-
-// Fallback for other Combined PIM types.
 template <class PIM>
 inline Eigen::Matrix<double, 15, 15> combinedImuFactorResidualCov(const PIM& pim) {
   return pim.preintMeasCov();

gtsam/navigation/ImuFactor.cpp

@@ -69,13 +69,19 @@ 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 ManifoldPreintegration, the perturbation on deltaXij is dX1, the right perturbation of the NavState manifold (as validated by the unit tests on NavState::retract).
   // 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).
+  // 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

gtsam/navigation/ImuFactor.h

@@ -173,24 +173,6 @@ class GTSAM_EXPORT PreintegratedImuMeasurementsT: public PreintegrationType {
 // controls which class PreintegratedImuMeasurements uses):
 using PreintegratedImuMeasurements = PreintegratedImuMeasurementsT<DefaultPreintegrationType>;
 
-// Manifold 9D: map Sigma -> "residual covariance" by rotating dp/dv blocks with dR.
-inline gtsam::Matrix9 imuFactorResidualCov(
-    const gtsam::PreintegratedImuMeasurementsT<gtsam::ManifoldPreintegration>& pim) {
-  const gtsam::Matrix9 Sigma = pim.preintMeasCov();
-
-  const gtsam::Matrix3 R = pim.deltaRij().matrix();
-
-  // J is 9x9: identity except dp/dv blocks rotated by dR.
-  gtsam::Matrix9 J = gtsam::Matrix9::Identity();
-  J.block<3,3>(3,3) = R;   // dp
-  J.block<3,3>(6,6) = R;   // dv
-
-  gtsam::Matrix9 noting;
-  noting.noalias() = J * Sigma * J.transpose();
-  return noting;
-}
-
-// Fallback for other PIM types.
 template <class PIM>
 inline gtsam::Matrix9 imuFactorResidualCov(const PIM& pim) {
   return pim.preintMeasCov();

gtsam/navigation/ManifoldPreintegration.cpp

@@ -78,7 +78,27 @@ void ManifoldPreintegration::update(const Vector3& measuredAcc,
 
   // Do update
   deltaTij_ += dt;
-  deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional
+
+  // 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;
 
   if (p().body_P_sensor) {
     // More complicated derivatives in case of non-trivial sensor pose

gtsam/navigation/tests/imuFactorTesting.h

@@ -131,8 +131,14 @@ struct ImuSimConfig {
 inline std::shared_ptr<gtsam::PreintegrationParams> MakeParamsU(const ImuSimConfig& cfg) {
   auto p = gtsam::PreintegrationParams::MakeSharedU(cfg.g);
   const Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
-  p->gyroscopeCovariance     = (cfg.sigma_g_c * cfg.sigma_g_c) * I;
-  p->accelerometerCovariance = (cfg.sigma_a_c * cfg.sigma_a_c) * I;
+  double cov_g_c = cfg.sigma_g_c * cfg.sigma_g_c;
+  double cov_a_c = cfg.sigma_a_c * cfg.sigma_a_c;
+  p->gyroscopeCovariance     = cov_g_c * I;
+  p->accelerometerCovariance = cov_a_c * I;
+  p->gyroscopeCovariance(0, 0) *= 10000;
+  p->gyroscopeCovariance(1, 1) *= 100;
+  p->accelerometerCovariance(0, 0) *= 10000;
+  p->accelerometerCovariance(1, 1) *= 100;
   p->integrationCovariance   = 1e-12 * I;
   p->use2ndOrderCoriolis     = false;
   return p;

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -321,8 +321,14 @@ MakeParamsU_Combined(const ImuSimConfig& cfg) {
       gtsam::Vector3(0.0, 0.0, -cfg.g));
 
   const gtsam::Matrix3 I = gtsam::Matrix3::Identity();
-  p->gyroscopeCovariance      = (cfg.sigma_g_c * cfg.sigma_g_c) * I;
-  p->accelerometerCovariance  = (cfg.sigma_a_c * cfg.sigma_a_c) * I;
+  double cov_g_c = cfg.sigma_g_c * cfg.sigma_g_c;
+  double cov_a_c = cfg.sigma_a_c * cfg.sigma_a_c;
+  p->gyroscopeCovariance     = cov_g_c * I;
+  p->accelerometerCovariance = cov_a_c * I;
+  p->gyroscopeCovariance(0, 0) *= 10000;
+  p->gyroscopeCovariance(1, 1) *= 100;
+  p->accelerometerCovariance(0, 0) *= 10000;
+  p->accelerometerCovariance(1, 1) *= 100;
   p->integrationCovariance    = 1e-12 * I;
 
   // Turn off bias random walk so we can compare to 9D EKF (no bias state)
@@ -381,10 +387,7 @@ static inline Maps15Nav BuildMaps15Nav_Manifold(
   m.G.block<3,3>(12,12) = -I;
 
   // ---- covG: covariance-error definition ----
-  // like 9D manifold: first 9 injected as identity in nav error coords
-  m.covG.setIdentity();
-  m.covG.block<3,3>(9,9)   = -I;
-  m.covG.block<3,3>(12,12) = -I;
+  m.covG = m.G;
 
   return m;
 }

gtsam/navigation/tests/testImuFactor.cpp

@@ -925,7 +925,7 @@ static inline Maps9 BuildMaps9_Manifold(
   m.G.block<3,3>(3,3) = A;
   m.G.block<3,3>(6,6) = A;
 
-  m.covG.setIdentity();
+  m.covG = m.G;
 
   m.G_inv = m.G.transpose();       // because blocks are I/A and A is orthonormal
   m.covG_inv = m.covG;             // Identity