diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 14548eafd..f23e41ec8 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -49,10 +49,9 @@ 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); + Matrix3 Fr; + PreintegratedRotation::integrateGyroMeasurement(measuredOmega, biasHat_, + deltaT, &Fr); // First order uncertainty propagation // The deltaT allows to pass from continuous time noise to discrete time diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index 720593558..bbc607800 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/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( - const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, - OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, +void PreintegratedRotation::integrateGyroMeasurement( + const Vector3& measuredOmega, const Vector3& bias, double deltaT, 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); - - // If asked, pass first derivative as well - if (optional_D_incrR_integratedOmega) { - *optional_D_incrR_integratedOmega << D_incrR_integratedOmega; - } + const Rot3 incrR = f(bias, H_bias); // Update deltaTij and rotation deltaTij_ += deltaT; @@ -109,8 +107,23 @@ void PreintegratedRotation::integrateMeasurement( // Update Jacobian const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = - incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT; + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + H_bias; +} + +// 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, diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index d5f930063..b87f4208b 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/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 - * rotation w.r.t. the integrated angular velocity - * @param F Optional Jacobian of the incremental rotation w.r.t. the bias - * estimate + * @param F Jacobian of internal compose, used in AhrsFactor. */ - void integrateMeasurement(const Vector3& measuredOmega, - const Vector3& biasHat, double deltaT, - OptionalJacobian<3, 3> D_incrR_integratedOmega = {}, - OptionalJacobian<3, 3> F = {}); + 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, @@ -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 /// @} diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index 89b714534..8e7e4e9e5 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -27,18 +27,28 @@ using namespace gtsam; //****************************************************************************** -namespace simple_roll { -auto p = std::make_shared(); -PreintegratedRotation pim(p); +// Example where gyro measures small rotation about x-axis, with bias. +namespace biased_x_rotation { const double omega = 0.1; -const Vector3 measuredOmega(omega, 0, 0); -const Vector3 bias(0, 0, 0); +const Vector3 trueOmega(omega, 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 paramsWithTransform() { + auto p = std::make_shared(); + p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}}); + return p; +} //****************************************************************************** -TEST(PreintegratedRotation, IncrementalRotation) { - using namespace simple_roll; +TEST(PreintegratedRotation, integrateMeasurement) { + // Example where IMU is identical to body frame, then omega is roll + using namespace biased_x_rotation; + auto p = std::make_shared(); + PreintegratedRotation pim(p); // Check the value. Matrix3 H_bias; @@ -51,6 +61,32 @@ TEST(PreintegratedRotation, IncrementalRotation) { // Check the derivative: EXPECT(assert_equal(numericalDerivative11(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(pim.deltaRij().inverse().AdjointMap(), F)); + + // Make sure delRdelBiasOmega is H_bias after integration. + EXPECT(assert_equal(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(); + 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( - numericalDerivative22(g, measuredOmega, bias), - -deltaT * D_incrR_integratedOmega)); -} + const Matrix3 oldJacobian = + numericalDerivative22(g, measuredOmega, bias); + EXPECT(assert_equal(oldJacobian, -deltaT * D_incrR_integratedOmega)); -//****************************************************************************** -static std::shared_ptr paramsWithTransform() { - auto p = std::make_shared(); - p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}}); - return p; -} + // 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)); -namespace roll_in_rotated_frame { -auto p = paramsWithTransform(); -PreintegratedRotation pim(p); -const double omega = 0.1; -const Vector3 measuredOmega(omega, 0, 0); -const Vector3 bias(0, 0, 0); -const double deltaT = 0.5; -} // namespace roll_in_rotated_frame + // Check that system matrix F is the first derivative of compose: + EXPECT(assert_equal(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(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(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(pim.deltaRij().inverse().AdjointMap(), F)); + + // Make sure delRdelBiasOmega is H_bias after integration. + EXPECT(assert_equal(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( - numericalDerivative22(g, measuredOmega, bias), - -deltaT * D_incrR_integratedOmega)); + const Matrix3 oldJacobian = + numericalDerivative22(g, measuredOmega, bias); + EXPECT(assert_equal(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(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(H_bias, pim.delRdelBiasOmega())); } //******************************************************************************