From 6eef703019e38af836d3dadbffb6d5627e4ff657 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 15 Nov 2022 16:21:42 -0500 Subject: [PATCH] reorder operations in resetIntegration and add docstrings --- gtsam/navigation/CombinedImuFactor.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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(); }