Fix CombinedImuFactor

doc/refs.bib

@@ -1,26 +1,72 @@
+%% This BibTeX bibliography file was created using BibDesk.
+%% https://bibdesk.sourceforge.io/
+
+%% Created for Varun Agrawal at 2021-09-27 17:39:09 -0400 
+
+
+%% Saved with string encoding Unicode (UTF-8) 
+
+
+
+@article{Lupton12tro,
+	author = {Lupton, Todd and Sukkarieh, Salah},
+	date-added = {2021-09-27 17:38:56 -0400},
+	date-modified = {2021-09-27 17:39:09 -0400},
+	doi = {10.1109/TRO.2011.2170332},
+	journal = {IEEE Transactions on Robotics},
+	number = {1},
+	pages = {61-76},
+	title = {Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions},
+	volume = {28},
+	year = {2012},
+	Bdsk-Url-1 = {https://doi.org/10.1109/TRO.2011.2170332}}
+
+@inproceedings{Forster15rss,
+	author = {Christian Forster and Luca Carlone and Frank Dellaert and Davide Scaramuzza},
+	booktitle = {Robotics: Science and Systems},
+	date-added = {2021-09-26 20:44:41 -0400},
+	date-modified = {2021-09-26 20:45:03 -0400},
+	title = {IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation},
+	year = {2015}}
+
 @article{Iserles00an,
-  title =	 {Lie-group methods},
-  author =	 {Iserles, Arieh and Munthe-Kaas, Hans Z and
-                  N{\o}rsett, Syvert P and Zanna, Antonella},
-  journal =	 {Acta Numerica 2000},
-  volume =	 {9},
-  pages =	 {215--365},
-  year =	 {2000},
-  publisher =	 {Cambridge Univ Press}
-}
+	author = {Iserles, Arieh and Munthe-Kaas, Hans Z and N{\o}rsett, Syvert P and Zanna, Antonella},
+	journal = {Acta Numerica 2000},
+	pages = {215--365},
+	publisher = {Cambridge Univ Press},
+	title = {Lie-group methods},
+	volume = {9},
+	year = {2000}}
 
 @book{Murray94book,
-  title =	 {A mathematical introduction to robotic manipulation},
-  author =	 {Murray, Richard M and Li, Zexiang and Sastry, S
-                  Shankar and Sastry, S Shankara},
-  year =	 {1994},
-  publisher =	 {CRC press}
-}
+	author = {Murray, Richard M and Li, Zexiang and Sastry, S Shankar and Sastry, S Shankara},
+	publisher = {CRC press},
+	title = {A mathematical introduction to robotic manipulation},
+	year = {1994}}
 
 @book{Spivak65book,
-  title =	 {Calculus on manifolds},
-  author =	 {Spivak, Michael},
-  volume =	 {1},
-  year =	 {1965},
-  publisher =	 {WA Benjamin New York}
-}
+	author = {Spivak, Michael},
+	publisher = {WA Benjamin New York},
+	title = {Calculus on manifolds},
+	volume = {1},
+	year = {1965}}
+
+@phdthesis{Nikolic16thesis,
+  title={Characterisation, calibration, and design of visual-inertial sensor systems for robot navigation},
+  author={Nikolic, Janosch},
+  year={2016},
+  school={ETH Zurich}
+}
+
+@book{Simon06book,
+  title={Optimal state estimation: Kalman, H infinity, and nonlinear approaches},
+  author={Simon, Dan},
+  year={2006},
+  publisher={John Wiley \& Sons}
+}
+
+@inproceedings{Trawny05report_IndirectKF,
+  title={Indirect Kalman Filter for 3 D Attitude Estimation},
+  author={Nikolas Trawny and Stergios I. Roumeliotis},
+  year={2005}
+}

examples/CombinedImuFactorsExample.cpp

