Fixed derivative with transform, deprecated integrateMeasurement

gtsam/navigation/AHRSFactor.cpp

@@ -49,10 +49,9 @@ void PreintegratedAhrsMeasurements::resetIntegration() {
 //------------------------------------------------------------------------------
 void PreintegratedAhrsMeasurements::integrateMeasurement(
     const Vector3& measuredOmega, double deltaT) {
-
+  Matrix3 Fr;
-  Matrix3 D_incrR_integratedOmega, Fr;
+  PreintegratedRotation::integrateGyroMeasurement(measuredOmega, biasHat_,
-  PreintegratedRotation::integrateMeasurement(measuredOmega,
+                                                  deltaT, &Fr);
-      biasHat_, deltaT, &D_incrR_integratedOmega, &Fr);
 
   // First order uncertainty propagation
   // The deltaT allows to pass from continuous time noise to discrete time

gtsam/navigation/PreintegratedRotation.cpp

@@ -74,34 +74,32 @@ Rot3 PreintegratedRotation::IncrementalRotation::operator()(
   Vector3 correctedOmega = measuredOmega - bias;
 
   // Then compensate for sensor-body displacement: we express the quantities
-  // (originally in the IMU frame) into the body frame
+  // (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) {
-    const Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
     // 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;
   Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias);  // expensive !!
-  if (H_bias)
+  if (H_bias) {
     *H_bias *= -deltaT;  // Correct so accurately reflects bias derivative
+    if (body_P_sensor) *H_bias *= body_R_sensor;
+  }
   return incrR;
 }
 
-void PreintegratedRotation::integrateMeasurement(
+void PreintegratedRotation::integrateGyroMeasurement(
-    const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
+    const Vector3& measuredOmega, const Vector3& bias, double deltaT,
-    OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
     OptionalJacobian<3, 3> F) {
-  Matrix3 D_incrR_integratedOmega;
+  Matrix3 H_bias;
   IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
-  const Rot3 incrR = f(biasHat, D_incrR_integratedOmega);
+  const Rot3 incrR = f(bias, H_bias);
-
-  // If asked, pass first derivative as well
-  if (optional_D_incrR_integratedOmega) {
-    *optional_D_incrR_integratedOmega << D_incrR_integratedOmega;
-  }
 
   // Update deltaTij and rotation
   deltaTij_ += deltaT;
@@ -109,8 +107,23 @@ void PreintegratedRotation::integrateMeasurement(
 
   // Update Jacobian
   const Matrix3 incrRt = incrR.transpose();
-  delRdelBiasOmega_ =
+  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + H_bias;
-      incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT;
+}
+
+// deprecated!
+void PreintegratedRotation::integrateMeasurement(
+    const Vector3& measuredOmega, const Vector3& bias, double deltaT,
+    OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
+    OptionalJacobian<3, 3> F) {
+  integrateGyroMeasurement(measuredOmega, bias, deltaT, F);
+
+  // If asked, pass obsolete Jacobians as well
+  if (optional_D_incrR_integratedOmega) {
+    Matrix3 H_bias;
+    IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
+    const Rot3 incrR = f(bias, H_bias);
+    *optional_D_incrR_integratedOmega << H_bias / -deltaT;
+  }
 }
 
 Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,

gtsam/navigation/PreintegratedRotation.h

@@ -175,7 +175,7 @@ class GTSAM_EXPORT PreintegratedRotation {
      * @param H_bias Jacobian of the rotation w.r.t. bias.
      * @return The incremental rotation
      */
-    Rot3 operator()(const Vector3& biasHat,
+    Rot3 operator()(const Vector3& bias,
                     OptionalJacobian<3, 3> H_bias = {}) const;
   };
 
@@ -183,17 +183,13 @@ class GTSAM_EXPORT PreintegratedRotation {
    * @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 biasHat The bias estimate
+   * @param bias The bias estimate
    * @param deltaT The time interval
-   * @param D_incrR_integratedOmega Optional Jacobian of the incremental
+   * @param F Jacobian of internal compose, used in AhrsFactor.
-   * rotation w.r.t. the integrated angular velocity
-   * @param F Optional Jacobian of the incremental rotation w.r.t. the bias
-   * estimate
    */
-  void integrateMeasurement(const Vector3& measuredOmega,
+  void integrateGyroMeasurement(const Vector3& measuredOmega,
-                            const Vector3& biasHat, double deltaT,
+                                const Vector3& bias, double deltaT,
-                            OptionalJacobian<3, 3> D_incrR_integratedOmega = {},
+                                OptionalJacobian<3, 3> F = {});
-                            OptionalJacobian<3, 3> F = {});
 
   /// Return a bias corrected version of the integrated rotation, with optional Jacobian
   Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
@@ -210,14 +206,20 @@ class GTSAM_EXPORT PreintegratedRotation {
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
   /// @deprecated: use IncrementalRotation functor with sane Jacobian
   inline Rot3 GTSAM_DEPRECATED incrementalRotation(
-      const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
+      const Vector3& measuredOmega, const Vector3& bias, double deltaT,
       OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
     IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
-    Rot3 incrR = f(biasHat, D_incrR_integratedOmega);
+    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: returned hard-to-understand Jacobian D_incrR_integratedOmega.
+  void GTSAM_DEPRECATED integrateMeasurement(
+      const Vector3& measuredOmega, const Vector3& bias, double deltaT,
+      OptionalJacobian<3, 3> D_incrR_integratedOmega, OptionalJacobian<3, 3> F);
+
 #endif
 
   /// @}

gtsam/navigation/tests/testPreintegratedRotation.cpp

@@ -27,18 +27,28 @@
 using namespace gtsam;
 
 //******************************************************************************
-namespace simple_roll {
+// Example where gyro measures small rotation about x-axis, with bias.
-auto p = std::make_shared<PreintegratedRotationParams>();
+namespace biased_x_rotation {
-PreintegratedRotation pim(p);
 const double omega = 0.1;
-const Vector3 measuredOmega(omega, 0, 0);
+const Vector3 trueOmega(omega, 0, 0);
-const Vector3 bias(0, 0, 0);
+const Vector3 bias(1, 2, 3);
+const Vector3 measuredOmega = trueOmega + bias;
 const double deltaT = 0.5;
-}  // namespace simple_roll
+}  // namespace biased_x_rotation
+
+// Create params where x and y axes are exchanged.
+static std::shared_ptr<PreintegratedRotationParams> paramsWithTransform() {
+  auto p = std::make_shared<PreintegratedRotationParams>();
+  p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}});
+  return p;
+}
 
 //******************************************************************************
-TEST(PreintegratedRotation, IncrementalRotation) {
+TEST(PreintegratedRotation, integrateMeasurement) {
-  using namespace simple_roll;
+  // Example where IMU is identical to body frame, then omega is roll
+  using namespace biased_x_rotation;
+  auto p = std::make_shared<PreintegratedRotationParams>();
+  PreintegratedRotation pim(p);
 
   // Check the value.
   Matrix3 H_bias;
@@ -51,6 +61,32 @@ TEST(PreintegratedRotation, IncrementalRotation) {
   // Check the derivative:
   EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(f, bias), H_bias));
 
+  // Check value of deltaRij() after integration.
+  Matrix3 F;
+  pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F);
+  EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
+
+  // Check that system matrix F is the first derivative of compose:
+  EXPECT(assert_equal<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F));
+
+  // Make sure delRdelBiasOmega is H_bias after integration.
+  EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
+}
+
+//******************************************************************************
+TEST(PreintegratedRotation, Deprecated) {
+  // Example where IMU is identical to body frame, then omega is roll
+  using namespace biased_x_rotation;
+  auto p = std::make_shared<PreintegratedRotationParams>();
+  PreintegratedRotation pim(p);
+
+  // Check the value.
+  Matrix3 H_bias;
+  PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT,
+                                               p->getBodyPSensor()};
+  Rot3 expected = Rot3::Roll(omega * deltaT);
+  EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9));
+
   // Ephemeral test for deprecated Jacobian:
   Matrix3 D_incrR_integratedOmega;
   (void)pim.incrementalRotation(measuredOmega, bias, deltaT,
@@ -58,30 +94,32 @@ TEST(PreintegratedRotation, IncrementalRotation) {
   auto g = [&](const Vector3& x, const Vector3& y) {
     return pim.incrementalRotation(x, y, deltaT, {});
   };
-  EXPECT(assert_equal<Matrix3>(
+  const Matrix3 oldJacobian =
-      numericalDerivative22<Rot3, Vector3, Vector3>(g, measuredOmega, bias),
+      numericalDerivative22<Rot3, Vector3, Vector3>(g, measuredOmega, bias);
-      -deltaT * D_incrR_integratedOmega));
+  EXPECT(assert_equal<Matrix3>(oldJacobian, -deltaT * D_incrR_integratedOmega));
-}
 
-//******************************************************************************
+  // Check deprecated version.
-static std::shared_ptr<PreintegratedRotationParams> paramsWithTransform() {
+  Matrix3 D_incrR_integratedOmega2, F;
-  auto p = std::make_shared<PreintegratedRotationParams>();
+  pim.integrateMeasurement(measuredOmega, bias, deltaT,
-  p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}});
+                           D_incrR_integratedOmega2, F);
-  return p;
+  EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
-}
 
