From 2dad81d671247c5b7d2ba8ad5688c16108193bd4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 08:59:48 -0700 Subject: [PATCH] Function object with sane Jacobian --- gtsam/navigation/PreintegratedRotation.cpp | 21 ++--- gtsam/navigation/PreintegratedRotation.h | 48 ++++++++--- .../tests/testPreintegratedRotation.cpp | 81 +++++++++---------- 3 files changed, 83 insertions(+), 67 deletions(-) diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index cd8f2b64c..720593558 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -68,17 +68,15 @@ 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 { - +Rot3 PreintegratedRotation::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(); + 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; } @@ -86,7 +84,10 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega, // 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 + return incrR; } void PreintegratedRotation::integrateMeasurement( @@ -94,8 +95,8 @@ void PreintegratedRotation::integrateMeasurement( OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, OptionalJacobian<3, 3> F) { Matrix3 D_incrR_integratedOmega; - const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT, - D_incrR_integratedOmega); + 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) { diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index ed7b6f29c..d5f930063 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -21,9 +21,9 @@ #pragma once -#include #include #include +#include namespace gtsam { @@ -159,19 +159,25 @@ class GTSAM_EXPORT PreintegratedRotation { /// @{ /** - * @brief 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. + * @brief Function object for incremental rotation. * @param measuredOmega The measured angular velocity (as given by the sensor) - * @param biasHat The bias estimate - * @param deltaT The time interval - * @param D_incrR_integratedOmega Jacobian of the incremental rotation w.r.t. - * delta_T * (measuredOmega - biasHat), possibly rotated by body_R_sensor. - * @return The incremental rotation + * @param deltaT The time interval over which the rotation is integrated. + * @param body_P_sensor Optional transform between body and IMU. */ - Rot3 incrementalRotation( - const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, - OptionalJacobian<3, 3> D_incrR_integratedOmega) const; + struct IncrementalRotation { + const Vector3& measuredOmega; + const double deltaT; + const std::optional& 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& biasHat, + OptionalJacobian<3, 3> H_bias = {}) const; + }; /** * @brief Calculate an incremental rotation given the gyro measurement and a @@ -198,6 +204,24 @@ 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& biasHat, double deltaT, + OptionalJacobian<3, 3> D_incrR_integratedOmega) const { + IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; + Rot3 incrR = f(biasHat, D_incrR_integratedOmega); + // Backwards compatible "weird" Jacobian, no longer used. + if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT; + return incrR; + } +#endif + + /// @} + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index e264a6929..89b714534 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -30,40 +30,36 @@ using namespace gtsam; namespace simple_roll { auto p = std::make_shared(); PreintegratedRotation pim(p); -const double roll = 0.1; -const Vector3 measuredOmega(roll, 0, 0); -const Vector3 biasHat(0, 0, 0); +const double omega = 0.1; +const Vector3 measuredOmega(omega, 0, 0); +const Vector3 bias(0, 0, 0); const double deltaT = 0.5; } // namespace simple_roll //****************************************************************************** -TEST(PreintegratedRotation, incrementalRotation) { +TEST(PreintegratedRotation, IncrementalRotation) { using namespace simple_roll; // Check the value. - Matrix3 D_incrR_integratedOmega; - const Rot3 incrR = pim.incrementalRotation(measuredOmega, biasHat, deltaT, - D_incrR_integratedOmega); - Rot3 expected = Rot3::Roll(roll * deltaT); + Matrix3 H_bias; + PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, + p->getBodyPSensor()}; + const Rot3 incrR = f(bias, H_bias); + Rot3 expected = Rot3::Roll(omega * deltaT); EXPECT(assert_equal(expected, incrR, 1e-9)); - // Lambda for numerical derivative: - auto f = [&](const Vector3& x, const Vector3& y) { + // Check the derivative: + EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)); + + // Ephemeral test for deprecated Jacobian: + Matrix3 D_incrR_integratedOmega; + (void)pim.incrementalRotation(measuredOmega, bias, deltaT, + D_incrR_integratedOmega); + auto g = [&](const Vector3& x, const Vector3& y) { return pim.incrementalRotation(x, y, deltaT, {}); }; - - // NOTE(frank): these derivatives as computed by the function violate the - // "Jacobian contract". We should refactor this. It's not clear that the - // deltaT factor is actually understood in calling code. - - // Check derivative with respect to measuredOmega EXPECT(assert_equal( - numericalDerivative21(f, measuredOmega, biasHat), - deltaT * D_incrR_integratedOmega)); - - // Check derivative with respect to biasHat - EXPECT(assert_equal( - numericalDerivative22(f, measuredOmega, biasHat), + numericalDerivative22(g, measuredOmega, bias), -deltaT * D_incrR_integratedOmega)); } @@ -77,40 +73,35 @@ static std::shared_ptr paramsWithTransform() { namespace roll_in_rotated_frame { auto p = paramsWithTransform(); PreintegratedRotation pim(p); -const double roll = 0.1; -const Vector3 measuredOmega(roll, 0, 0); -const Vector3 biasHat(0, 0, 0); +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 //****************************************************************************** -TEST(PreintegratedRotation, incrementalRotationWithTransform) { +TEST(PreintegratedRotation, IncrementalRotationWithTransform) { using namespace roll_in_rotated_frame; // Check the value. - Matrix3 D_incrR_integratedOmega; - const Rot3 incrR = pim.incrementalRotation(measuredOmega, biasHat, deltaT, - D_incrR_integratedOmega); - Rot3 expected = Rot3::Pitch(roll * deltaT); - EXPECT(assert_equal(expected, incrR, 1e-9)); + 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)); - // Lambda for numerical derivative: - auto f = [&](const Vector3& x, const Vector3& y) { + // Check the derivative: + EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)); + + // Ephemeral test for deprecated Jacobian: + Matrix3 D_incrR_integratedOmega; + (void)pim.incrementalRotation(measuredOmega, bias, deltaT, + D_incrR_integratedOmega); + auto g = [&](const Vector3& x, const Vector3& y) { return pim.incrementalRotation(x, y, deltaT, {}); }; - - // NOTE(frank): Here, once again, the derivatives are weird, as they do not - // take the rotation into account. They *are* the derivatives of the rotated - // omegas, but not the derivatives with respect to the function arguments. - - // Check derivative with respect to measuredOmega EXPECT(assert_equal( - numericalDerivative21(f, measuredOmega, biasHat), - deltaT * D_incrR_integratedOmega)); - - // Check derivative with respect to biasHat - EXPECT(assert_equal( - numericalDerivative22(f, measuredOmega, biasHat), + numericalDerivative22(g, measuredOmega, bias), -deltaT * D_incrR_integratedOmega)); }