@@ -60,13 +60,14 @@ namespace po = boost::program_options;
 
 po::variables_map parseOptions(int argc, char* argv[]) {
   po::options_description desc;
-  desc.add_options()("help,h", "produce help message")(
-      "data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
-      "path to the CSV file with the IMU data")(
-      "output_filename",
-      po::value<string>()->default_value("imuFactorExampleResults.csv"),
-      "path to the result file to use")("use_isam", po::bool_switch(),
-                                        "use ISAM as the optimizer");
+  desc.add_options()("help,h", "produce help message")  // help message
+      ("data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
+       "path to the CSV file with the IMU data")  // path to the data file
+      ("output_filename",
+       po::value<string>()->default_value("imuFactorExampleResults.csv"),
+       "path to the result file to use")  // filename to save results to
+      ("use_isam", po::bool_switch(),
+       "use ISAM as the optimizer");  // flag for ISAM optimizer
 
   po::variables_map vm;
   po::store(po::parse_command_line(argc, argv, desc), vm);
@@ -106,7 +107,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
       I_3x3 * 1e-8;  // error committed in integrating position from velocities
   Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
   Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
-  Matrix66 bias_acc_omega_int =
+  Matrix66 bias_acc_omega_init =
       I_6x6 * 1e-5;  // error in the bias used for preintegration
 
   auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
@@ -122,7 +123,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
   // PreintegrationCombinedMeasurements params:
   p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
   p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
-  p->biasAccOmegaInt = bias_acc_omega_int;
+  p->biasAccOmegaInit = bias_acc_omega_init;
 
   return p;
 }

examples/ImuFactorsExample.cpp

@@ -94,7 +94,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
       I_3x3 * 1e-8;  // error committed in integrating position from velocities
   Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
   Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
-  Matrix66 bias_acc_omega_int =
+  Matrix66 bias_acc_omega_init =
       I_6x6 * 1e-5;  // error in the bias used for preintegration
 
   auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
@@ -110,7 +110,7 @@ boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
   // PreintegrationCombinedMeasurements params:
   p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
   p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
-  p->biasAccOmegaInt = bias_acc_omega_int;
+  p->biasAccOmegaInit = bias_acc_omega_init;
 
   return p;
 }

gtsam/base/Vector.h

@@ -60,6 +60,7 @@ GTSAM_MAKE_VECTOR_DEFS(9)
 GTSAM_MAKE_VECTOR_DEFS(10)
 GTSAM_MAKE_VECTOR_DEFS(11)
 GTSAM_MAKE_VECTOR_DEFS(12)
+GTSAM_MAKE_VECTOR_DEFS(15)
 
 typedef Eigen::VectorBlock<Vector> SubVector;
 typedef Eigen::VectorBlock<const Vector> ConstSubVector;

gtsam/navigation/CombinedImuFactor.cpp

@@ -39,7 +39,7 @@ void PreintegrationCombinedParams::print(const string& s) const {
        << endl;
   cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]"
        << endl;
-  cout << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\n]"
+  cout << "biasAccOmegaInit:\n[\n" << biasAccOmegaInit << "\n]"
        << endl;
 }
 
@@ -52,7 +52,7 @@ bool PreintegrationCombinedParams::equals(const PreintegratedRotationParams& oth
                             tol) &&
          equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance,
                             tol) &&
-         equal_with_abs_tol(biasAccOmegaInt, e->biasAccOmegaInt, tol);
+         equal_with_abs_tol(biasAccOmegaInit, e->biasAccOmegaInit, tol);
 }
 
 //------------------------------------------------------------------------------
