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

more comprehensive initialization of ImuFactors

release/4.3a0
Varun Agrawal 2024-12-22 17:34:41 -05:00
parent ac47d8f27c
commit 1a96544308
2 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/navigation/CombinedImuFactor.h
View File

@ -84,7 +84,7 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements
/// @{ /// @{
/// Default constructor only for serialization and wrappers /// Default constructor only for serialization and wrappers
PreintegratedCombinedMeasurements() { preintMeasCov_.setZero(); } PreintegratedCombinedMeasurements() { resetIntegration(); }
/** /**
* Default constructor, initializes the class with no measurements * Default constructor, initializes the class with no measurements
@ -97,7 +97,9 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias(), const imuBias::ConstantBias& biasHat = imuBias::ConstantBias(),
const Eigen::Matrix<double, 15, 15>& preintMeasCov = const Eigen::Matrix<double, 15, 15>& preintMeasCov =
Eigen::Matrix<double, 15, 15>::Zero()) Eigen::Matrix<double, 15, 15>::Zero())
: PreintegrationType(p, biasHat), preintMeasCov_(preintMeasCov) {} : PreintegrationType(p, biasHat), preintMeasCov_(preintMeasCov) {
PreintegrationType::resetIntegration();
}
/** /**
* Construct preintegrated directly from members: base class and * Construct preintegrated directly from members: base class and
@ -108,7 +110,9 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements( PreintegratedCombinedMeasurements(
const PreintegrationType& base, const PreintegrationType& base,
const Eigen::Matrix<double, 15, 15>& preintMeasCov) const Eigen::Matrix<double, 15, 15>& preintMeasCov)
: PreintegrationType(base), preintMeasCov_(preintMeasCov) {} : PreintegrationType(base), preintMeasCov_(preintMeasCov) {
PreintegrationType::resetIntegration();
}
/// Virtual destructor /// Virtual destructor
~PreintegratedCombinedMeasurements() override {} ~PreintegratedCombinedMeasurements() override {}

5
gtsam/navigation/ImuFactor.h
View File

@ -79,7 +79,7 @@ public:
/// Default constructor for serialization and wrappers /// Default constructor for serialization and wrappers
PreintegratedImuMeasurements() { PreintegratedImuMeasurements() {
preintMeasCov_.setZero(); resetIntegration();
} }
/** /**
@ -90,7 +90,7 @@ public:
PreintegratedImuMeasurements(const std::shared_ptr<PreintegrationParams>& p, PreintegratedImuMeasurements(const std::shared_ptr<PreintegrationParams>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
PreintegrationType(p, biasHat) { PreintegrationType(p, biasHat) {
preintMeasCov_.setZero(); resetIntegration();
} }
/** /**
@ -101,6 +101,7 @@ public:
PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov) PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov)
: PreintegrationType(base), : PreintegrationType(base),
preintMeasCov_(preintMeasCov) { preintMeasCov_(preintMeasCov) {
PreintegrationType::resetIntegration();
} }
/// Virtual destructor /// Virtual destructor