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() { void PreintegratedCombinedMeasurements::resetIntegration() {
// Base class method to reset the preintegrated measurements
PreintegrationType::resetIntegration();
// Set the initial bias covariance to // Set the initial bias covariance to
// the estimated covariance from the last step. // the estimated covariance from the last step.
p().biasAccOmegaInt = preintMeasCov_.block<6, 6>(9, 9); p().biasAccOmegaInt = preintMeasCov_.block<6, 6>(9, 9);
PreintegrationType::resetIntegration();
preintMeasCov_.setZero(); preintMeasCov_.setZero();
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::resetIntegration( void PreintegratedCombinedMeasurements::resetIntegration(
const gtsam::Matrix6& Q_init) { const gtsam::Matrix6& Q_init) {
p().biasAccOmegaInt = Q_init; // Base class method to reset the preintegrated measurements
PreintegrationType::resetIntegration(); PreintegrationType::resetIntegration();
p().biasAccOmegaInt = Q_init;
preintMeasCov_.setZero(); preintMeasCov_.setZero();
} }