12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update CombinedImuFactor test to better test for integration reset

release/4.3a0
Varun Agrawal 2022-11-15 19:24:22 -05:00
parent 41d0606816
commit 3006a76444
1 changed files with 15 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -36,14 +36,17 @@
namespace testing { namespace testing {
// Create default parameters with Z-down and above noise parameters // Create default parameters with Z-down and above noise parameters
static boost::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() { static boost::shared_ptr<PreintegratedCombinedMeasurements::Params> Params(
const Matrix3& biasAccCovariance = Matrix3::Zero(),
const Matrix3& biasOmegaCovariance = Matrix3::Zero(),
const Matrix6& biasAccOmegaInt = Matrix6::Zero()) {
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(kGravity); auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(kGravity);
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
p->integrationCovariance = 0.0001 * I_3x3; p->integrationCovariance = 0.0001 * I_3x3;
p->biasAccCovariance = Z_3x3; p->biasAccCovariance = biasAccCovariance;
p->biasOmegaCovariance = Z_3x3; p->biasOmegaCovariance = biasOmegaCovariance;
p->biasAccOmegaInt = Z_6x6; p->biasAccOmegaInt = biasAccOmegaInt;
return p; return p;
} }
} // namespace testing } // namespace testing
@ -332,7 +335,12 @@ TEST(CombinedImuFactor, ResetIntegration) {
const double T = 3.0; // seconds const double T = 3.0; // seconds
CombinedScenarioRunner runner(scenario, testing::Params(), T / 10); auto preinMeasCov = 0.001 * Eigen::Matrix<double, 15, 15>::Identity();
CombinedScenarioRunner runner(
scenario,
testing::Params(Matrix3::Zero(), Matrix3::Zero(),
0.1 * Matrix6::Identity()),
T / 10, imuBias::ConstantBias(), preinMeasCov);
PreintegratedCombinedMeasurements pim = runner.integrate(T); PreintegratedCombinedMeasurements pim = runner.integrate(T);
// Make copy for testing different conditions // Make copy for testing different conditions
@ -340,8 +348,8 @@ TEST(CombinedImuFactor, ResetIntegration) {
// Test default method // Test default method
pim.resetIntegration(); pim.resetIntegration();
Matrix6 expected = Z_6x6; Matrix6 expected = 0.001 * I_6x6;
EXPECT(assert_equal(expected, pim.p().biasAccOmegaInt , 1e-9)); EXPECT(assert_equal(expected, pim.p().biasAccOmegaInt, 1e-9));
// Test method where Q_init is provided // Test method where Q_init is provided
Matrix6 expected_Q_init = I_6x6 * 0.001; Matrix6 expected_Q_init = I_6x6 * 0.001;