-namespace roll_in_rotated_frame {
+  // Check that system matrix F is the first derivative of compose:
-auto p = paramsWithTransform();
+  EXPECT(assert_equal<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F));
-PreintegratedRotation pim(p);
+
-const double omega = 0.1;
+  // Check that deprecated Jacobian is correct.
-const Vector3 measuredOmega(omega, 0, 0);
+  EXPECT(assert_equal(D_incrR_integratedOmega, D_incrR_integratedOmega2, 1e-9));
-const Vector3 bias(0, 0, 0);
+
-const double deltaT = 0.5;
+  // Make sure delRdelBiasOmega is H_bias after integration.
-}  // namespace roll_in_rotated_frame
+  EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
+}
 
 //******************************************************************************
 TEST(PreintegratedRotation, IncrementalRotationWithTransform) {
-  using namespace roll_in_rotated_frame;
+  // Example where IMU is rotated, so measured omega indicates pitch.
+  using namespace biased_x_rotation;
+  auto p = paramsWithTransform();
+  PreintegratedRotation pim(p);
 
   // Check the value.
   Matrix3 H_bias;
@@ -93,6 +131,32 @@ TEST(PreintegratedRotation, IncrementalRotationWithTransform) {
   // Check the derivative:
   EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(f, bias), H_bias));
 
