diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 57e86d3b6..142faedd3 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -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(); }