@@ -93,9 +93,14 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
 //------------------------------------------------------------------------------
 void PreintegratedCombinedMeasurements::integrateMeasurement(
     const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
+  if (dt <= 0) {
+    throw std::runtime_error(
+        "PreintegratedCombinedMeasurements::integrateMeasurement: dt <=0");
+  }
+
   // Update preintegrated measurements.
-  Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
-  Matrix93 B, C;
+  Matrix9 A; // Jacobian wrt preintegrated measurements without bias (df/dx)
+  Matrix93 B, C;  // Jacobian of state wrpt accel bias and omega bias respectively.
   PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
 
   // Update preintegrated measurements covariance: as in [2] we consider a first
@@ -105,47 +110,80 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
   // and preintegrated measurements
 
   // Single Jacobians to propagate covariance
-  // TODO(frank): should we not also account for bias on position?
-  Matrix3 theta_H_biasOmega = -C.topRows<3>();
-  Matrix3 vel_H_biasAcc = -B.bottomRows<3>();
+  Matrix3 theta_H_omega = C.topRows<3>();
+  Matrix3 pos_H_acc = B.middleRows<3>(3);
+  Matrix3 vel_H_acc = B.bottomRows<3>();
+
+  Matrix3 theta_H_biasOmegaInit = -theta_H_omega;
+  Matrix3 pos_H_biasAccInit = -pos_H_acc;
+  Matrix3 vel_H_biasAcc = -vel_H_acc;
+  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>(6, 9) = vel_H_biasAcc;
+  F.block<3, 3>(0, 12) = theta_H_omega;
+  F.block<3, 3>(3, 9) = pos_H_acc;
+  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]).
+  preintMeasCov_ = F * preintMeasCov_ * F.transpose();
+
   // propagate uncertainty
   // TODO(frank): use noiseModel routine so we can have arbitrary noise models.
   const Matrix3& aCov = p().accelerometerCovariance;
   const Matrix3& wCov = p().gyroscopeCovariance;
   const Matrix3& iCov = p().integrationCovariance;
+  const Matrix6& bInitCov = p().biasAccOmegaInit;
 
   // first order uncertainty propagation
-  // Optimized matrix multiplication   (1/dt) * G * measurementCovariance *
-  // G.transpose()
+  // Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose()
   Eigen::Matrix<double, 15, 15> G_measCov_Gt;
   G_measCov_Gt.setZero(15, 15);
 
   // BLOCK DIAGONAL TERMS
-  D_t_t(&G_measCov_Gt) = dt * iCov;
-  D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
-      * (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0))
-      * (vel_H_biasAcc.transpose());
-  D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
-      * (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
-      * (theta_H_biasOmega.transpose());
+  D_R_R(&G_measCov_Gt) =
+      (theta_H_omega * (wCov / dt) * theta_H_omega.transpose())  //
+      + (theta_H_biasOmegaInit * (bInitCov.block<3, 3>(3, 3) / dt) *
+         theta_H_biasOmegaInit.transpose());
+
+  D_t_t(&G_measCov_Gt) =
+      (pos_H_acc * (aCov / dt) * pos_H_acc.transpose())  //
+      + (pos_H_biasAccInit * (bInitCov.block<3, 3>(0, 0) / dt) *
+         pos_H_biasAccInit.transpose())  //
+      + (dt * iCov);
+
+  D_v_v(&G_measCov_Gt) =
+      (vel_H_acc * (aCov / dt) * vel_H_acc.transpose())  //
+      + (vel_H_biasAccInit * (bInitCov.block<3, 3>(0, 0) / dt) *
+         vel_H_biasAccInit.transpose());
+
   D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
   D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
 
   // OFF BLOCK DIAGONAL TERMS
-  Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0)
-      * theta_H_biasOmega.transpose();
-  D_v_R(&G_measCov_Gt) = temp;
-  D_R_v(&G_measCov_Gt) = temp.transpose();
-  preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
+  D_R_t(&G_measCov_Gt) = theta_H_biasOmegaInit *
+                         (bInitCov.block<3, 3>(3, 0) / dt) *
+                         pos_H_biasAccInit.transpose();
+  D_R_v(&G_measCov_Gt) = theta_H_biasOmegaInit *
+                         (bInitCov.block<3, 3>(3, 0) / dt) *
+                         vel_H_biasAccInit.transpose();
+  D_t_R(&G_measCov_Gt) = pos_H_biasAccInit * (bInitCov.block<3, 3>(0, 3) / dt) *
+                         theta_H_biasOmegaInit.transpose();
+  D_t_v(&G_measCov_Gt) =
+      (pos_H_acc * (aCov / dt) * vel_H_acc.transpose()) +
+      (pos_H_biasAccInit * (bInitCov.block<3, 3>(0, 0) / dt) *
+       vel_H_biasAccInit.transpose());
+  D_v_R(&G_measCov_Gt) = vel_H_biasAccInit * (bInitCov.block<3, 3>(0, 3) / dt) *
+                         theta_H_biasOmegaInit.transpose();
+  D_v_t(&G_measCov_Gt) =
+      (vel_H_acc * (aCov / dt) * pos_H_acc.transpose()) +
+      (vel_H_biasAccInit * (bInitCov.block<3, 3>(0, 0) / dt) *
+       pos_H_biasAccInit.transpose());
+
+  preintMeasCov_.noalias() += G_measCov_Gt;
 }
 
 //------------------------------------------------------------------------------

gtsam/navigation/CombinedImuFactor.h

@@ -51,6 +51,7 @@ typedef ManifoldPreintegration PreintegrationType;
  *     TRO, 28(1):61-76, 2012.
  * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
  *     Computation of the Jacobian Matrices", Tech. Report, 2013.
+ *     Available in this repo as "PreintegratedIMUJacobians.pdf".
  * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on
  *     Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,
  *     Robotics: Science and Systems (RSS), 2015.
@@ -61,19 +62,19 @@ typedef ManifoldPreintegration PreintegrationType;
 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 for pre-integration
+  Matrix6 biasAccOmegaInit;     ///< 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) {}
+        biasAccOmegaInit(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) {
+    biasOmegaCovariance(I_3x3), biasAccOmegaInit(I_6x6) {
 
   }
 
@@ -92,11 +93,11 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
 
   void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
   void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
-  void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
+  void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInit=cov; }
 
   const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
   const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
-  const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
+  const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInit; }
 
 private:
 
@@ -108,7 +109,7 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
     ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
     ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
-    ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
+    ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInit);
   }
 
 public:

gtsam/navigation/ImuFactor.cpp

@@ -59,7 +59,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
 
   // Update preintegrated measurements (also get Jacobian)
   Matrix9 A;  // overall Jacobian wrt preintegrated measurements (df/dx)
-  Matrix93 B, C;
+  Matrix93 B, C;  // Jacobian of state wrpt accel bias and omega bias respectively.
   PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
 
   // first order covariance propagation:
@@ -73,11 +73,13 @@ void PreintegratedImuMeasurements::integrateMeasurement(
   const Matrix3& iCov = p().integrationCovariance;
 
   // (1/dt) allows to pass from continuous time noise to discrete time noise
+  // Update the uncertainty on the state (matrix A in [4]).
   preintMeasCov_ = A * preintMeasCov_ * A.transpose();
+  // These 2 updates account for uncertainty on the IMU measurement (matrix B in [4]).
   preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose();
   preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose();
 
-  // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3
+  // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 (9x3 matrix)
   preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt;
 }
 

gtsam/navigation/ImuFactor.h

@@ -53,6 +53,7 @@ typedef ManifoldPreintegration PreintegrationType;
  *     TRO, 28(1):61-76, 2012.
  * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
  *     Computation of the Jacobian Matrices", Tech. Report, 2013.
+ *     Available in this repo as "PreintegratedIMUJacobians.pdf".
  * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
  *     Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
  *     Robotics: Science and Systems (RSS), 2015.

gtsam/navigation/PreintegrationBase.cpp

@@ -157,9 +157,9 @@ Vector9 PreintegrationBase::computeError(const NavState& state_i,
       state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0,
                                H1 || H3 ? &D_error_predict : 0);
 
-  if (H1) *H1 << D_error_predict* D_predict_state_i;
+  if (H1) *H1 << D_error_predict * D_predict_state_i;
   if (H2) *H2 << D_error_state_j;
-  if (H3) *H3 << D_error_predict* D_predict_bias_i;
+  if (H3) *H3 << D_error_predict * D_predict_bias_i;
 
   return error;
 }