diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index f7b6aa43f..3141f8245 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -120,8 +120,9 @@ public: * Default constructor, initializes the class with no measurements * @param bias Current estimate of acceleration and rotation rate biases */ - PreintegratedCombinedMeasurements(const boost::shared_ptr& p, - const imuBias::ConstantBias& biasHat) + PreintegratedCombinedMeasurements( + const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : PreintegrationBase(p, biasHat) { preintMeasCov_.setZero(); } diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 7e05c6d41..aeda774d5 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -69,9 +69,7 @@ void PreintegratedImuMeasurements::integrateMeasurement( const Matrix3& wCov = p().gyroscopeCovariance; const Matrix3& iCov = p().integrationCovariance; - // NOTE(luca): (1/dt) allows to pass from continuous time noise to discrete - // time noise - // measurementCovariance_discrete = measurementCovariance_contTime/dt + // (1/dt) allows to pass from continuous time noise to discrete time noise preintMeasCov_ = A * preintMeasCov_ * A.transpose(); preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose(); preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose(); diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 177c9b8f3..f2d031dc0 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -202,11 +202,12 @@ public: Rot3 deltaRij() const { return Rot3::Expmap(deltaXij_.theta()); } NavState deltaXij() const { return NavState::Retract(deltaXij_.vector()); } - const Matrix3& delRdelBiasOmega() const { return delRdelBiasOmega_; } - const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_; } - const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_; } - const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_; } - const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_; } + const Matrix93 zeta_H_biasAcc() { + return (Matrix93() << Z_3x3, delPdelBiasAcc_, delVdelBiasAcc_).finished(); + } + const Matrix93 zeta_H_biasOmega() { + return (Matrix93() << delRdelBiasOmega_, delPdelBiasOmega_, delVdelBiasOmega_).finished(); + } // Exposed for MATLAB Vector6 biasHatVector() const { return biasHat_.vector(); } diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 8ed0f751f..06ceef994 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -34,30 +34,6 @@ #include "imuFactorTesting.h" -namespace { - -// Auxiliary functions to test preintegrated Jacobians -// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ -/* ************************************************************************* */ -PreintegratedCombinedMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias) { - auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81); - PreintegratedCombinedMeasurements pim(p, bias); - integrateMeasurements(testing::SomeMeasurements(), &pim); - return pim; -} - -Vector3 evaluatePreintegratedMeasurementsPosition( - const imuBias::ConstantBias& bias) { - return evaluatePreintegratedMeasurements(bias).deltaPij(); -} - -Vector3 evaluatePreintegratedMeasurementsVelocity( - const imuBias::ConstantBias& bias) { - return evaluatePreintegratedMeasurements(bias).deltaVij(); -} -} - /* ************************************************************************* */ TEST( CombinedImuFactor, PreintegratedMeasurements ) { // Linearization point @@ -146,38 +122,24 @@ TEST( CombinedImuFactor, ErrorWithBiases ) { /* ************************************************************************* */ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { - // Actual preintegrated values - PreintegratedCombinedMeasurements pim = - evaluatePreintegratedMeasurements(kZeroBiasHat); + auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81); + testing::SomeMeasurements measurements; - // Check derivative of rotation - boost::function theta = + boost::function zeta = [=](const Vector3& a, const Vector3& w) { - return evaluatePreintegratedMeasurements(Bias(a, w)).theta(); + PreintegratedImuMeasurements pim(p, Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.zeta(); }; - EXPECT( - assert_equal(numericalDerivative21(theta, Z_3x1, Z_3x1), Matrix(Z_3x3))); - EXPECT(assert_equal(numericalDerivative22(theta, Z_3x1, Z_3x1), - pim.delRdelBiasOmega(), 1e-7)); - // Compute numerical derivatives - Matrix expectedDelPdelBias = - numericalDerivative11( - evaluatePreintegratedMeasurementsPosition, kZeroBiasHat); - Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); - Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); + // Actual pre-integrated values + PreintegratedCombinedMeasurements pim(p); + testing::integrateMeasurements(measurements, &pim); - Matrix expectedDelVdelBias = - numericalDerivative11( - &evaluatePreintegratedMeasurementsVelocity, kZeroBiasHat); - Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); - Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - - // Compare Jacobians - EXPECT(assert_equal(expectedDelPdelBiasAcc, pim.delPdelBiasAcc())); - EXPECT(assert_equal(expectedDelPdelBiasOmega, pim.delPdelBiasOmega(), 1e-8)); - EXPECT(assert_equal(expectedDelVdelBiasAcc, pim.delVdelBiasAcc())); - EXPECT(assert_equal(expectedDelVdelBiasOmega, pim.delVdelBiasOmega(), 1e-8)); + EXPECT(assert_equal(numericalDerivative21(zeta, Z_3x1, Z_3x1), + pim.zeta_H_biasAcc())); + EXPECT(assert_equal(numericalDerivative22(zeta, Z_3x1, Z_3x1), + pim.zeta_H_biasOmega(), 1e-7)); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 80c7f6502..c02b49fbf 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -63,24 +63,7 @@ static boost::shared_ptr defaultParams() { return p; } -// Auxiliary functions to test pre-integrated Jacobians -// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ /* ************************************************************************* */ -PreintegratedImuMeasurements evaluatePreintegratedMeasurements( - const Bias& bias) { - PreintegratedImuMeasurements pim(defaultParams(), bias); - integrateMeasurements(testing::SomeMeasurements(), &pim); - return pim; -} - -Vector3 evaluatePreintegratedMeasurementsPosition(const Bias& bias) { - return evaluatePreintegratedMeasurements(bias).deltaPij(); -} - -Vector3 evaluatePreintegratedMeasurementsVelocity(const Bias& bias) { - return evaluatePreintegratedMeasurements(bias).deltaVij(); -} - Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT) { return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); @@ -454,37 +437,23 @@ TEST(ImuFactor, fistOrderExponential) { /* ************************************************************************* */ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { - // Actual pre-integrated values - PreintegratedImuMeasurements preintegrated = - evaluatePreintegratedMeasurements(kZeroBias); + testing::SomeMeasurements measurements; - // Check derivative of rotation - boost::function theta = + boost::function zeta = [=](const Vector3& a, const Vector3& w) { - return evaluatePreintegratedMeasurements(Bias(a, w)).theta(); + PreintegratedImuMeasurements pim(defaultParams(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.zeta(); }; - EXPECT( - assert_equal(numericalDerivative21(theta, Z_3x1, Z_3x1), Matrix(Z_3x3))); - EXPECT(assert_equal(numericalDerivative22(theta, Z_3x1, Z_3x1), - preintegrated.delRdelBiasOmega(), 1e-7)); - // Check derivative of translation - Matrix expectedDelPdelBias = numericalDerivative11( - &evaluatePreintegratedMeasurementsPosition, kZeroBias); - Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); - Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); - EXPECT(assert_equal(expectedDelPdelBiasOmega, - preintegrated.delPdelBiasOmega(), 1e-8)); + // Actual pre-integrated values + PreintegratedImuMeasurements pim(defaultParams()); + testing::integrateMeasurements(measurements, &pim); - // Check derivative of velocity - Matrix expectedDelVdelBias = numericalDerivative11( - &evaluatePreintegratedMeasurementsVelocity, kZeroBias); - Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); - Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); - EXPECT(assert_equal(expectedDelVdelBiasOmega, - preintegrated.delVdelBiasOmega(), 1e-8)); + EXPECT(assert_equal(numericalDerivative21(zeta, Z_3x1, Z_3x1), + pim.zeta_H_biasAcc())); + EXPECT(assert_equal(numericalDerivative22(zeta, Z_3x1, Z_3x1), + pim.zeta_H_biasOmega(), 1e-7)); } /* ************************************************************************* */