biascorrectedDeltaRij tests

Author: dellaert

gtsam/navigation/PreintegratedRotation.cpp

@@ -95,11 +95,11 @@ Rot3 PreintegratedRotation::IncrementalRotation::operator()(
 }
 
 void PreintegratedRotation::integrateGyroMeasurement(
-    const Vector3& measuredOmega, const Vector3& bias, double deltaT,
+    const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
     OptionalJacobian<3, 3> F) {
   Matrix3 H_bias;
   IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
-  const Rot3 incrR = f(bias, H_bias);
+  const Rot3 incrR = f(biasHat, H_bias);
 
   // Update deltaTij and rotation
   deltaTij_ += deltaT;
@@ -112,16 +112,16 @@ void PreintegratedRotation::integrateGyroMeasurement(
 
 // deprecated!
 void PreintegratedRotation::integrateMeasurement(
-    const Vector3& measuredOmega, const Vector3& bias, double deltaT,
+    const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
     OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
     OptionalJacobian<3, 3> F) {
-  integrateGyroMeasurement(measuredOmega, bias, deltaT, F);
+  integrateGyroMeasurement(measuredOmega, biasHat, 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);
+    const Rot3 incrR = f(biasHat, H_bias);
     *optional_D_incrR_integratedOmega << H_bias / -deltaT;
   }
 }

gtsam/navigation/PreintegratedRotation.h

@@ -135,18 +135,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
@@ -158,6 +150,35 @@ class GTSAM_EXPORT PreintegratedRotation {
   /// @name Main functionality
   /// @{
 
+  /**
+   * @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 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 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;
+
+  /// Integrate coriolis correction in body frame rot_i
+  Vector3 integrateCoriolis(const Rot3& rot_i) const;
+
+  /// @}
+
+  /// @name Internal, exposed for testing only
+  /// @{
+
   /**
    * @brief Function object for incremental rotation.
    * @param measuredOmega The measured angular velocity (as given by the sensor)
@@ -179,25 +200,6 @@ class GTSAM_EXPORT PreintegratedRotation {
                     OptionalJacobian<3, 3> H_bias = {}) const;
   };
 
-  /**
-   * @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 bias estimate
-   * @param deltaT The time interval
-   * @param F Jacobian of internal compose, used in AhrsFactor.
-   */
-  void integrateGyroMeasurement(const Vector3& measuredOmega,
-                                const Vector3& bias, double deltaT,
-                                OptionalJacobian<3, 3> F = {});
-
-  /// Return a bias corrected version of the integrated rotation, with optional Jacobian
-  Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
-                             OptionalJacobian<3, 3> H = {}) const;
-
-  /// Integrate coriolis correction in body frame rot_i
-  Vector3 integrateCoriolis(const Rot3& rot_i) const;
-
   /// @}
 
   /// @name Deprecated API
@@ -215,9 +217,10 @@ class GTSAM_EXPORT PreintegratedRotation {
     return incrR;
   }
 
-  /// @deprecated: returned hard-to-understand Jacobian D_incrR_integratedOmega.
+  /// @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& bias, double deltaT,
+      const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
       OptionalJacobian<3, 3> D_incrR_integratedOmega, OptionalJacobian<3, 3> F);
 
 #endif

gtsam/navigation/tests/testAHRSFactor.cpp

@@ -301,29 +301,6 @@ TEST(AHRSFactor, FirstOrderPreIntegratedMeasurements) {
   // 1e-3 needs to be added only when using quaternions for rotations
 }
 
-//******************************************************************************
-// TEST(AHRSFactor, PimWithBodyDisplacement) {
-//   Vector3 bias(0, 0, 0.3);
-//   Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3);
-//   double deltaT = 1.0;
-
-//   auto p = std::make_shared<PreintegratedAhrsMeasurements::Params>();
-//   p->gyroscopeCovariance = kMeasuredOmegaCovariance;
-//   p->body_P_sensor = Pose3(Rot3::Roll(M_PI_2), Point3(0, 0, 0));
-//   PreintegratedAhrsMeasurements pim(p, Vector3::Zero());
-
-//   pim.integrateMeasurement(measuredOmega, deltaT);
-
-//   Vector3 biasOmegaIncr(0.01, 0.0, 0.0);
-//   Matrix3 actualH;
-//   pim.biascorrectedDeltaRij(biasOmegaIncr, actualH);
-
-//   // Numerical derivative using a lambda function:
-//   auto f = [&](const Vector3& bias) { return pim.biascorrectedDeltaRij(bias); };
-//   Matrix3 expectedH = numericalDerivative11<Rot3, Vector3>(f, bias);
-//   EXPECT(assert_equal(expectedH, actualH));
-// }
-
 //******************************************************************************
 TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
   Vector3 bias(0, 0, 0.3);

gtsam/navigation/tests/testPreintegratedRotation.cpp

@@ -71,6 +71,22 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) {
 
   // Make sure delRdelBiasOmega is H_bias after integration.
   EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
+
+  // Check if we make a correction to the bias, the value and Jacobian are
+  // correct. Note that the bias is subtracted from the measurement, and the
+  // integration time is taken into account, so we expect -deltaT*delta change.
+  Matrix3 H;
+  const double delta = 0.05;
+  const Vector3 biasOmegaIncr(delta, 0, 0);
+  Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
+  EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected));
+  EXPECT(assert_equal(Rot3::Roll((omega - delta) * deltaT), corrected, 1e-9));
+
+  // TODO(frank): again the derivative is not the *sane* one!
+  // auto g = [&](const Vector3& increment) {
+  //   return pim.biascorrectedDeltaRij(increment, {});
+  // };
+  // EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(g, Z_3x1), H));
 }
 
 //******************************************************************************
@@ -100,6 +116,20 @@ TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) {
 
   // Make sure delRdelBiasOmega is H_bias after integration.
   EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
+
+  // Check the bias correction in same way, but will now yield pitch change.
+  Matrix3 H;
+  const double delta = 0.05;
+  const Vector3 biasOmegaIncr(delta, 0, 0);
+  Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
+  EQUALITY(Vector3(0, -deltaT * delta, 0), expected.logmap(corrected));
+  EXPECT(assert_equal(Rot3::Pitch((omega - delta) * deltaT), corrected, 1e-9));
+
+  // TODO(frank): again the derivative is not the *sane* one!
+  // auto g = [&](const Vector3& increment) {
+  //   return pim.biascorrectedDeltaRij(increment, {});
+  // };
+  // EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(g, Z_3x1), H));
 }
 
 //******************************************************************************