diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index bbc607800..b10d8f772 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/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; } } diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index b87f4208b..a36674062 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -135,18 +135,10 @@ class GTSAM_EXPORT PreintegratedRotation { /// @name Access instance variables /// @{ - const std::shared_ptr& 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() 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 diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 9d512d595..95674ce78 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/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(); -// 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(f, bias); -// EXPECT(assert_equal(expectedH, actualH)); -// } - //****************************************************************************** TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) { Vector3 bias(0, 0, 0.3); diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index bc51bf9d3..b6486d030 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -71,6 +71,22 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) { // Make sure delRdelBiasOmega is H_bias after integration. EXPECT(assert_equal(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(g, Z_3x1), H)); } //****************************************************************************** @@ -100,6 +116,20 @@ TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) { // Make sure delRdelBiasOmega is H_bias after integration. EXPECT(assert_equal(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(g, Z_3x1), H)); } //******************************************************************************