Correctly deal with sensor rotation in AHRS

.gitignore

@@ -19,3 +19,4 @@ CMakeLists.txt.user*
 xcode/
 /Dockerfile
 /python/gtsam/notebooks/.ipynb_checkpoints/ellipses-checkpoint.ipynb
+.cache/

gtsam/navigation/AHRSFactor.cpp

@@ -49,13 +49,14 @@ void PreintegratedAhrsMeasurements::resetIntegration() {
 //------------------------------------------------------------------------------
 void PreintegratedAhrsMeasurements::integrateMeasurement(
     const Vector3& measuredOmega, double deltaT) {
-
-  Matrix3 D_incrR_integratedOmega, Fr;
-  PreintegratedRotation::integrateMeasurement(measuredOmega,
-      biasHat_, deltaT, &D_incrR_integratedOmega, &Fr);
-
-  // first order uncertainty propagation
-  // the deltaT allows to pass from continuous time noise to discrete time noise
+  Matrix3 Fr;
+  PreintegratedRotation::integrateGyroMeasurement(measuredOmega, biasHat_,
+                                                  deltaT, &Fr);
+
+  // First order uncertainty propagation
+  // The deltaT allows to pass from continuous time noise to discrete time
+  // noise. Comparing with the IMUFactor.cpp implementation, the latter is an
+  // approximation for C * (wCov / dt) * C.transpose(), with C \approx I * dt.
   preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + p().gyroscopeCovariance * deltaT;
 }
 

gtsam/navigation/PreintegratedRotation.cpp

@@ -68,49 +68,66 @@ bool PreintegratedRotation::equals(const PreintegratedRotation& other,
       && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
 }
 
-Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
-    const Vector3& biasHat, double deltaT,
-    OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
-
+namespace internal {
+Rot3 IncrementalRotation::operator()(
+    const Vector3& bias, OptionalJacobian<3, 3> H_bias) const {
   // First we compensate the measurements for the bias
-  Vector3 correctedOmega = measuredOmega - biasHat;
+  Vector3 correctedOmega = measuredOmega - bias;
 
   // Then compensate for sensor-body displacement: we express the quantities
-  // (originally in the IMU frame) into the body frame
-  if (p_->body_P_sensor) {
-    Matrix3 body_R_sensor = p_->body_P_sensor->rotation().matrix();
+  // (originally in the IMU frame) into the body frame. If Jacobian is
+  // requested, the rotation matrix is obtained as `rotate` Jacobian.
+  Matrix3 body_R_sensor;
+  if (body_P_sensor) {
     // rotation rate vector in the body frame
-    correctedOmega = body_R_sensor * correctedOmega;
+    correctedOmega = body_P_sensor->rotation().rotate(
+        correctedOmega, {}, H_bias ? &body_R_sensor : nullptr);
   }
 
   // rotation vector describing rotation increment computed from the
   // current rotation rate measurement
   const Vector3 integratedOmega = correctedOmega * deltaT;
-  return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
+  Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias);  // expensive !!
+  if (H_bias) {
+    *H_bias *= -deltaT;  // Correct so accurately reflects bias derivative
+    if (body_P_sensor) *H_bias *= body_R_sensor;
+  }
+  return incrR;
 }
+}  // namespace internal
 
-void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
-    const Vector3& biasHat, double deltaT,
-    OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
+void PreintegratedRotation::integrateGyroMeasurement(
+    const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
     OptionalJacobian<3, 3> F) {
-  Matrix3 D_incrR_integratedOmega;
-  const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
-      D_incrR_integratedOmega);
-
-  // If asked, pass first derivative as well
-  if (optional_D_incrR_integratedOmega) {
-    *optional_D_incrR_integratedOmega << D_incrR_integratedOmega;
-  }
+  Matrix3 H_bias;
+  internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
+  const Rot3 incrR = f(biasHat, H_bias);
 
   // Update deltaTij and rotation
   deltaTij_ += deltaT;
   deltaRij_ = deltaRij_.compose(incrR, F);
 
   // Update Jacobian
   const Matrix3 incrRt = incrR.transpose();
-  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
-      - D_incrR_integratedOmega * deltaT;
+  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + H_bias;
+}
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
+void PreintegratedRotation::integrateMeasurement(
+    const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
+    OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
+    OptionalJacobian<3, 3> F) {
+  integrateGyroMeasurement(measuredOmega, biasHat, deltaT, F);
+
+  // If asked, pass obsolete Jacobians as well
+  if (optional_D_incrR_integratedOmega) {
+    Matrix3 H_bias;
+    internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
+    const Rot3 incrR = f(biasHat, H_bias);
+    *optional_D_incrR_integratedOmega << H_bias / -deltaT;
+  }
 }
