diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index a9d4a28bb..cd8f2b64c 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -89,13 +89,13 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega, return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! } -void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega, - const Vector3& biasHat, double deltaT, +void PreintegratedRotation::integrateMeasurement( + const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, 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); + D_incrR_integratedOmega); // If asked, pass first derivative as well if (optional_D_incrR_integratedOmega) { @@ -108,8 +108,8 @@ void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega, // Update Jacobian const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - - D_incrR_integratedOmega * deltaT; + delRdelBiasOmega_ = + incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT; } Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr, diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index c80399f14..ed7b6f29c 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -65,7 +65,6 @@ struct GTSAM_EXPORT PreintegratedRotationParams { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - namespace bs = ::boost::serialization; ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance); ar & BOOST_SERIALIZATION_NVP(body_P_sensor); @@ -159,15 +158,34 @@ class GTSAM_EXPORT PreintegratedRotation { /// @name Main functionality /// @{ - /// 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. - Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, - OptionalJacobian<3, 3> D_incrR_integratedOmega) const; + /** + * @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. + * @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 + */ + Rot3 incrementalRotation( + const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, + OptionalJacobian<3, 3> D_incrR_integratedOmega) const; - /// Calculate an incremental rotation given the gyro measurement and a time interval, - /// and update both deltaTij_ and deltaRij_. - void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, + /** + * @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 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 + */ + void integrateMeasurement(const Vector3& measuredOmega, + const Vector3& biasHat, double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega = {}, OptionalJacobian<3, 3> F = {}); diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp new file mode 100644 index 000000000..e264a6929 --- /dev/null +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -0,0 +1,122 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testPreintegratedRotation.cpp + * @brief Unit test for PreintegratedRotation + * @author Frank Dellaert + */ + +#include +#include +#include + +#include + +#include "gtsam/base/Matrix.h" +#include "gtsam/base/Vector.h" + +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 deltaT = 0.5; +} // namespace simple_roll + +//****************************************************************************** +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); + EXPECT(assert_equal(expected, incrR, 1e-9)); + + // Lambda for numerical derivative: + auto f = [&](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), + -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; +} + +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 deltaT = 0.5; +} // namespace roll_in_rotated_frame + +//****************************************************************************** +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)); + + // Lambda for numerical derivative: + auto f = [&](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), + -deltaT * D_incrR_integratedOmega)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//******************************************************************************