+  // Check value of deltaRij() after integration.
+  Matrix3 F;
+  pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F);
+  EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
+
+  // Check that system matrix F is the first derivative of compose:
+  EXPECT(assert_equal<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F));
+
+  // Make sure delRdelBiasOmega is H_bias after integration.
+  EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
+}
+
+//******************************************************************************
+TEST(PreintegratedRotation, DeprecatedWithTransform) {
+  // Example where IMU is rotated, so measured omega indicates pitch.
+  using namespace biased_x_rotation;
+  auto p = paramsWithTransform();
+  PreintegratedRotation pim(p);
+
+  // Check the value.
+  Matrix3 H_bias;
+  PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT,
+                                               p->getBodyPSensor()};
+  Rot3 expected = Rot3::Pitch(omega * deltaT);
+  EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9));
+
   // Ephemeral test for deprecated Jacobian:
   Matrix3 D_incrR_integratedOmega;
   (void)pim.incrementalRotation(measuredOmega, bias, deltaT,
@@ -100,9 +164,24 @@ TEST(PreintegratedRotation, IncrementalRotationWithTransform) {
   auto g = [&](const Vector3& x, const Vector3& y) {
     return pim.incrementalRotation(x, y, deltaT, {});
   };
-  EXPECT(assert_equal<Matrix3>(
+  const Matrix3 oldJacobian =
-      numericalDerivative22<Rot3, Vector3, Vector3>(g, measuredOmega, bias),
+      numericalDerivative22<Rot3, Vector3, Vector3>(g, measuredOmega, bias);
-      -deltaT * D_incrR_integratedOmega));
+  EXPECT(assert_equal<Matrix3>(oldJacobian, -deltaT * D_incrR_integratedOmega));
+
+  // Check deprecated version.
+  Matrix3 D_incrR_integratedOmega2, F;
+  pim.integrateMeasurement(measuredOmega, bias, deltaT,
+                           D_incrR_integratedOmega2, F);
+  EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
+
+  // Check that system matrix F is the first derivative of compose:
+  EXPECT(assert_equal<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F));
+
+  // Check that deprecated Jacobian is correct.
+  EXPECT(assert_equal(D_incrR_integratedOmega, D_incrR_integratedOmega2, 1e-9));
+
+  // Make sure delRdelBiasOmega is H_bias after integration.
+  EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
 }
 
 //******************************************************************************