+#endif
 
 Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
     OptionalJacobian<3, 3> H) const {

gtsam/navigation/PreintegratedRotation.h

@@ -21,12 +21,37 @@
 
 #pragma once
 
-#include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/std_optional_serialization.h>
+#include <gtsam/geometry/Pose3.h>
+#include "gtsam/dllexport.h"
 
 namespace gtsam {
 
+namespace internal {
+/**
+ * @brief Function object for incremental rotation.
+ * @param measuredOmega The measured angular velocity (as given by the sensor)
+ * @param deltaT The time interval over which the rotation is integrated.
+ * @param body_P_sensor Optional transform between body and IMU.
+ */
+struct GTSAM_EXPORT IncrementalRotation {
+  const Vector3& measuredOmega;
+  const double deltaT;
+  const std::optional<Pose3>& body_P_sensor;
+
+  /**
+   * @brief Integrate angular velocity, but corrected by bias.
+   * @param bias The bias estimate
+   * @param H_bias Jacobian of the rotation w.r.t. bias.
+   * @return The incremental rotation
+   */
+  Rot3 operator()(const Vector3& bias,
+                  OptionalJacobian<3, 3> H_bias = {}) const;
+};
+
+}  // namespace internal
+
 /// Parameters for pre-integration:
 /// Usage: Create just a single Params and pass a shared pointer to the constructor
 struct GTSAM_EXPORT PreintegratedRotationParams {
@@ -65,7 +90,6 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    namespace bs = ::boost::serialization;
     ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
     ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
 
@@ -136,18 +160,10 @@ class GTSAM_EXPORT PreintegratedRotation {
 
   /// @name Access instance variables
   /// @{
-  const std::shared_ptr<Params>& params() const {
-    return p_;
-  }
-  const double& deltaTij() const {
-    return deltaTij_;
-  }
-  const Rot3& deltaRij() const {
-    return deltaRij_;
-  }
-  const Matrix3& delRdelBiasOmega() const {
-    return delRdelBiasOmega_;
-  }
+  const std::shared_ptr<Params>& params() const { return p_; }
+  const double& deltaTij() const { return deltaTij_; }
+  const Rot3& deltaRij() const { return deltaRij_; }
+  const Matrix3& delRdelBiasOmega() const { return delRdelBiasOmega_; }
   /// @}
 
   /// @name Testable
@@ -159,19 +175,24 @@ class GTSAM_EXPORT PreintegratedRotation {
   /// @name Main functionality
   /// @{
 
-  /// Take the gyro measurement, correct it using the (constant) bias estimate
-  /// and possibly the sensor pose, and then integrate it forward in time to yield
-  /// an incremental rotation.
-  Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
-                           OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
-
-  /// Calculate an incremental rotation given the gyro measurement and a time interval,
-  /// and update both deltaTij_ and deltaRij_.
-  void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
-                            OptionalJacobian<3, 3> D_incrR_integratedOmega = {},
-                            OptionalJacobian<3, 3> F = {});
-
-  /// Return a bias corrected version of the integrated rotation, with optional Jacobian
+  /**
+   * @brief Calculate an incremental rotation given the gyro measurement and a
+   * time interval, and update both deltaTij_ and deltaRij_.
+   * @param measuredOmega The measured angular velocity (as given by the sensor)
+   * @param bias The biasHat estimate
+   * @param deltaT The time interval
+   * @param F optional Jacobian of internal compose, used in AhrsFactor.
+   */
+  void integrateGyroMeasurement(const Vector3& measuredOmega,
+                                const Vector3& biasHat, double deltaT,
+                                OptionalJacobian<3, 3> F = {});
+
+  /**
+   * @brief Return a bias corrected version of the integrated rotation.
+   * @param biasOmegaIncr An increment with respect to biasHat used above.
+   * @param H optional Jacobian of the correction w.r.t. the bias increment.
+   * @note The *key* functionality of this class used in optimizing the bias.
+   */
   Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
                              OptionalJacobian<3, 3> H = {}) const;
 
@@ -180,6 +201,31 @@ class GTSAM_EXPORT PreintegratedRotation {
 
   /// @}
 
+  /// @name Deprecated API
+  /// @{
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
+  /// @deprecated: use IncrementalRotation functor with sane Jacobian
+  inline Rot3 GTSAM_DEPRECATED incrementalRotation(
+      const Vector3& measuredOmega, const Vector3& bias, double deltaT,
+      OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
+    internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
+    Rot3 incrR = f(bias, D_incrR_integratedOmega);
+    // Backwards compatible "weird" Jacobian, no longer used.
+    if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT;
+    return incrR;
+  }
+
+  /// @deprecated: use integrateGyroMeasurement from now on
+  /// @note this returned hard-to-understand Jacobian D_incrR_integratedOmega.
+  void GTSAM_DEPRECATED integrateMeasurement(
+      const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
+      OptionalJacobian<3, 3> D_incrR_integratedOmega, OptionalJacobian<3, 3> F);
+
+#endif
+
+  /// @}
+
  private:
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */