Merge pull request #1308 from borglab/combined-imu-factor

gtsam/navigation/CombinedImuFactor.cpp

@@ -74,10 +74,20 @@ bool PreintegratedCombinedMeasurements::equals(
 
 //------------------------------------------------------------------------------
 void PreintegratedCombinedMeasurements::resetIntegration() {
+  // Base class method to reset the preintegrated measurements
   PreintegrationType::resetIntegration();
   preintMeasCov_.setZero();
 }
 
+//------------------------------------------------------------------------------
+void PreintegratedCombinedMeasurements::resetIntegration(
+    const gtsam::Matrix6& Q_init) {
+  // Base class method to reset the preintegrated measurements
+  PreintegrationType::resetIntegration();
+  p().biasAccOmegaInt = Q_init;
+  preintMeasCov_.setZero();
+}
+
 //------------------------------------------------------------------------------
 // sugar for derivative blocks
 #define D_R_R(H) (H)->block<3,3>(0,0)
@@ -112,21 +122,21 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
   // and preintegrated measurements
 
   // Single Jacobians to propagate covariance
-  Matrix3 theta_H_biasOmega = C.topRows<3>();
+  Matrix3 theta_H_omega = C.topRows<3>();
-  Matrix3 pos_H_biasAcc = B.middleRows<3>(3);
+  Matrix3 pos_H_acc = B.middleRows<3>(3);
-  Matrix3 vel_H_biasAcc = B.bottomRows<3>();
+  Matrix3 vel_H_acc = B.bottomRows<3>();
 
-  Matrix3 theta_H_biasOmegaInit = -theta_H_biasOmega;
+  Matrix3 theta_H_biasOmegaInit = -theta_H_omega;
-  Matrix3 pos_H_biasAccInit = -pos_H_biasAcc;
+  Matrix3 pos_H_biasAccInit = -pos_H_acc;
-  Matrix3 vel_H_biasAccInit = -vel_H_biasAcc;
+  Matrix3 vel_H_biasAccInit = -vel_H_acc;
 
   // overall Jacobian wrt preintegrated measurements (df/dx)
   Eigen::Matrix<double, 15, 15> F;
   F.setZero();
   F.block<9, 9>(0, 0) = A;
-  F.block<3, 3>(0, 12) = theta_H_biasOmega;
+  F.block<3, 3>(0, 12) = theta_H_omega;
-  F.block<3, 3>(3, 9) = pos_H_biasAcc;
+  F.block<3, 3>(3, 9) = pos_H_acc;
-  F.block<3, 3>(6, 9) = vel_H_biasAcc;
+  F.block<3, 3>(6, 9) = vel_H_acc;
   F.block<6, 6>(9, 9) = I_6x6;
 
   // Update the uncertainty on the state (matrix F in [4]).
@@ -151,17 +161,17 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
 
   // BLOCK DIAGONAL TERMS
   D_R_R(&G_measCov_Gt) =
-      (theta_H_biasOmega * (wCov / dt) * theta_H_biasOmega.transpose())  //
+      (theta_H_omega * (wCov / dt) * theta_H_omega.transpose())  //
       +
       (theta_H_biasOmegaInit * bInitCov22 * theta_H_biasOmegaInit.transpose());
 
   D_t_t(&G_measCov_Gt) =
-      (pos_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose())           //
+      (pos_H_acc * (aCov / dt) * pos_H_acc.transpose())           //
       + (pos_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose())  //
       + (dt * iCov);
 
   D_v_v(&G_measCov_Gt) =
-      (vel_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose())  //
+      (vel_H_acc * (aCov / dt) * vel_H_acc.transpose())  //
       + (vel_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
 
   D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
@@ -175,12 +185,12 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
   D_t_R(&G_measCov_Gt) =
       pos_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
   D_t_v(&G_measCov_Gt) =
-      (pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose()) +
+      (pos_H_acc * (aCov / dt) * vel_H_acc.transpose()) +
       (pos_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
   D_v_R(&G_measCov_Gt) =
       vel_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
   D_v_t(&G_measCov_Gt) =
-      (vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose()) +
+      (vel_H_acc * (aCov / dt) * pos_H_acc.transpose()) +
       (vel_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose());
 
   preintMeasCov_.noalias() += G_measCov_Gt;

gtsam/navigation/CombinedImuFactor.h

@@ -23,10 +23,7 @@
 #pragma once
 
 /* GTSAM includes */
-#include <gtsam/navigation/ManifoldPreintegration.h>
+#include <gtsam/navigation/PreintegrationCombinedParams.h>
-#include <gtsam/navigation/TangentPreintegration.h>
-#include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/base/Matrix.h>
 
 namespace gtsam {
 
@@ -56,67 +53,6 @@ typedef ManifoldPreintegration PreintegrationType;
  *     Robotics: Science and Systems (RSS), 2015.
  */
 
-/// Parameters for pre-integration using PreintegratedCombinedMeasurements:
-/// Usage: Create just a single Params and pass a shared pointer to the constructor
-struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
-  Matrix3 biasAccCovariance;    ///< continuous-time "Covariance" describing accelerometer bias random walk
-  Matrix3 biasOmegaCovariance;  ///< continuous-time "Covariance" describing gyroscope bias random walk
-  Matrix6 biasAccOmegaInt;     ///< covariance of bias used as initial estimate.
-
-  /// Default constructor makes uninitialized params struct.
-  /// Used for serialization.
-  PreintegrationCombinedParams()
-      : biasAccCovariance(I_3x3),
-        biasOmegaCovariance(I_3x3),
-        biasAccOmegaInt(I_6x6) {}
-
-  /// See two named constructors below for good values of n_gravity in body frame
-  PreintegrationCombinedParams(const Vector3& _n_gravity) :
-    PreintegrationParams(_n_gravity), biasAccCovariance(I_3x3),
-    biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
-
-  }
-
-  // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
-  static std::shared_ptr<PreintegrationCombinedParams> MakeSharedD(double g = 9.81) {
-    return std::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, g)));
-  }
-
-  // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
-  static std::shared_ptr<PreintegrationCombinedParams> MakeSharedU(double g = 9.81) {
-    return std::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g)));
-  }
-
-  void print(const std::string& s="") const override;
-  bool equals(const PreintegratedRotationParams& other, double tol) const override;
-
-  void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
-  void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
-  void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; }
-  
-  const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
-  const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
-  const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }
-  
-private:
-
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
-  /** Serialization function */
-  friend class boost::serialization::access;
-  template <class ARCHIVE>
-  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
-    namespace bs = ::boost::serialization;
-    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
-    ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
-    ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
-    ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
-  }
-#endif
-
-public:
-  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
-};
-
 /**
  * PreintegratedCombinedMeasurements integrates the IMU measurements
  * (rotation rates and accelerations) and the corresponding covariance matrix.
@@ -127,17 +63,17 @@ public:
  *
  * @ingroup navigation
  */
-class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType {
+class GTSAM_EXPORT PreintegratedCombinedMeasurements
-
+    : public PreintegrationType {
-public:
+ public:
   typedef PreintegrationCombinedParams Params;
 
  protected:
   /* Covariance matrix of the preintegrated measurements
-   * COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY BiasAcc BiasOmega]
+   * COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY BiasAcc
-   * (first-order propagation from *measurementCovariance*).
+   * BiasOmega] (first-order propagation from *measurementCovariance*).
-   * PreintegratedCombinedMeasurements also include the biases and keep the correlation
+   * PreintegratedCombinedMeasurements also include the biases and keep the
-   * between the preintegrated measurements and the biases
+   * correlation between the preintegrated measurements and the biases
    */
   Eigen::Matrix<double, 15, 15> preintMeasCov_;
 
@@ -148,31 +84,31 @@ public:
   /// @{
 
   /// Default constructor only for serialization and wrappers
-  PreintegratedCombinedMeasurements() {
+  PreintegratedCombinedMeasurements() { preintMeasCov_.setZero(); }
-    preintMeasCov_.setZero();
-  }
 
   /**
    *  Default constructor, initializes the class with no measurements
    *  @param p Parameters, typically fixed in a single application
    *  @param biasHat Current estimate of acceleration and rotation rate biases
+   *  @param preintMeasCov Covariance matrix used in noise model.
    */
   PreintegratedCombinedMeasurements(
       const std::shared_ptr<Params>& p,
-      const imuBias::ConstantBias& biasHat = imuBias::ConstantBias())
+      const imuBias::ConstantBias& biasHat = imuBias::ConstantBias(),
-      : PreintegrationType(p, biasHat) {
+      const Eigen::Matrix<double, 15, 15>& preintMeasCov =
-    preintMeasCov_.setZero();
+          Eigen::Matrix<double, 15, 15>::Zero())
-  }
+      : PreintegrationType(p, biasHat), preintMeasCov_(preintMeasCov) {}
 
   /**
-  *  Construct preintegrated directly from members: base class and preintMeasCov
+   *  Construct preintegrated directly from members: base class and
+   * preintMeasCov
    *  @param base               PreintegrationType instance
    *  @param preintMeasCov      Covariance matrix used in noise model.
    */
-  PreintegratedCombinedMeasurements(const PreintegrationType& base, const Eigen::Matrix<double, 15, 15>& preintMeasCov)
+  PreintegratedCombinedMeasurements(
-     : PreintegrationType(base),
+      const PreintegrationType& base,
-       preintMeasCov_(preintMeasCov) {
+      const Eigen::Matrix<double, 15, 15>& preintMeasCov)
-  }
+      : PreintegrationType(base), preintMeasCov_(preintMeasCov) {}
 
   /// Virtual destructor
   ~PreintegratedCombinedMeasurements() override {}
@@ -185,6 +121,14 @@ public:
   /// Re-initialize PreintegratedCombinedMeasurements
   void resetIntegration() override;
 
+  /**
+   * @brief Re-initialize PreintegratedCombinedMeasurements with initial bias
+   * covariance estimate.
+   *
+   * @param Q_init The initial bias covariance estimates as a 6x6 matrix.
+   */
+  void resetIntegration(const gtsam::Matrix6& Q_init);
+
   /// const reference to params, shadows definition in base class
   Params& p() const { return *std::static_pointer_cast<Params>(this->p_); }
   /// @}
@@ -198,13 +142,13 @@ public:
   /// @name Testable
   /// @{
   /// print
-  void print(const std::string& s = "Preintegrated Measurements:") const override;
+  void print(
+      const std::string& s = "Preintegrated Measurements:") const override;
   /// equals
   bool equals(const PreintegratedCombinedMeasurements& expected,
               double tol = 1e-9) const;
   /// @}
 
-
   /// @name Main functionality
   /// @{
 
@@ -219,7 +163,8 @@ public:
    * @param dt Time interval between two consecutive IMU measurements
    */
   void integrateMeasurement(const Vector3& measuredAcc,
-      const Vector3& measuredOmega, const double dt) override;
+                            const Vector3& measuredOmega,
+                            const double dt) override;
 
   /// @}
 
