Fix/covariances

gtsam/navigation/CombinedImuFactor.h

@@ -193,6 +193,11 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurementsT : public PreintegrationTyp
 // For backward compatibility:
 using PreintegratedCombinedMeasurements = PreintegratedCombinedMeasurementsT<DefaultPreintegrationType>;
 
+template <class PIM>
+inline Eigen::Matrix<double, 15, 15> combinedImuFactorResidualCov(const PIM& pim) {
+  return pim.preintMeasCov();
+}
+
 /**
  * CombinedImuFactor is a 6-ways factor involving previous state (pose and
  * velocity of the vehicle, as well as bias at previous time step), and current
@@ -247,7 +252,7 @@ class GTSAM_EXPORT CombinedImuFactorT
   CombinedImuFactorT(
       Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
       const PIM& preintegratedMeasurements)
-      : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov()),
+      : Base(noiseModel::Gaussian::Covariance(combinedImuFactorResidualCov(preintegratedMeasurements)),
              pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
         pim_(preintegratedMeasurements) {}
 

gtsam/navigation/ImuFactor.h

@@ -173,6 +173,24 @@ 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();
+}
+
 /**
  * ImuFactor is a 5-ways factor involving previous state (pose and velocity of
  * the vehicle at previous time step), current state (pose and velocity at
@@ -220,7 +238,7 @@ class GTSAM_EXPORT ImuFactorT: public NoiseModelFactorN<Pose3, Vector3, Pose3, V
    */
   ImuFactorT(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
       const PIM& preintegratedMeasurements)
-      : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov()),
+      : Base(noiseModel::Gaussian::Covariance(imuFactorResidualCov(preintegratedMeasurements)),
              pose_i, vel_i, pose_j, vel_j, bias),
         pim_(preintegratedMeasurements) {}
 
@@ -378,7 +396,7 @@ class GTSAM_EXPORT ImuFactor2T : public NoiseModelFactorN<NavState, NavState, im
    */
   ImuFactor2T(Key state_i, Key state_j, Key bias,
              const PIM& preintegratedMeasurements)
-      : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov()),
+      : Base(noiseModel::Gaussian::Covariance(imuFactorResidualCov(preintegratedMeasurements)),
              state_i, state_j, bias),
         pim_(preintegratedMeasurements) {}
 

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,7 +192,7 @@ void ManifoldPreintegration::update(const Vector3& measuredAcc,
 
   // Do update
   deltaTij_ += dt;
-  deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional
+  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/imuFactorTesting.h

@@ -19,6 +19,7 @@
 
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/navigation/ImuBias.h>
+#include <iostream>
 
 using namespace std;
 using namespace gtsam;
@@ -45,6 +46,128 @@ auto radians = [](double t) { return t * M_PI / 180; };
 [[maybe_unused]] static const double kGyroSigma =
     radians(0.5) / 60;                                        // 0.5 degree ARW
 [[maybe_unused]] static const double kAccelSigma = 0.1 / 60;  // 10 cm VRW
+
+// --------------------------- small helpers ---------------------------
+// Rotation near: angle of R0^{-1} R1 should be <= tol_rad.
+#define EXPECT_ROT3_NEAR(R0, R1, tol_rad)                                         \
+  do {                                                                            \
+    const gtsam::Vector3 _w = gtsam::Rot3::Logmap((R0).between((R1)));            \
+    const double _w_norm = _w.norm();                                             \
+    if (_w_norm > (tol_rad)) {                                                    \
+      std::cout << "EXPECT_ROT3_NEAR failed\n"                                    \
+                << "  w: [" << _w.transpose() << "]\n"                            \
+                << "  w_norm: " << _w_norm << " tol: " << (tol_rad) << "\n"       \
+                << "  R0:\n" << (R0).matrix() << "\n"                             \
+                << "  R1:\n" << (R1).matrix() << "\n";                            \
+    }                                                                             \
+    EXPECT(_w_norm <= (tol_rad));                                                 \
+  } while (0)
+
+// Robust-ish numeric compare for matrices, with abs+rel tolerance on Fro norm.
+// Passes if ||A-B||_F <= abs_tol + rel_tol * max(1, ||B||_F)
+#define EXPECT_MAT_NEAR(A, B, abs_tol, rel_tol)                                   \
+  do {                                                                            \
+    const auto& _A = (A);                                                         \
+    const auto& _B = (B);                                                         \
+    const double _bnorm = _B.norm();                                              \
+    const double _denom = std::max(1.0, _bnorm);                                  \
+    const double _err   = (_A - _B).norm();                                       \
+    const double _tol = (abs_tol) + (rel_tol) * _denom;                           \
+    if (_err > _tol) {                                                            \
+      std::cout << "EXPECT_MAT_NEAR failed\n"                                     \
+                << "  err: " << _err << " tol: " << _tol                          \
+                << " (abs: " << (abs_tol) << ", rel: " << (rel_tol) << ", bnorm: "\
+                << _bnorm << ")\n"                                                \
+                << "  A:\n" << _A << "\n"                                         \
+                << "  B:\n" << _B << "\n";                                        \
+    }                                                                             \
+    EXPECT(_err <= _tol);                                                         \
+  } while (0)
+
+struct ImuSimConfig {
+  double g = 9.81;
+  double dt = 0.005;
+
+  // Noise model used inside preintegration/ekf (white noise)
+  double sigma_g_c = 5e-2;
+  double sigma_a_c = 3e-2;
+  double sigma_gw_c = 7e-3;
+  double sigma_aw_c = 2e-3;
+
+  gtsam::Rot3  Rws0;
+  gtsam::Point3 pws0;
+  gtsam::Vector3 vws0;
+  gtsam::imuBias::ConstantBias bias;  // (acc, gyro)
+
+  // NAV9 covariance (order: [dtheta, dp, dv])
+  Eigen::Matrix<double,9,9> P0_nav9;
+
+  explicit ImuSimConfig(unsigned int seed = 0, bool zero_bw = true) {
+    std::mt19937 rng(seed);
+    std::normal_distribution<double> N01(0.0, 1.0);
+    auto n = [&](){ return N01(rng); };
+
+    const gtsam::Vector3 w0(0.2*n(), 0.2*n(), 0.2*n());
+    Rws0 = gtsam::Rot3::Expmap(w0);
+
+    pws0 = gtsam::Point3(n(), n(), n());
+    vws0 = gtsam::Vector3(n(), n(), n());
+
+    const gtsam::Vector3 ba0(0.05*n(), 0.05*n(), 0.05*n());
+    const gtsam::Vector3 bg0(0.01*n(), 0.01*n(), 0.01*n());
+    bias = gtsam::imuBias::ConstantBias(ba0, bg0);
+
+    P0_nav9.setZero();
+    P0_nav9.diagonal().segment<3>(0).setConstant(1.0);     // rot
+    P0_nav9.diagonal().segment<3>(3).setConstant(100.0);   // pos
+    P0_nav9.diagonal().segment<3>(6).setConstant(10.0);    // vel
+    if (zero_bw) {
+      sigma_gw_c = 0;
+      sigma_aw_c = 0;
+    }
+  }
+};
+
+inline std::shared_ptr<gtsam::PreintegrationParams> MakeParamsU(const ImuSimConfig& cfg) {
+  auto p = gtsam::PreintegrationParams::MakeSharedU(cfg.g);
+  const Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
+  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;
+}
+
+struct ImuRawSample {
+  gtsam::Vector3 measuredOmega;
+  gtsam::Vector3 measuredAcc;
+  double dt;
+};
+
+inline std::vector<ImuRawSample> MakeRandomImuMeasurements(
+    const ImuSimConfig& cfg,
+    double T,
+    unsigned int seed) {
+  const int N = static_cast<int>(std::round(T / cfg.dt));
+  std::vector<ImuRawSample> out;
+  if (N <= 0) return out;
+  out.reserve(N);
+
+  const Vector3 acc_mean = Vector3(0.0, 0.0, cfg.g);
+  for (int k = 0; k < N; ++k) {
+    Eigen::Vector3d omega = Eigen::Vector3d::Random();  // range from [-1, 1]
+    Eigen::Vector3d acc = Eigen::Vector3d::Random();
+    acc += acc_mean;
+    out.push_back(ImuRawSample{omega, acc, cfg.dt});
+  }
+  return out;
+}
 }  // namespace
 
 namespace testing {