Merge pull request #1943 from borglab/improvements

borglab · Jan 10, 2025 · 0c44f9f · 0c44f9f
2 parents 22bdd2a + 9910bb6
commit 0c44f9f
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 8 deletions.
diff --git a/gtsam/hybrid/tests/testHybridMotionModel.cpp b/gtsam/hybrid/tests/testHybridMotionModel.cpp
@@ -101,7 +101,7 @@ std::pair<double, double> 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
@@ -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<double, 15, 15>& preintMeasCov =
           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
@@ -108,7 +110,9 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements
   PreintegratedCombinedMeasurements(
       const PreintegrationType& base,
       const Eigen::Matrix<double, 15, 15>& 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
@@ -79,7 +79,7 @@ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType {
 
   /// Default constructor for serialization and wrappers
   PreintegratedImuMeasurements() {
-    preintMeasCov_.setZero();
+    resetIntegration();
   }
 
  /**
@@ -90,7 +90,7 @@ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType {
   PreintegratedImuMeasurements(const std::shared_ptr<PreintegrationParams>& p,
       const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
       PreintegrationType(p, biasHat) {
-    preintMeasCov_.setZero();
+    resetIntegration();
   }
 
 /**
@@ -101,6 +101,7 @@ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType {
   PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov)
      : PreintegrationType(base),
        preintMeasCov_(preintMeasCov) {
+    PreintegrationType::resetIntegration();
   }
 
   /// Virtual destructor
@@ -113,7 +114,7 @@ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType {
   /// equals
   bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const;
 
-  /// Re-initialize PreintegratedIMUMeasurements
+  /// Re-initialize PreintegratedImuMeasurements
   void resetIntegration() override;
 
   /**
@@ -159,7 +160,7 @@ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType {
  * 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
@@ -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 <gtsam/navigation/CombinedImuFactor.h>
 virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
   PreintegrationCombinedParams(gtsam::Vector n_gravity);