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

fix setter and getter for biasAccOmegaInit

release/4.3a0
Varun Agrawal 2022-08-02 14:14:13 -04:00
parent 322c08071e
commit ac28b0e969
3 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/navigation/CombinedImuFactor.h
View File

@ -93,11 +93,11 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; } void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=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& getBiasAccCovariance() const { return biasAccCovariance; }
const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; } const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInit; } const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInit; }
private: private:

4
gtsam/navigation/navigation.i
View File

@ -165,11 +165,11 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
void setBiasAccCovariance(Matrix cov); void setBiasAccCovariance(Matrix cov);
void setBiasOmegaCovariance(Matrix cov); void setBiasOmegaCovariance(Matrix cov);
void setBiasAccOmegaInt(Matrix cov); void setBiasAccOmegaInit(Matrix cov);
Matrix getBiasAccCovariance() const ; Matrix getBiasAccCovariance() const ;
Matrix getBiasOmegaCovariance() const ; Matrix getBiasOmegaCovariance() const ;
Matrix getBiasAccOmegaInt() const; Matrix getBiasAccOmegaInit() const;
}; };

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

@ -276,10 +276,11 @@ TEST(CombinedImuFactor, SameCovariance) {
auto combined_params = PreintegrationCombinedParams::MakeSharedU(); auto combined_params = PreintegrationCombinedParams::MakeSharedU();
combined_params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3); combined_params->setAccelerometerCovariance(pow(0.01, 2) * I_3x3);
combined_params->setGyroscopeCovariance(pow(1.75e-4, 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 // 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 // The IMU preintegration object for CombinedImuFactor
PreintegratedCombinedMeasurements cpim(combined_params, currentBias); PreintegratedCombinedMeasurements cpim(combined_params, currentBias);