From 53712149f98b8325ce91fdab22f8fc2b0f395e10 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 19 Sep 2021 21:15:51 -0400 Subject: [PATCH] actually test the covariances and fix bug --- gtsam/navigation/CombinedImuFactor.cpp | 4 +- .../tests/testCombinedImuFactor.cpp | 40 ++++++++----------- 2 files changed, 19 insertions(+), 25 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index cd579d716..c8d1b10fe 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -141,11 +141,11 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( D_t_t(&G_measCov_Gt) = (pos_H_biasAcc * (aCov / dt) // aCov_updated / dt * pos_H_biasAcc.transpose()) + (dt * iCov); - D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc + D_v_v(&G_measCov_Gt) = vel_H_biasAcc * (aCov / dt) // aCov_updated / dt * (vel_H_biasAcc.transpose()); - D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega + D_R_R(&G_measCov_Gt) = theta_H_biasOmega * (wCov / dt) // wCov_updated / dt * (theta_H_biasOmega.transpose()); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index f7acfa79e..0dbf961fc 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -269,27 +269,14 @@ static boost::shared_ptr Params() { // EXPECT(assert_equal(expected, actual.preintMeasCov())); // } -TEST(CombinedImuFactor, SameCovarianceCombined) { - auto params = PreintegrationCombinedParams::MakeSharedU(); - - params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3); - params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3); - params->setIntegrationCovariance(pow(0, 2) * I_3x3); - params->setOmegaCoriolis(Vector3::Zero()); - - imuBias::ConstantBias currentBias; - - PreintegratedCombinedMeasurements pim(params, currentBias); +TEST(CombinedImuFactor, SameCovariance) { Vector3 accMeas(0.1577, -0.8251, 9.6111); Vector3 omegaMeas(-0.0210, 0.0311, 0.0145); double deltaT = 0.01; - pim.integrateMeasurement(accMeas, omegaMeas, deltaT); - std::cout << pim.preintMeasCov() << std::endl << std::endl; -} - -TEST(CombinedImuFactor, SameCovariance) { + imuBias::ConstantBias currentBias; + auto params = PreintegrationParams::MakeSharedU(); params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3); @@ -297,16 +284,23 @@ TEST(CombinedImuFactor, SameCovariance) { params->setIntegrationCovariance(pow(0, 2) * I_3x3); params->setOmegaCoriolis(Vector3::Zero()); - imuBias::ConstantBias currentBias; - PreintegratedImuMeasurements pim(params, currentBias); - - Vector3 accMeas(0.1577, -0.8251, 9.6111); - Vector3 omegaMeas(-0.0210, 0.0311, 0.0145); - double deltaT = 0.01; pim.integrateMeasurement(accMeas, omegaMeas, deltaT); - std::cout << pim.preintMeasCov() << std::endl << std::endl; + // std::cout << pim.preintMeasCov() << std::endl << std::endl; + + auto combined_params = PreintegrationCombinedParams::MakeSharedU(); + + combined_params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3); + combined_params->setGyroscopeCovariance(pow(1.75e-4, 2) * I_3x3); + combined_params->setIntegrationCovariance(pow(0, 2) * I_3x3); + combined_params->setOmegaCoriolis(Vector3::Zero()); + + PreintegratedCombinedMeasurements cpim(combined_params, currentBias); + cpim.integrateMeasurement(accMeas, omegaMeas, deltaT); + + // std::cout << cpim.preintMeasCov() << std::endl << std::endl; + EXPECT(assert_equal(pim.preintMeasCov(), cpim.preintMeasCov().block(0, 0, 9, 9))); }