Add preintegration on Gal(3) (W.I.P.)

- no IMU biases
- preintegration covariance propagation is for RI error

Author: giuliodelama

gtsam/navigation/GalileanImuFactor.cpp

@@ -27,21 +27,65 @@ PreintegratedImuMeasurementsG::PreintegratedImuMeasurementsG(
 
 // -- Integration API ---------------------------------------------------------
 void PreintegratedImuMeasurementsG::resetIntegration() {
-  throw std::runtime_error("Not implemented: resetIntegration");
+  preintMatrix_ = Gal3().Identity();
+  preintMeasCov_.setZero();
+  preintBiasJacobian_.setZero();
 }
 
 void PreintegratedImuMeasurementsG::integrateMeasurement(
     const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
-  throw std::runtime_error("Not implemented: integrateMeasurement");
+  // Correct measurements for bias and construct input
+  Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
+  Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
+  if (p_->body_P_sensor) {
+    const auto& bRs = p_->body_P_sensor->rotation();
+    acc = bRs * acc;
+    omega = bRs * omega;
+  }
+  Vector10 wInput = Vector10::Zero();
+  wInput.head<3>() = omega;
+  wInput.segment<3>(3) = acc;
+  wInput(9) = 1.0;  // time scale
+  Matrix10 Jl;
+  Gal3 wExp = Gal3::Expmap(-wInput * dt, Jl).inverse();
+  Matrix10 preintMatrixAdjoint = preintMatrix_.AdjointMap();
+
+  // Propagate mean - from eq. (22) in the paper
+  preintMatrix_ = preintMatrix_.compose(wExp);
+
+  // Propagate covariance - from eq. (35) in the paper
+  // IMPORTANT: the covariance propagation is based on the Right-Invariant (RI)
+  // error since the equivariant error (34) is by definition RI. If we want to
+  // use the Left-Invariant (LI) error to use NavState in the error formulation
+  // for the factor, we need to change the covariance propagation accordingly
+  // with the Adjoint. For RI error, the A_ matrix is identity.
+  Matrix10 B_ = preintMatrixAdjoint * Jl * dt;
+  Matrix10 Qd = Matrix10::Zero();
+  Qd.block<3, 3>(0, 0) = p_->gyroscopeCovariance / dt;
+  Qd.block<3, 3>(3, 3) = p_->accelerometerCovariance / dt;
+  preintMeasCov_.noalias() += B_ * Qd * B_.transpose();
+
+  // Propagate bias Jacobian - from eq. (38) in the paper
+  Matrix20 phiBiasJacobian = Matrix20::Identity();
+  phiBiasJacobian.block<10, 10>(9, 9) = -preintMatrixAdjoint * Jl * dt;
+  preintBiasJacobian_ = phiBiasJacobian * preintBiasJacobian_;
 }
 
 // -- print / equals ----------------------------------------------------------
 void PreintegratedImuMeasurementsG::print(const std::string& s) const {
-  throw std::runtime_error("Not implemented: print");
+  std::cout << (s.empty() ? s : s + "\n")
+            << "PreintegratedImuMeasurementsG:" << std::endl;
+  std::cout << "  preintMatrix_: " << preintMatrix_ << std::endl;
+  std::cout << "  preintMeasCov_: \n" << preintMeasCov_ << std::endl;
+  std::cout << "  preintBiasJacobian_: \n" << preintBiasJacobian_ << std::endl;
+  std::cout << "  biasHat_: " << biasHat_.vector().transpose() << std::endl;
 }
 bool PreintegratedImuMeasurementsG::equals(
     const PreintegratedImuMeasurementsG& o, double tol) const {
-  throw std::runtime_error("Not implemented: equals");
+  return p_->equals(*o.p_, tol) && biasHat_.equals(o.biasHat_, tol) &&
+         preintMatrix_.equals(o.preintMatrix_, tol) &&
+         equal_with_abs_tol(preintMeasCov_, o.preintMeasCov_, tol) &&
+         equal_with_abs_tol(preintBiasJacobian_, o.preintBiasJacobian_, tol);
 }
 
 }  // namespace gtsam

gtsam/navigation/GalileanImuFactor.h

@@ -32,11 +32,12 @@
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/basis/Chebyshev2.h>
+#include <gtsam/geometry/Gal3.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/navigation/ImuFactor.h>
 #include <gtsam/navigation/NavState.h>
 #include <gtsam/navigation/PreintegrationBase.h>
-#include <gtsam/navigation/PreintegrationParams.h>
+#include <gtsam/navigation/PreintegrationCombinedParams.h>
 
 #include <optional>
 #include <vector>
@@ -47,8 +48,17 @@ namespace gtsam {
 class GTSAM_EXPORT PreintegratedImuMeasurementsG : public PreintegrationBase {
   using Base = PreintegrationBase;
   using Params = PreintegrationBase::Params;
-
-  std::shared_ptr<Params> p_;
+  using Matrix10 = Eigen::Matrix<double, 10, 10>;
+  using Matrix20 = Eigen::Matrix<double, 20, 20>;
+
+ protected:
+  std::shared_ptr<Params> p_;  ///< Preintegration parameters
+  Gal3 preintMatrix_;  ///< Preintegration Matrix (Galilean group element)
+  Matrix20 preintBiasJacobian_;  ///< Jacobian w.r.t. bias states
+  Matrix10 preintMeasCov_;       ///< Preintegration covariance
+  ///< The dimension of the preintegration covariance matrix is 10x10
+  ///< Because it includes Galilean Group (n=10)
+  ///< preintROTATION, preintVELOCITY, preintPOSITION, preintTIME
 
  public:
   // Constructors (stubs)
@@ -57,29 +67,33 @@ class GTSAM_EXPORT PreintegratedImuMeasurementsG : public PreintegrationBase {
       const imuBias::ConstantBias& biasHat = {});
 
   // Public (const) accessors – stubs
-  double deltaTij() const {
-    throw std::runtime_error("Not implemented: deltaTij");
-  }
-  Rot3 deltaRij() const override {
-    throw std::runtime_error("Not implemented: deltaRij");
-  }
-  Vector3 deltaPij() const override {
-    throw std::runtime_error("Not implemented: deltaPij");
-  }
-  Vector3 deltaVij() const override {
-    throw std::runtime_error("Not implemented: deltaVij");
-  }
+  double deltaTij() const { return preintMatrix_.time(); }
+  Rot3 deltaRij() const override { return preintMatrix_.rotation(); }
+  Vector3 deltaPij() const override { return preintMatrix_.translation(); }
+  Vector3 deltaVij() const override { return preintMatrix_.velocity(); }
   NavState deltaXij() const override {
-    throw std::runtime_error("Not implemented: deltaXij");
-  }
-  Matrix9 preintMeasCov() const {
-    throw std::runtime_error("Not implemented: preintMeasCov");
+    return NavState(preintMatrix_.rotation(), preintMatrix_.translation(),
+                    preintMatrix_.velocity());
   }
+  Matrix9 preintMeasCov() const { return preintMeasCov_.block<9, 9>(0, 0); }
 
   // Bias correction (first‑order) – stub
   Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
                              OptionalJacobian<9, 6> H = {}) const override {
-    throw std::runtime_error("Not implemented: biasCorrectedDelta");
+    const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
+    Gal3 correctedPreintMatrix =
+        Gal3::Expmap(preintBiasJacobian_.block<10, 6>(0, 0) * biasIncr.vector())
+            .compose(preintMatrix_);
+
+    Vector9 xi;
+    NavState::dR(xi) = Rot3::Logmap(correctedPreintMatrix.rotation());
+    NavState::dP(xi) = correctedPreintMatrix.translation();
+    NavState::dV(xi) = correctedPreintMatrix.velocity();
+
+    if (H) {
+      throw std::runtime_error("Not implemented: biasCorrectedDelta Jacobian");
+    }
+    return xi;
   }
 
   // integration API
@@ -94,10 +108,9 @@ class GTSAM_EXPORT PreintegratedImuMeasurementsG : public PreintegrationBase {
   bool equals(const PreintegratedImuMeasurementsG& other,
               double tol = 1e-9) const;
 
-  void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
-              const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override {
-    throw std::runtime_error("Not implemented: update");
-  }
+  // void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
+  //             const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override
+  //             {}
 };
 
 // Tell GTSAM about PreintegratedImuMeasurementsG print/equals: