Fix merge conflicts

.github/scripts/boost.sh

@@ -1,18 +0,0 @@
-### Script to install Boost
-BOOST_FOLDER=boost_${BOOST_VERSION//./_}
-
-# Download Boost
-wget https://boostorg.jfrog.io/artifactory/main/release/${BOOST_VERSION}/source/${BOOST_FOLDER}.tar.gz
-
-# Unzip
-tar -zxf ${BOOST_FOLDER}.tar.gz
-
-# Bootstrap
-cd ${BOOST_FOLDER}/
-./bootstrap.sh --with-libraries=serialization,filesystem,thread,system,atomic,date_time,timer,chrono,program_options,regex
-
-# Build and install
-sudo ./b2 -j$(nproc) install
-
-# Rebuild ld cache
-sudo ldconfig

.github/scripts/unix.sh

@@ -58,8 +58,10 @@ function configure()
   fi
 
   # GTSAM_BUILD_WITH_MARCH_NATIVE=OFF: to avoid crashes in builder VMs
+  # CMAKE_CXX_FLAGS="-w": Suppress warnings to avoid IO latency in CI logs
   cmake $SOURCE_DIR \
       -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \
+      -DCMAKE_CXX_FLAGS="-w" \
       -DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \
       -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
       -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
@@ -70,11 +72,9 @@ function configure()
       -DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \
       -DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \
       -DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \
+      -DGTSAM_FORCE_SHARED_LIB=${GTSAM_FORCE_SHARED_LIB:-OFF} \
       -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-      -DGTSAM_SINGLE_TEST_EXE=OFF \
+      -DGTSAM_SINGLE_TEST_EXE=OFF
-      -DBOOST_ROOT=$BOOST_ROOT \
-      -DBoost_NO_SYSTEM_PATHS=ON \
-      -DBoost_ARCHITECTURE=-x64
 }
 
 
@@ -97,7 +97,7 @@ function build ()
 
   if [ "$(uname)" == "Linux" ]; then
     if (($(nproc) > 2)); then
-      make -j$(nproc)
+      make -j4
     else
       make -j2
     fi

.github/workflows/build-linux.yml

@@ -1,6 +1,6 @@
 name: Linux CI
 
-on: [push, pull_request]
+on: [pull_request]
 
 jobs:
   build:
@@ -12,7 +12,6 @@ jobs:
       CTEST_PARALLEL_LEVEL: 2
       CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
       GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
-      BOOST_VERSION: 1.67.0
 
     strategy:
       fail-fast: true
@@ -82,7 +81,7 @@ jobs:
 
       - name: Install Boost
         run: |
-          bash .github/scripts/boost.sh
+          sudo apt-get -y install libboost-all-dev
 
       - name: Build and Test
         run: bash .github/scripts/unix.sh -t

.github/workflows/build-python.yml

@@ -53,6 +53,7 @@ jobs:
     steps:
       - name: Checkout
         uses: actions/checkout@v3
+
       - name: Install (Linux)
         if: runner.os == 'Linux'
         run: |
@@ -79,6 +80,7 @@ jobs:
             echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
             echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
           fi
+
       - name: Install (macOS)
         if: runner.os == 'macOS'
         run: |
@@ -88,22 +90,27 @@ jobs:
           sudo xcode-select -switch /Applications/Xcode.app
           echo "CC=clang" >> $GITHUB_ENV
           echo "CXX=clang++" >> $GITHUB_ENV
+
       - name: Set GTSAM_WITH_TBB Flag
         if: matrix.flag == 'tbb'
         run: |
           echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV
           echo "GTSAM Uses TBB"
+
       - name: Set Swap Space
         if: runner.os == 'Linux'
         uses: pierotofy/set-swap-space@master
         with:
           swap-size-gb: 6
+
       - name: Install Dependencies
         run: |
           bash .github/scripts/python.sh -d
+
       - name: Build
         run: |
           bash .github/scripts/python.sh -b
+
       - name: Test
         run: |
           bash .github/scripts/python.sh -t

.github/workflows/build-special.yml

@@ -12,7 +12,7 @@ jobs:
       CTEST_PARALLEL_LEVEL: 2
       CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
       GTSAM_BUILD_UNSTABLE: ON
-      BOOST_VERSION: 1.67.0
+      GTSAM_FORCE_SHARED_LIB: ON  # Make shared library to save memory on CI
 
     strategy:
       fail-fast: false
@@ -100,14 +100,20 @@ jobs:
 
           sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev
 
-          sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
+          if [ "${{ matrix.compiler }}" = "gcc" ]; then
-          echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
+            sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
-          echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
+            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
+            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
+          else
+            sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
+            echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
+            echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
+          fi
 
       - name: Install Boost
         if: runner.os == 'Linux'
         run: |
-          bash .github/scripts/boost.sh
+          sudo apt-get -y install libboost-all-dev
 
       - name: Install (macOS)
         if: runner.os == 'macOS'
@@ -164,6 +170,13 @@ jobs:
           echo "GTSAM_BUILD_UNSTABLE=OFF" >> $GITHUB_ENV
           echo "GTSAM 'unstable' will not be built."
 
+      - name: Set Swap Space
+        if: runner.os == 'Linux'
+        uses: pierotofy/set-swap-space@master
+        with:
+          swap-size-gb: 12
+
+
       - name: Build & Test
         run: |
           bash .github/scripts/unix.sh -t

doc/CodingGuidelines.lyx

@@ -5,6 +5,7 @@
 \textclass article
 \begin_preamble
 \usepackage{color}
+\usepackage{listings}
 
 \definecolor{mygreen}{rgb}{0,0.6,0}
 \definecolor{mygray}{rgb}{0.5,0.5,0.5}

gtsam/hybrid/GaussianMixtureFactor.h

@@ -143,6 +143,9 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
    */
   double error(const HybridValues &values) const override;
 
+  /// Getter for GaussianFactor decision tree
+  const Factors &factors() const { return factors_; }
+
   /// Add MixtureFactor to a Sum, syntactic sugar.
   friend GaussianFactorGraphTree &operator+=(
       GaussianFactorGraphTree &sum, const GaussianMixtureFactor &factor) {

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -190,8 +190,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
 /* ************************************************************************ */
 // If any GaussianFactorGraph in the decision tree contains a nullptr, convert
 // that leaf to an empty GaussianFactorGraph. Needed since the DecisionTree will
-// otherwise create a GFG with a single (null) factor.
+// otherwise create a GFG with a single (null) factor, which doesn't register as null.
-// TODO(dellaert): still a mystery to me why this is needed.
 GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) {
   auto emptyGaussian = [](const GaussianFactorGraph &graph) {
     bool hasNull =
@@ -275,9 +274,9 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
     };
 
     DecisionTree<Key, double> probabilities(eliminationResults, probability);
-    return {std::make_shared<HybridConditional>(gaussianMixture),
+    return {
-            std::make_shared<DecisionTreeFactor>(discreteSeparator,
+        std::make_shared<HybridConditional>(gaussianMixture),
-                                                   probabilities)};
+        std::make_shared<DecisionTreeFactor>(discreteSeparator, probabilities)};
   } else {
     // Otherwise, we create a resulting GaussianMixtureFactor on the separator,
     // taking care to correct for conditional constant.

gtsam/hybrid/HybridSmoother.cpp

@@ -46,7 +46,7 @@ Ordering HybridSmoother::getOrdering(
   std::copy(allDiscrete.begin(), allDiscrete.end(),
             std::back_inserter(newKeysDiscreteLast));
 
-  const VariableIndex index(newFactors);
+  const VariableIndex index(factors);
 
   // Get an ordering where the new keys are eliminated last
   Ordering ordering = Ordering::ColamdConstrainedLast(

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -17,6 +17,7 @@
 
 #include <gtsam/discrete/DiscreteBayesNet.h>
 #include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
 #include <gtsam/hybrid/HybridNonlinearISAM.h>
@@ -35,14 +36,15 @@
 // Include for test suite
 #include <CppUnitLite/TestHarness.h>
 
-#include "Switching.h"
-
 #include <bitset>
 
+#include "Switching.h"
+
 using namespace std;
 using namespace gtsam;
 
 using symbol_shorthand::X;
+using symbol_shorthand::Z;
 
 Ordering getOrdering(HybridGaussianFactorGraph& factors,
                      const HybridGaussianFactorGraph& newFactors) {
@@ -91,8 +93,7 @@ TEST(HybridEstimation, Full) {
     hybridOrdering.push_back(M(k));
   }
 
-  HybridBayesNet::shared_ptr bayesNet =
+  HybridBayesNet::shared_ptr bayesNet = graph.eliminateSequential();
-      graph.eliminateSequential();
 
   EXPECT_LONGS_EQUAL(2 * K - 1, bayesNet->size());
 
@@ -338,7 +339,6 @@ TEST(HybridEstimation, ProbabilityMultifrontal) {
   Switching switching(K, between_sigma, measurement_sigma, measurements,
                       "1/1 1/1");
   auto graph = switching.linearizedFactorGraph;
-  Ordering ordering = getOrdering(graph, HybridGaussianFactorGraph());
 
   // Get the tree of unnormalized probabilities for each mode sequence.
   AlgebraicDecisionTree<Key> expected_probPrimeTree = getProbPrimeTree(graph);
@@ -503,6 +503,179 @@ TEST(HybridEstimation, CorrectnessViaSampling) {
   }
 }
 
+/****************************************************************************/
+/**
+ * Helper function to add the constant term corresponding to
+ * the difference in noise models.
+ */
+std::shared_ptr<GaussianMixtureFactor> mixedVarianceFactor(
+    const MixtureFactor& mf, const Values& initial, const Key& mode,
+    double noise_tight, double noise_loose, size_t d, size_t tight_index) {
+  GaussianMixtureFactor::shared_ptr gmf = mf.linearize(initial);
+
+  constexpr double log2pi = 1.8378770664093454835606594728112;
+  // logConstant will be of the tighter model
+  double logNormalizationConstant = log(1.0 / noise_tight);
+  double logConstant = -0.5 * d * log2pi + logNormalizationConstant;
+
+  auto func = [&](const Assignment<Key>& assignment,
+                  const GaussianFactor::shared_ptr& gf) {
+    if (assignment.at(mode) != tight_index) {
+      double factor_log_constant = -0.5 * d * log2pi + log(1.0 / noise_loose);
+
+      GaussianFactorGraph _gfg;
+      _gfg.push_back(gf);
+      Vector c(d);
+      for (size_t i = 0; i < d; i++) {
+        c(i) = std::sqrt(2.0 * (logConstant - factor_log_constant));
+      }
+
+      _gfg.emplace_shared<JacobianFactor>(c);
+      return std::make_shared<JacobianFactor>(_gfg);
+    } else {
+      return dynamic_pointer_cast<JacobianFactor>(gf);
+    }
+  };
+  auto updated_components = gmf->factors().apply(func);
+  auto updated_gmf = std::make_shared<GaussianMixtureFactor>(
+      gmf->continuousKeys(), gmf->discreteKeys(), updated_components);
+
+  return updated_gmf;
+}
+
+/****************************************************************************/
+TEST(HybridEstimation, ModeSelection) {
+  HybridNonlinearFactorGraph graph;
+  Values initial;
+
+  auto measurement_model = noiseModel::Isotropic::Sigma(1, 0.1);
+  auto motion_model = noiseModel::Isotropic::Sigma(1, 1.0);
+
+  graph.emplace_shared<PriorFactor<double>>(X(0), 0.0, measurement_model);
+  graph.emplace_shared<PriorFactor<double>>(X(1), 0.0, measurement_model);
+
+  DiscreteKeys modes;
+  modes.emplace_back(M(0), 2);
+
+  // The size of the noise model
+  size_t d = 1;
+  double noise_tight = 0.5, noise_loose = 5.0;
+
+  auto model0 = std::make_shared<MotionModel>(
+           X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_loose)),
+       model1 = std::make_shared<MotionModel>(
+           X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight));
+
+  std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
+
+  KeyVector keys = {X(0), X(1)};
+  MixtureFactor mf(keys, modes, components);
+
+  initial.insert(X(0), 0.0);
+  initial.insert(X(1), 0.0);
+
+  auto gmf =
+      mixedVarianceFactor(mf, initial, M(0), noise_tight, noise_loose, d, 1);
+  graph.add(gmf);
+
+  auto gfg = graph.linearize(initial);
+
+  HybridBayesNet::shared_ptr bayesNet = gfg->eliminateSequential();
+
+  HybridValues delta = bayesNet->optimize();
+  EXPECT_LONGS_EQUAL(1, delta.discrete().at(M(0)));
+
+  /**************************************************************/
+  HybridBayesNet bn;
+  const DiscreteKey mode{M(0), 2};
+
+  bn.push_back(
+      GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(0), Z_1x1, 0.1));
+  bn.push_back(
+      GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(1), Z_1x1, 0.1));
+  bn.emplace_back(new GaussianMixture(
+      {Z(0)}, {X(0), X(1)}, {mode},
+      {GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1),
+                                                Z_1x1, noise_loose),
+       GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1),
+                                                Z_1x1, noise_tight)}));
+
+  VectorValues vv;
+  vv.insert(Z(0), Z_1x1);
+
+  auto fg = bn.toFactorGraph(vv);
+  auto expected_posterior = fg.eliminateSequential();
+
+  EXPECT(assert_equal(*expected_posterior, *bayesNet, 1e-6));
+}
+
+/****************************************************************************/
+TEST(HybridEstimation, ModeSelection2) {
+  using symbol_shorthand::Z;
+
+  // The size of the noise model
+  size_t d = 3;
+  double noise_tight = 0.5, noise_loose = 5.0;
+
+  HybridBayesNet bn;
+  const DiscreteKey mode{M(0), 2};
+
+  bn.push_back(
+      GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(0), Z_3x1, 0.1));
+  bn.push_back(
+      GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(1), Z_3x1, 0.1));
+  bn.emplace_back(new GaussianMixture(
+      {Z(0)}, {X(0), X(1)}, {mode},
+      {GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1),
+                                                Z_3x1, noise_loose),
+       GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1),
+                                                Z_3x1, noise_tight)}));
+
+  VectorValues vv;
+  vv.insert(Z(0), Z_3x1);
+
+  auto fg = bn.toFactorGraph(vv);
+
+  auto expected_posterior = fg.eliminateSequential();
+
+  // =====================================
+
+  HybridNonlinearFactorGraph graph;
+  Values initial;
+
+  auto measurement_model = noiseModel::Isotropic::Sigma(d, 0.1);
+  auto motion_model = noiseModel::Isotropic::Sigma(d, 1.0);
+
+  graph.emplace_shared<PriorFactor<Vector3>>(X(0), Z_3x1, measurement_model);
+  graph.emplace_shared<PriorFactor<Vector3>>(X(1), Z_3x1, measurement_model);
+
+  DiscreteKeys modes;
+  modes.emplace_back(M(0), 2);
+
+  auto model0 = std::make_shared<BetweenFactor<Vector3>>(
+           X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_loose)),
+       model1 = std::make_shared<BetweenFactor<Vector3>>(
+           X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight));
+
+  std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
+
+  KeyVector keys = {X(0), X(1)};
+  MixtureFactor mf(keys, modes, components);
+
+  initial.insert<Vector3>(X(0), Z_3x1);
+  initial.insert<Vector3>(X(1), Z_3x1);
+
+  auto gmf =
+      mixedVarianceFactor(mf, initial, M(0), noise_tight, noise_loose, d, 1);
+  graph.add(gmf);
+
+  auto gfg = graph.linearize(initial);
+
+  HybridBayesNet::shared_ptr bayesNet = gfg->eliminateSequential();
+
+  EXPECT(assert_equal(*expected_posterior, *bayesNet, 1e-6));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/linear/HessianFactor.h

