diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 0ffb386a0..5772fc5ae 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -84,7 +84,7 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements /// @{ /// Default constructor only for serialization and wrappers - PreintegratedCombinedMeasurements() { preintMeasCov_.setZero(); } + PreintegratedCombinedMeasurements() { resetIntegration(); } /** * Default constructor, initializes the class with no measurements @@ -97,7 +97,9 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements const imuBias::ConstantBias& biasHat = imuBias::ConstantBias(), const Eigen::Matrix& preintMeasCov = Eigen::Matrix::Zero()) - : PreintegrationType(p, biasHat), preintMeasCov_(preintMeasCov) {} + : PreintegrationType(p, biasHat), preintMeasCov_(preintMeasCov) { + PreintegrationType::resetIntegration(); + } /** * Construct preintegrated directly from members: base class and @@ -108,7 +110,9 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements PreintegratedCombinedMeasurements( const PreintegrationType& base, const Eigen::Matrix& preintMeasCov) - : PreintegrationType(base), preintMeasCov_(preintMeasCov) {} + : PreintegrationType(base), preintMeasCov_(preintMeasCov) { + PreintegrationType::resetIntegration(); + } /// Virtual destructor ~PreintegratedCombinedMeasurements() override {} diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index a3d7dce75..fd087aaee 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -79,7 +79,7 @@ public: /// Default constructor for serialization and wrappers PreintegratedImuMeasurements() { - preintMeasCov_.setZero(); + resetIntegration(); } /** @@ -90,7 +90,7 @@ public: PreintegratedImuMeasurements(const std::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : PreintegrationType(p, biasHat) { - preintMeasCov_.setZero(); + resetIntegration(); } /** @@ -101,6 +101,7 @@ public: PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov) : PreintegrationType(base), preintMeasCov_(preintMeasCov) { + PreintegrationType::resetIntegration(); } /// Virtual destructor