diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 28a314247..7e45e5380 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -93,11 +93,11 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; } void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; } - void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInit=cov; } + void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInit=cov; } const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; } const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; } - const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInit; } + const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInit; } private: diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 6ede1645f..ab95d5a29 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -165,11 +165,11 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { void setBiasAccCovariance(Matrix cov); void setBiasOmegaCovariance(Matrix cov); - void setBiasAccOmegaInt(Matrix cov); + void setBiasAccOmegaInit(Matrix cov); Matrix getBiasAccCovariance() const ; Matrix getBiasOmegaCovariance() const ; - Matrix getBiasAccOmegaInt() const; + Matrix getBiasAccOmegaInit() const; }; diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 48a89d7a0..de8a97ee1 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -276,10 +276,11 @@ TEST(CombinedImuFactor, SameCovariance) { 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()); // Set bias integration covariance explicitly to zero - combined_params->setBiasAccOmegaInt(Z_6x6); + combined_params->setIntegrationCovariance(Z_3x3); + combined_params->setOmegaCoriolis(Z_3x1); + // Set bias initial covariance explicitly to zero + combined_params->setBiasAccOmegaInit(Z_6x6); // The IMU preintegration object for CombinedImuFactor PreintegratedCombinedMeasurements cpim(combined_params, currentBias);