diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp index a4de6a17b..387094c47 100644 --- a/gtsam/hybrid/tests/testHybridMotionModel.cpp +++ b/gtsam/hybrid/tests/testHybridMotionModel.cpp @@ -101,7 +101,7 @@ std::pair approximateDiscreteMarginal( // Do importance sampling double w0 = 0.0, w1 = 0.0; std::mt19937_64 rng(42); - for (int i = 0; i < N; i++) { + for (size_t i = 0; i < N; i++) { HybridValues sample = q.sample(&rng); sample.insert(given); double weight = hbn.evaluate(sample) / q.evaluate(sample); diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index b4c3ae8dc..87444e624 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 b19989a77..2e6dff0dc 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 @@ -113,7 +114,7 @@ public: /// equals bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const; - /// Re-initialize PreintegratedIMUMeasurements + /// Re-initialize PreintegratedImuMeasurements void resetIntegration() override; /** @@ -159,7 +160,7 @@ public: * the vehicle at previous time step), current state (pose and velocity at * current time step), and the bias estimate. Following the preintegration * scheme proposed in [2], the ImuFactor includes many IMU measurements, which - * are "summarized" using the PreintegratedIMUMeasurements class. + * are "summarized" using the PreintegratedImuMeasurements class. * Note that this factor does not model "temporal consistency" of the biases * (which are usually slowly varying quantities), which is up to the caller. * See also CombinedImuFactor for a class that does this for you. diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index adb8bf2bb..ceeab3b35 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -156,6 +156,22 @@ virtual class ImuFactor: gtsam::NonlinearFactor { void serialize() const; }; +virtual class ImuFactor2: gtsam::NonlinearFactor { + ImuFactor2(); + ImuFactor2(size_t state_i, size_t state_j, + size_t bias, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); + + // Standard Interface + gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; + gtsam::Vector evaluateError(const gtsam::NavState& state_i, + gtsam::NavState& state_j, + const gtsam::imuBias::ConstantBias& bias_i); + + // enable serialization functionality + void serialize() const; +}; + #include virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { PreintegrationCombinedParams(gtsam::Vector n_gravity);