From 11d0ad0d78c9235fb8758ed9469d99571aded864 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 27 Jul 2015 16:12:41 +0200 Subject: [PATCH] Covariance regression values --- gtsam/navigation/tests/testImuFactor.cpp | 37 ++++++++++++++++++++---- 1 file changed, 31 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 1f1815fba..0ab2767f3 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -755,15 +755,27 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), Point3(1, 0, 0)); - ImuFactor::PreintegratedMeasurements pre_int_data( + ImuFactor::PreintegratedMeasurements pim( imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + Matrix expected(9,9); + expected << + 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,// + 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,// + 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,// + 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,// + 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,// + 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0,// + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0290687976, 0.0, 0.0,// + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0290687976, 0.0,// + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.03; + EXPECT(assert_equal(expected, pim.preintMeasCov(), 1e-7)); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity, kNonZeroOmegaCoriolis); Values values; @@ -864,16 +876,29 @@ TEST(ImuFactor, PredictArbitrary) { Vector3 measuredAcc(0.1, 0.2, -9.81); double deltaT = 0.001; - ImuFactor::PreintegratedMeasurements pre_int_data( + ImuFactor::PreintegratedMeasurements pim( imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true); for (int i = 0; i < 1000; ++i) - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + Matrix expected(9,9); + expected << // + 0.142448905, 0.00345595825, -0.0225794125, 0.34774305, 0.0119449979, -0.075667905, -0.0144839494, -0.0454268484, 0.00489577218,// + 0.00345595825, 0.143318431, 0.0200549262, 0.0112877167, 0.351503176, 0.0629164907, 0.044978128, -0.0149428917, 0.00839301168, // + -0.0225794125, 0.0200549262, 0.0104041905, -0.0580647212, 0.051116506, 0.0285371399, 0.0100471195, 0.00609093775, 0.000448720395, // + 0.34774305, 0.0112877167, -0.0580647212, 0.911721561, 0.0412249672, -0.205920425, -0.0409843415, -0.13554868, 0.00879031682, // + 0.0119449979, 0.351503176, 0.051116506, 0.0412249672, 0.928013807, 0.169935105, 0.134423822, -0.0471183681, 0.0162199769, // + -0.075667905, 0.0629164907, 0.0285371399, -0.205920425, 0.169935105, 0.09407764, 0.0383280513, 0.0247643646, 0.00112485862,// + -0.0144839494, 0.044978128, 0.0100471195, -0.0409843415, 0.134423822, 0.0383280513, 0.0299999995, 0.0, 0.0, // + -0.0454268484, -0.0149428917, 0.00609093775, -0.13554868, -0.0471183681, 0.0247643646, 0.0, 0.0299999995, 0.0, // + 0.00489577218, 0.00839301168, 0.000448720395, 0.00879031682, 0.0162199769, 0.00112485862, 0.0, 0.0, 0.0299999995; + EXPECT(assert_equal(expected, pim.preintMeasCov(), 1e-7)); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, kGravity, + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity, kZeroOmegaCoriolis); // Predict