@@ -131,7 +131,7 @@ namespace gtsam {
      * term, and f the constant term.
      * JacobianFactor error is \f[ 0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb \f]
      * HessianFactor  error is \f[ 0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx    - x'*g   + 0.5*f    \f]
-     * So, with \f$ A = [A1 A2] \f$ and \f$ G=A*'M*A = [A1';A2']*M*[A1 A2] \f$ we have
+     * So, with \f$ A = [A1 A2] \f$ and \f$ G=A'*M*A = [A1';A2']*M*[A1 A2] \f$ we have
      \code
       n1*n1 G11 = A1'*M*A1
       n1*n2 G12 = A1'*M*A2

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 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
-  *  @param base               PreintegrationType instance
+   * preintMeasCov
-  *  @param preintMeasCov      Covariance matrix used in noise model.
+   *  @param base               PreintegrationType instance
-  */
+   *  @param preintMeasCov      Covariance matrix used in noise model.
-  PreintegratedCombinedMeasurements(const PreintegrationType& base, const Eigen::Matrix<double, 15, 15>& preintMeasCov)
+   */
-     : PreintegrationType(base),
+  PreintegratedCombinedMeasurements(
-       preintMeasCov_(preintMeasCov) {
+      const PreintegrationType& base,
-  }
+      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. */
@@ -328,11 +272,13 @@ public:
 
   /// vector of errors
   Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
-      const Pose3& pose_j, const Vector3& vel_j,
+                       const Pose3& pose_j, const Vector3& vel_j,
-      const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
+                       const imuBias::ConstantBias& bias_i,
-      OptionalMatrixType H1, OptionalMatrixType H2, 
+                       const imuBias::ConstantBias& bias_j,
-      OptionalMatrixType H3, OptionalMatrixType H4, 
+                       OptionalMatrixType H1, OptionalMatrixType H2,
-      OptionalMatrixType H5, OptionalMatrixType H6) const override;
+                       OptionalMatrixType H3, OptionalMatrixType H4,
+                       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)