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

reorder operations in resetIntegration and add docstrings

release/4.3a0
Varun Agrawal 2022-11-15 16:21:42 -05:00
parent 7f14b6e6ce
commit 6eef703019
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -72,18 +72,20 @@ bool PreintegratedCombinedMeasurements::equals(
//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::resetIntegration() {
// Base class method to reset the preintegrated measurements
PreintegrationType::resetIntegration();
// Set the initial bias covariance to
// the estimated covariance from the last step.
p().biasAccOmegaInt = preintMeasCov_.block<6, 6>(9, 9);
PreintegrationType::resetIntegration();
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::resetIntegration(
const gtsam::Matrix6& Q_init) {
p().biasAccOmegaInt = Q_init;
// Base class method to reset the preintegrated measurements
PreintegrationType::resetIntegration();
p().biasAccOmegaInt = Q_init;
preintMeasCov_.setZero();
}