From 3006a764446d6b7680a2aae840f32dc642d26c8a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 15 Nov 2022 19:24:22 -0500 Subject: [PATCH] update CombinedImuFactor test to better test for integration reset --- .../tests/testCombinedImuFactor.cpp | 22 +++++++++++++------ 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 250a0dc42..fcda1da11 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -36,14 +36,17 @@ namespace testing { // Create default parameters with Z-down and above noise parameters -static boost::shared_ptr Params() { +static boost::shared_ptr Params( + const Matrix3& biasAccCovariance = Matrix3::Zero(), + const Matrix3& biasOmegaCovariance = Matrix3::Zero(), + const Matrix6& biasAccOmegaInt = Matrix6::Zero()) { auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(kGravity); p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; p->integrationCovariance = 0.0001 * I_3x3; - p->biasAccCovariance = Z_3x3; - p->biasOmegaCovariance = Z_3x3; - p->biasAccOmegaInt = Z_6x6; + p->biasAccCovariance = biasAccCovariance; + p->biasOmegaCovariance = biasOmegaCovariance; + p->biasAccOmegaInt = biasAccOmegaInt; return p; } } // namespace testing @@ -332,7 +335,12 @@ TEST(CombinedImuFactor, ResetIntegration) { const double T = 3.0; // seconds - CombinedScenarioRunner runner(scenario, testing::Params(), T / 10); + auto preinMeasCov = 0.001 * Eigen::Matrix::Identity(); + CombinedScenarioRunner runner( + scenario, + testing::Params(Matrix3::Zero(), Matrix3::Zero(), + 0.1 * Matrix6::Identity()), + T / 10, imuBias::ConstantBias(), preinMeasCov); PreintegratedCombinedMeasurements pim = runner.integrate(T); // Make copy for testing different conditions @@ -340,8 +348,8 @@ TEST(CombinedImuFactor, ResetIntegration) { // Test default method pim.resetIntegration(); - Matrix6 expected = Z_6x6; - EXPECT(assert_equal(expected, pim.p().biasAccOmegaInt , 1e-9)); + Matrix6 expected = 0.001 * I_6x6; + EXPECT(assert_equal(expected, pim.p().biasAccOmegaInt, 1e-9)); // Test method where Q_init is provided Matrix6 expected_Q_init = I_6x6 * 0.001;