@@ -235,7 +180,7 @@ public:
   }
 #endif
 
-public:
+ public:
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
@@ -244,34 +189,32 @@ public:
  * velocity of the vehicle, as well as bias at previous time step), and current
  * state (pose, velocity, bias at current time step). Following the pre-
  * integration scheme proposed in [2], the CombinedImuFactor includes many IMU
- * measurements, which are "summarized" using the PreintegratedCombinedMeasurements
+ * measurements, which are "summarized" using the
- * class. There are 3 main differences wrpt the ImuFactor class:
+ * PreintegratedCombinedMeasurements class. There are 3 main differences wrpt
- * 1) The factor is 6-ways, meaning that it also involves both biases (previous
+ * the ImuFactor class: 1) The factor is 6-ways, meaning that it also involves
- *    and current time step).Therefore, the factor internally imposes the biases
+ * both biases (previous and current time step).Therefore, the factor internally
- *    to be slowly varying; in particular, the matrices "biasAccCovariance" and
+ * imposes the biases to be slowly varying; in particular, the matrices
- *    "biasOmegaCovariance" described the random walk that models bias evolution.
+ * "biasAccCovariance" and "biasOmegaCovariance" described the random walk that
- * 2) The preintegration covariance takes into account the noise in the bias
+ * models bias evolution. 2) The preintegration covariance takes into account
- *    estimate used for integration.
+ * the noise in the bias estimate used for integration. 3) The covariance matrix
- * 3) The covariance matrix of the PreintegratedCombinedMeasurements preserves
+ * of the PreintegratedCombinedMeasurements preserves the correlation between
- *    the correlation between the bias uncertainty and the preintegrated
+ * the bias uncertainty and the preintegrated measurements uncertainty.
- *    measurements uncertainty.
  *
  * @ingroup navigation
  */
-class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3,
+class GTSAM_EXPORT CombinedImuFactor
-    Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {
+    : public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
-public:
+                               imuBias::ConstantBias, imuBias::ConstantBias> {
-
+ public:
-private:
+ private:
-
   typedef CombinedImuFactor This;
   typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
-      imuBias::ConstantBias, imuBias::ConstantBias> Base;
+                            imuBias::ConstantBias, imuBias::ConstantBias>
+      Base;
 
   PreintegratedCombinedMeasurements _PIM_;
 
-public:
+ public:
-
   // Provide access to Matrix& version of evaluateError:
   using Base::evaluateError;
 
@@ -315,7 +258,8 @@ public:
                                             DefaultKeyFormatter) const override;
 
   /// equals
-  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
+  bool equals(const NonlinearFactor& expected,
+              double tol = 1e-9) const override;
   /// @}
 
   /** Access the preintegrated measurements. */
@@ -329,10 +273,12 @@ public:
   /// vector of errors
   Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
                        const Pose3& pose_j, const Vector3& vel_j,
-      const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
+                       const imuBias::ConstantBias& bias_i,
+                       const imuBias::ConstantBias& bias_j,
                        OptionalMatrixType H1, OptionalMatrixType H2,
                        OptionalMatrixType H3, OptionalMatrixType H4,
-      OptionalMatrixType H5, OptionalMatrixType H6) const override;
+                       OptionalMatrixType H5,
+                       OptionalMatrixType H6) const override;
 
  private:
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
@@ -347,7 +293,7 @@ public:
   }
 #endif
 
-public:
+ public:
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // class CombinedImuFactor

gtsam/navigation/PreintegrationCombinedParams.h

@@ -0,0 +1,106 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  PreintegrationCombinedParams.h
+ *  @author Luca Carlone
+ *  @author Stephen Williams
+ *  @author Richard Roberts
+ *  @author Vadim Indelman
+ *  @author David Jensen
+ *  @author Frank Dellaert
+ *  @author Varun Agrawal
+ **/
+
+#pragma once
+
+/* GTSAM includes */
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/serialization.h>
+#include <gtsam/navigation/ManifoldPreintegration.h>
+#include <gtsam/navigation/PreintegrationCombinedParams.h>
+#include <gtsam/navigation/TangentPreintegration.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/// Parameters for pre-integration using PreintegratedCombinedMeasurements:
+/// Usage: Create just a single Params and pass a shared pointer to the
+/// constructor
+struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
+  Matrix3 biasAccCovariance;    ///< continuous-time "Covariance" describing
+                                ///< accelerometer bias random walk
+  Matrix3 biasOmegaCovariance;  ///< continuous-time "Covariance" describing
+                                ///< gyroscope bias random walk
+  Matrix6 biasAccOmegaInt;  ///< covariance of bias used as initial estimate.
+
+  /// Default constructor makes uninitialized params struct.
+  /// Used for serialization.
+  PreintegrationCombinedParams()
+      : biasAccCovariance(I_3x3),
+        biasOmegaCovariance(I_3x3),
+        biasAccOmegaInt(I_6x6) {}
+
+  /// See two named constructors below for good values of n_gravity in body
+  /// frame
+  PreintegrationCombinedParams(const Vector3& n_gravity)
+      : PreintegrationParams(n_gravity),
+        biasAccCovariance(I_3x3),
+        biasOmegaCovariance(I_3x3),
+        biasAccOmegaInt(I_6x6) {}
+
+  // Default Params for a Z-down navigation frame, such as NED: gravity points
+  // along positive Z-axis
+  static std::shared_ptr<PreintegrationCombinedParams> MakeSharedD(
+      double g = 9.81) {
+    return std::shared_ptr<PreintegrationCombinedParams>(
+        new PreintegrationCombinedParams(Vector3(0, 0, g)));
+  }
+
+  // Default Params for a Z-up navigation frame, such as ENU: gravity points
+  // along negative Z-axis
+  static std::shared_ptr<PreintegrationCombinedParams> MakeSharedU(
+      double g = 9.81) {
+    return std::shared_ptr<PreintegrationCombinedParams>(
+        new PreintegrationCombinedParams(Vector3(0, 0, -g)));
+  }
+
+  void print(const std::string& s = "") const override;
+  bool equals(const PreintegratedRotationParams& other,
+              double tol) const override;
+
+  void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance = cov; }
+  void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance = cov; }
+  void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt = cov; }
+
+  const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
+  const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
+  const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }
+
+ private:
+#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    namespace bs = ::boost::serialization;
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
+    ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
+    ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
+    ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
+  }
+#endif
+
+ public:
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace gtsam

gtsam/navigation/ScenarioRunner.cpp

@@ -106,7 +106,7 @@ Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const {
 PreintegratedCombinedMeasurements CombinedScenarioRunner::integrate(
     double T, const Bias& estimatedBias, bool corrupted) const {
   gttic_(integrate);
-  PreintegratedCombinedMeasurements pim(p_, estimatedBias);
+  PreintegratedCombinedMeasurements pim(p_, estimatedBias, preintMeasCov_);
 
   const double dt = imuSampleTime();
   const size_t nrSteps = T / dt;

gtsam/navigation/ScenarioRunner.h

@@ -118,15 +118,19 @@ class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner {
  private:
   const SharedParams p_;
   const Bias estimatedBias_;
+  const Eigen::Matrix<double, 15, 15> preintMeasCov_;
 
  public:
   CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p,
                          double imuSampleTime = 1.0 / 100.0,
-                         const Bias& bias = Bias())
+                         const Bias& bias = Bias(),
+                         const Eigen::Matrix<double, 15, 15>& preintMeasCov =
+                             Eigen::Matrix<double, 15, 15>::Zero())
       : ScenarioRunner(scenario, static_cast<ScenarioRunner::SharedParams>(p),
                        imuSampleTime, bias),
         p_(p),
-        estimatedBias_(bias) {}
+        estimatedBias_(bias),
+        preintMeasCov_(preintMeasCov) {}
 
   /// Integrate measurements for T seconds into a PIM
   PreintegratedCombinedMeasurements integrate(

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -36,14 +36,17 @@
 
 namespace testing {
 // Create default parameters with Z-down and above noise parameters
-static std::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() {
+static std::shared_ptr<PreintegratedCombinedMeasurements::Params> Params(
+    const Matrix3& biasAccCovariance = Matrix3::Zero(),
+    const Matrix3& biasOmegaCovariance = Matrix3::Zero(),
+    const Matrix6& biasAccOmegaInt = Matrix6::Zero()) {
   auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(kGravity);
   p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
   p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
   p->integrationCovariance = 0.0001 * I_3x3;
-  p->biasAccCovariance = Z_3x3;
+  p->biasAccCovariance = biasAccCovariance;
-  p->biasOmegaCovariance = Z_3x3;
+  p->biasOmegaCovariance = biasOmegaCovariance;
-  p->biasAccOmegaInt = Z_6x6;
+  p->biasAccOmegaInt = biasAccOmegaInt;
   return p;
 }
 }  // namespace testing
@@ -250,6 +253,7 @@ TEST(CombinedImuFactor, CheckCovariance) {
   EXPECT(assert_equal(expected, actual.preintMeasCov()));
 }
 
+/* ************************************************************************* */
 // Test that the covariance values for the ImuFactor and the CombinedImuFactor
 // (top-left 9x9) are the same
 TEST(CombinedImuFactor, SameCovariance) {
@@ -316,6 +320,43 @@ TEST(CombinedImuFactor, Accelerating) {
   EXPECT(assert_equal(estimatedCov, expected, 0.1));
 }
 
+/* ************************************************************************* */
+TEST(CombinedImuFactor, ResetIntegration) {
+  const double a = 0.2, v = 50;
+
+  // Set up body pointing towards y axis, and start at 10,20,0 with velocity
+  // going in X The body itself has Z axis pointing down
+  const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
+  const Point3 initial_position(10, 20, 0);
+  const Vector3 initial_velocity(v, 0, 0);
+
+  const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
+                                      Vector3(a, 0, 0));
+
+  const double T = 3.0;  // seconds
+
+  auto preinMeasCov = 0.001 * Eigen::Matrix<double, 15, 15>::Identity();
+  CombinedScenarioRunner runner(
+      scenario,
+      testing::Params(Matrix3::Zero(), Matrix3::Zero(),
+                      0.1 * Matrix6::Identity()),
+      T / 10, imuBias::ConstantBias(), preinMeasCov);
+
+  PreintegratedCombinedMeasurements pim = runner.integrate(T);
+  // Make copy for testing different conditions
+  PreintegratedCombinedMeasurements pim2 = pim;
+
+  // Test default method
+  pim.resetIntegration();
+  Matrix6 expected = 0.1 * I_6x6;
+  EXPECT(assert_equal(expected, pim.p().biasAccOmegaInt, 1e-9));
+
+  // Test method where Q_init is provided
+  Matrix6 expected_Q_init = I_6x6 * 0.001;
+  pim2.resetIntegration(expected_Q_init);
+  EXPECT(assert_equal(expected_Q_init, pim.p().biasAccOmegaInt, 1e-9));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

python/gtsam/examples/CombinedImuFactorExample.py

@@ -0,0 +1,256 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+A script validating and demonstrating inference with the CombinedImuFactor.
+
+Author: Varun Agrawal
+"""
+
+# pylint: disable=no-name-in-module,unused-import,arguments-differ,import-error,wrong-import-order
+
+from __future__ import print_function
+
+import argparse
+import math
+
+import matplotlib.pyplot as plt
+import numpy as np
+from gtsam.symbol_shorthand import B, V, X
+from gtsam.utils.plot import plot_pose3
+from mpl_toolkits.mplot3d import Axes3D
+from PreintegrationExample import POSES_FIG, PreintegrationExample
+
+import gtsam
+
+GRAVITY = 9.81
+
+np.set_printoptions(precision=3, suppress=True)
+
+
+def parse_args() -> argparse.Namespace:
+    """Parse command line arguments."""
+    parser = argparse.ArgumentParser("CombinedImuFactorExample.py")
+    parser.add_argument("--twist_scenario",
+                        default="sick_twist",
+                        choices=("zero_twist", "forward_twist", "loop_twist",
+                                 "sick_twist"))
+    parser.add_argument("--time",
+                        "-T",
+                        default=12,
+                        type=int,
+                        help="Total navigation time in seconds")
+    parser.add_argument("--compute_covariances",
+                        default=False,
+                        action='store_true')
+    parser.add_argument("--verbose", default=False, action='store_true')
+    return parser.parse_args()
+
+
+class CombinedImuFactorExample(PreintegrationExample):
+    """Class to run example of the Imu Factor."""
+    def __init__(self, twist_scenario: str = "sick_twist"):
+        self.velocity = np.array([2, 0, 0])
+        self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
+        self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
+        self.biasNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.001)
+
+        # Choose one of these twists to change scenario:
+        twist_scenarios = dict(
+            zero_twist=(np.zeros(3), np.zeros(3)),
+            forward_twist=(np.zeros(3), self.velocity),
+            loop_twist=(np.array([0, -math.radians(30), 0]), self.velocity),
+            sick_twist=(np.array([math.radians(30), -math.radians(30),
+                                  0]), self.velocity))
+
+        accBias = np.array([-0.3, 0.1, 0.2])
+        gyroBias = np.array([0.1, 0.3, -0.1])
+        bias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
+
+        params = gtsam.PreintegrationCombinedParams.MakeSharedU(GRAVITY)
+
+        # Some arbitrary noise sigmas
+        gyro_sigma = 1e-3
+        accel_sigma = 1e-3
+        I_3x3 = np.eye(3)
+        params.setGyroscopeCovariance(gyro_sigma**2 * I_3x3)
+        params.setAccelerometerCovariance(accel_sigma**2 * I_3x3)
+        params.setIntegrationCovariance(1e-7**2 * I_3x3)
+
+        dt = 1e-2
+        super(CombinedImuFactorExample,
+              self).__init__(twist_scenarios[twist_scenario], bias, params, dt)
+
+    def addPrior(self, i: int, graph: gtsam.NonlinearFactorGraph):
+        """Add a prior on the navigation state at time `i`."""
+        state = self.scenario.navState(i)
+        graph.push_back(
+            gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise))
+        graph.push_back(
+            gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise))
+        graph.push_back(
+            gtsam.PriorFactorConstantBias(B(i), self.actualBias,
+                                          self.biasNoise))
+
+    def optimize(self, graph: gtsam.NonlinearFactorGraph,
+                 initial: gtsam.Values):
+        """Optimize using Levenberg-Marquardt optimization."""
+        params = gtsam.LevenbergMarquardtParams()
+        params.setVerbosityLM("SUMMARY")
+        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
+        result = optimizer.optimize()
+        return result
+
+    def plot(self,
+             values: gtsam.Values,
+             title: str = "Estimated Trajectory",
+             fignum: int = POSES_FIG + 1,
+             show: bool = False):
+        """
+        Plot poses in values.
+
+        Args:
+            values: The values object with the poses to plot.
+            title: The title of the plot.
+            fignum: The matplotlib figure number.
+                POSES_FIG is a value from the PreintegrationExample
+                which we simply increment to generate a new figure.
+            show: Flag indicating whether to display the figure.
+        """
+        i = 0
+        while values.exists(X(i)):
+            pose_i = values.atPose3(X(i))
+            plot_pose3(fignum, pose_i, 1)
+            i += 1
+        plt.title(title)
+
+        gtsam.utils.plot.set_axes_equal(fignum)
+
+        i = 0
+        while values.exists(B(i)):
+            print("Bias Value {0}".format(i), values.atConstantBias(B(i)))
+            i += 1
+
+        plt.ioff()
+
+        if show:
+            plt.show()
+
+    def run(self,
+            T: int = 12,
+            compute_covariances: bool = False,
+            verbose: bool = True):
+        """
+        Main runner.
+
+        Args:
+            T: Total trajectory time.
+            compute_covariances: Flag indicating whether to compute marginal covariances.
+            verbose: Flag indicating if printing should be verbose.
+        """
+        graph = gtsam.NonlinearFactorGraph()
+
+        # initialize data structure for pre-integrated IMU measurements
+        pim = gtsam.PreintegratedCombinedMeasurements(self.params,
+                                                      self.actualBias)
+
+        num_poses = T  # assumes 1 factor per second
+        initial = gtsam.Values()
+
+        # simulate the loop
+        i = 0  # state index
+        initial_state_i = self.scenario.navState(0)
+        initial.insert(X(i), initial_state_i.pose())
+        initial.insert(V(i), initial_state_i.velocity())
+        initial.insert(B(i), self.actualBias)
+
+        # add prior on beginning
+        self.addPrior(0, graph)
+
+        for k, t in enumerate(np.arange(0, T, self.dt)):
+            # get measurements and add them to PIM
+            measuredOmega = self.runner.measuredAngularVelocity(t)
+            measuredAcc = self.runner.measuredSpecificForce(t)
+            pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
+
+            # Plot IMU many times
+            if k % 10 == 0:
+                self.plotImu(t, measuredOmega, measuredAcc)
+
+            if (k + 1) % int(1 / self.dt) == 0:
+                # Plot every second
+                self.plotGroundTruthPose(t, scale=1)
+                plt.title("Ground Truth Trajectory")
+
+                # create IMU factor every second
+                factor = gtsam.CombinedImuFactor(X(i), V(i), X(i + 1),
+                                                 V(i + 1), B(i), B(i + 1), pim)
+                graph.push_back(factor)
+
+                if verbose:
+                    print(factor)
+                    print("Predicted state at {0}:\n{1}".format(
+                        t + self.dt,
+                        pim.predict(initial_state_i, self.actualBias)))
+
+                pim.resetIntegration()
+
+                rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3) * 0.1)
+                translationNoise = gtsam.Point3(*np.random.randn(3) * 1)
+                poseNoise = gtsam.Pose3(rotationNoise, translationNoise)
+
+                actual_state_i = self.scenario.navState(t + self.dt)
+                print("Actual state at {0}:\n{1}".format(
+                    t + self.dt, actual_state_i))
+
+                # Set initial state to current
+                initial_state_i = actual_state_i
+
+                noisy_state_i = gtsam.NavState(
+                    actual_state_i.pose().compose(poseNoise),
+                    actual_state_i.velocity() + np.random.randn(3) * 0.1)
+                noisy_bias_i = self.actualBias + gtsam.imuBias.ConstantBias(
+                    np.random.randn(3) * 0.1,
+                    np.random.randn(3) * 0.1)
+
+                initial.insert(X(i + 1), noisy_state_i.pose())
+                initial.insert(V(i + 1), noisy_state_i.velocity())
+                initial.insert(B(i + 1), noisy_bias_i)
+                i += 1
+
+        # add priors on end
+        self.addPrior(num_poses - 1, graph)
+
+        initial.print("Initial values:")
+
+        result = self.optimize(graph, initial)
+
+        result.print("Optimized values:")
+        print("------------------")
+        print("Initial Error =", graph.error(initial))
+        print("Final Error =", graph.error(result))
+        print("------------------")
+
+        if compute_covariances:
+            # Calculate and print marginal covariances
+            marginals = gtsam.Marginals(graph, result)
+            print("Covariance on bias:\n",
+                  marginals.marginalCovariance(BIAS_KEY))
+            for i in range(num_poses):
+                print("Covariance on pose {}:\n{}\n".format(
+                    i, marginals.marginalCovariance(X(i))))
+                print("Covariance on vel {}:\n{}\n".format(
+                    i, marginals.marginalCovariance(V(i))))
+
+        self.plot(result, show=True)
+
+
+if __name__ == '__main__':
+    args = parse_args()
+
+    CombinedImuFactorExample(args.twist_scenario).run(args.time,
+                                                      args.compute_covariances,
+                                                      args.verbose)

python/gtsam/examples/ImuFactorExample.py

@@ -17,15 +17,15 @@ from __future__ import print_function
 import argparse
 import math
 
-import gtsam
 import matplotlib.pyplot as plt
 import numpy as np
 from gtsam.symbol_shorthand import B, V, X
 from gtsam.utils.plot import plot_pose3
 from mpl_toolkits.mplot3d import Axes3D
-
 from PreintegrationExample import POSES_FIG, PreintegrationExample
 
+import gtsam
+
 BIAS_KEY = B(0)
 GRAVITY = 9.81
 
@@ -185,7 +185,9 @@ class ImuFactorExample(PreintegrationExample):
 
                 if verbose:
                     print(factor)
-                    print(pim.predict(initial_state_i, self.actualBias))
+                    print("Predicted state at {0}:\n{1}".format(
+                        t + self.dt,
+                        pim.predict(initial_state_i, self.actualBias)))
 
                 pim.resetIntegration()
 
@@ -197,6 +199,9 @@ class ImuFactorExample(PreintegrationExample):
                 print("Actual state at {0}:\n{1}".format(
                     t + self.dt, actual_state_i))
 
+                # Set initial state to current
+                initial_state_i = actual_state_i
+
                 noisy_state_i = gtsam.NavState(
                     actual_state_i.pose().compose(poseNoise),
                     actual_state_i.velocity() + np.random.randn(3) * 0.1)