diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 8748a7fcf..9db7aac72 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -444,28 +444,6 @@ TEST(ImuFactor, fistOrderExponential) { EXPECT(assert_equal(expectedRot, actualRot)); } -/* ************************************************************************* */ -#ifdef GTSAM_TANGENT_PREINTEGRATION -TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { - testing::SomeMeasurements measurements; - - boost::function preintegrated = - [=](const Vector3& a, const Vector3& w) { - PreintegratedImuMeasurements pim(testing::Params(), Bias(a, w)); - testing::integrateMeasurements(measurements, &pim); - return pim.preintegrated(); - }; - - // Actual pre-integrated values - PreintegratedImuMeasurements pim(testing::Params()); - testing::integrateMeasurements(measurements, &pim); - - EXPECT(assert_equal(numericalDerivative21(preintegrated, kZero, kZero), - pim.preintegrated_H_biasAcc())); - EXPECT(assert_equal(numericalDerivative22(preintegrated, kZero, kZero), - pim.preintegrated_H_biasOmega(), 1e-3)); -} -#endif /* ************************************************************************* */ Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, const Vector3& measuredAcc, const Vector3& measuredOmega) { diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index 96c391112..a3f688dda 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -26,17 +26,6 @@ #include "imuFactorTesting.h" -static const double kDt = 0.1; - -Vector9 f(ManifoldPreintegration pim, const Vector3& a, const Vector3& w) { - NavState state = pim.deltaXij(); - Matrix9 aH1; - Matrix93 aH2, aH3; - pim.update(a, w, kDt, &aH1, &aH2, &aH3); - NavState newNavState = pim.deltaXij(); - return state.localCoordinates(newNavState); -} - namespace testing { // Create default parameters with Z-down and above noise parameters static boost::shared_ptr Params() { @@ -49,34 +38,54 @@ static boost::shared_ptr Params() { } /* ************************************************************************* */ -TEST(ManifoldPreintegration, UpdateEstimate1) { - ManifoldPreintegration pim(testing::Params()); - ManifoldPreintegration pimActual(pim); - const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); - Vector9 zeta; - zeta.setZero(); - Matrix9 aH1; - Matrix93 aH2, aH3; - pim.update(acc, omega, kDt, &aH1, &aH2, &aH3); - EXPECT(assert_equal(numericalDerivative31(f, pimActual, acc, omega), aH1, 1e-9)); - EXPECT(assert_equal(numericalDerivative32(f, pimActual, acc, omega), aH2, 1e-9)); - EXPECT(assert_equal(numericalDerivative33(f, pimActual, acc, omega), aH3, 1e-9)); -} +TEST(ManifoldPreintegration, BiasCorrectionJacobians) { + testing::SomeMeasurements measurements; -/* ************************************************************************* */ -TEST(ManifoldPreintegration, UpdateEstimate2) { - ManifoldPreintegration pim(testing::Params()); + boost::function deltaRij = + [=](const Vector3& a, const Vector3& w) { + ManifoldPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaRij(); + }; - const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); - Vector9 zeta; - zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3; - Matrix9 aH1; - Matrix93 aH2, aH3; - pim.update(acc, omega, kDt, &aH1, &aH2, &aH3); - // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 - EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3)); - EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-8)); - EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); + boost::function deltaPij = + [=](const Vector3& a, const Vector3& w) { + ManifoldPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaPij(); + }; + + boost::function deltaVij = + [=](const Vector3& a, const Vector3& w) { + ManifoldPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaVij(); + }; + + // Actual pre-integrated values + ManifoldPreintegration pim(testing::Params()); + testing::integrateMeasurements(measurements, &pim); + + EXPECT( + assert_equal(numericalDerivative21(deltaRij, kZero, kZero), + Matrix3(Z_3x3))); + EXPECT( + assert_equal(numericalDerivative22(deltaRij, kZero, kZero), + pim.delRdelBiasOmega(), 1e-3)); + + EXPECT( + assert_equal(numericalDerivative21(deltaPij, kZero, kZero), + pim.delPdelBiasAcc())); + EXPECT( + assert_equal(numericalDerivative22(deltaPij, kZero, kZero), + pim.delPdelBiasOmega(), 1e-3)); + + EXPECT( + assert_equal(numericalDerivative21(deltaVij, kZero, kZero), + pim.delVdelBiasAcc())); + EXPECT( + assert_equal(numericalDerivative22(deltaVij, kZero, kZero), + pim.delVdelBiasOmega(), 1e-3)); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testTangentPreintegration.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp index af6e283bd..65fd55fa3 100644 --- a/gtsam/navigation/tests/testTangentPreintegration.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -72,6 +72,29 @@ TEST(TangentPreintegration, UpdateEstimate2) { EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); } +/* ************************************************************************* */ +TEST(ImuFactor, BiasCorrectionJacobians) { + testing::SomeMeasurements measurements; + + boost::function preintegrated = + [=](const Vector3& a, const Vector3& w) { + TangentPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.preintegrated(); + }; + + // Actual pre-integrated values + TangentPreintegration pim(testing::Params()); + testing::integrateMeasurements(measurements, &pim); + + EXPECT( + assert_equal(numericalDerivative21(preintegrated, kZero, kZero), + pim.preintegrated_H_biasAcc())); + EXPECT( + assert_equal(numericalDerivative22(preintegrated, kZero, kZero), + pim.preintegrated_H_biasOmega(), 1e-3)); +} + /* ************************************************************************* */ TEST(TangentPreintegration, computeError) { TangentPreintegration pim(testing::Params());