split ImuFactor in .h and .cpp

gtsam/navigation/ImuFactor.cpp

@@ -0,0 +1,438 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  ImuFactor.h
+ *  @author Luca Carlone
+ *  @author Stephen Williams
+ *  @author Richard Roberts
+ *  @author Vadim Indelman
+ *  @author David Jensen
+ *  @author Frank Dellaert
+ **/
+
+#include <gtsam/navigation/ImuFactor.h>
+
+/* External or standard includes */
+#include <ostream>
+
+namespace gtsam {
+
+//------------------------------------------------------------------------------
+// Inner class PreintegratedMeasurements
+//------------------------------------------------------------------------------
+ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
+    const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
+    const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
+    const bool use2ndOrderIntegration) :
+                  biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()),
+                  // TODO: add this deltaRij_(Rot3()),
+                  deltaTij_(0.0),
+                  delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
+                  delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
+                  delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration)
+{
+  measurementCovariance_.setZero();
+  measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
+  measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
+  measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
+  PreintMeasCov_.setZero(9,9);
+}
+
+//------------------------------------------------------------------------------
+void ImuFactor::PreintegratedMeasurements::print(const std::string& s) const {
+  std::cout << s << std::endl;
+  biasHat_.print("  biasHat");
+  std::cout << "  deltaTij " << deltaTij_ << std::endl;
+  std::cout << "  deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
+  std::cout << "  deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
+  deltaRij_.print("  deltaRij ");
+  std::cout << "  measurementCovariance = \n [ " << measurementCovariance_ << " ]" << std::endl;
+  std::cout << "  PreintMeasCov = \n [ " << PreintMeasCov_ << " ]" << std::endl;
+}
+
+//------------------------------------------------------------------------------
+bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const {
+  return biasHat_.equals(expected.biasHat_, tol)
+      && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
+      && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
+      && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
+      && deltaRij_.equals(expected.deltaRij_, tol)
+      && std::fabs(deltaTij_ - expected.deltaTij_) < tol
+      && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
+      && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
+      && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
+      && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
+      && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
+}
+
+//------------------------------------------------------------------------------
+void ImuFactor::PreintegratedMeasurements::resetIntegration(){
+  deltaPij_ = Vector3::Zero();
+  deltaVij_ = Vector3::Zero();
+  deltaRij_ = Rot3();
+  deltaTij_ = 0.0;
+  delPdelBiasAcc_ = Z_3x3;
+  delPdelBiasOmega_ = Z_3x3;
+  delVdelBiasAcc_ = Z_3x3;
+  delVdelBiasOmega_ = Z_3x3;
+  delRdelBiasOmega_ = Z_3x3;
+  PreintMeasCov_.setZero(9,9);
+}
+
+//------------------------------------------------------------------------------
+void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
+    const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
+    boost::optional<const Pose3&> body_P_sensor) {
+
+  // NOTE: order is important here because each update uses old values (i.e., we have to update
+  // jacobians and covariances before updating preintegrated measurements).
+
+  // First we compensate the measurements for the bias
+  Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
+  Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
+
+  // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
+  if(body_P_sensor){
+    Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
+    correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
+    Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
+    correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
+    // linear acceleration vector in the body frame
+  }
+
+  const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
+  const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
+
+  const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
+
+  // Update Jacobians
+  /* ----------------------------------------------------------------------------------------------------------------------- */
+  if(!use2ndOrderIntegration_){
+    delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
+    delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
+  }else{
+    delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
+    delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
+                                            * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
+  }
+  delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
+  delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
+  delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr  * deltaT;
+
+  // Update preintegrated measurements covariance
+  // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework
+  /* ----------------------------------------------------------------------------------------------------------------------- */
+  const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
+  const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
+
+  Rot3 Rot_j = deltaRij_ * Rincr;
+  const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
+  const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
+
+  Matrix H_pos_pos    = I_3x3;
+  Matrix H_pos_vel    = I_3x3 * deltaT;
+  Matrix H_pos_angles = Z_3x3;
+
+  Matrix H_vel_pos    = Z_3x3;
+  Matrix H_vel_vel    = I_3x3;
+  Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
+  // analytic expression corresponding to the following numerical derivative
+  // Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
+
+  Matrix H_angles_pos   = Z_3x3;
+  Matrix H_angles_vel    = Z_3x3;
+  Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
+  // analytic expression corresponding to the following numerical derivative
+  // Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
+
+  // overall Jacobian wrt preintegrated measurements (df/dx)
+  Matrix F(9,9);
+  F << H_pos_pos, H_pos_vel,  H_pos_angles,
+      H_vel_pos, H_vel_vel, H_vel_angles,
+      H_angles_pos, H_angles_vel, H_angles_angles;
+
+  // first order uncertainty propagation:
+  // the deltaT allows to pass from continuous time noise to discrete time noise
+  // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
+  // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
+  PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ;
+
+  // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
+  // This in only kept for documentation.
+  //
+  // Matrix G(9,9);
+  // G << I_3x3 * deltaT, Z_3x3,  Z_3x3,
+  //      Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
+  //      Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
+  //
+  // PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
+
+  // Update preintegrated measurements (this has to be done after the update of covariances and jacobians!)
+  /* ----------------------------------------------------------------------------------------------------------------------- */
+  if(!use2ndOrderIntegration_){
+    deltaPij_ += deltaVij_ * deltaT;
+  }else{
+    deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
+  }
+  deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
+  deltaRij_ = deltaRij_ * Rincr;
+  deltaTij_ += deltaT;
+}
+
+//------------------------------------------------------------------------------
+// ImuFactor methods
+//------------------------------------------------------------------------------
+ImuFactor::ImuFactor() :
+    preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3), use2ndOrderCoriolis_(false){}
+
+//------------------------------------------------------------------------------
+ImuFactor::ImuFactor(
+    Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
+    const PreintegratedMeasurements& preintegratedMeasurements,
+    const Vector3& gravity, const Vector3& omegaCoriolis,
+    boost::optional<const Pose3&> body_P_sensor,
+    const bool use2ndOrderCoriolis) :
+        Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias),
+        preintegratedMeasurements_(preintegratedMeasurements),
+        gravity_(gravity),
+        omegaCoriolis_(omegaCoriolis),
+        body_P_sensor_(body_P_sensor),
+        use2ndOrderCoriolis_(use2ndOrderCoriolis){
+}
+
+//------------------------------------------------------------------------------
+gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
+  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+      gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+}
+
+//------------------------------------------------------------------------------
+void ImuFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
+  std::cout << s << "ImuFactor("
+      << keyFormatter(this->key1()) << ","
+      << keyFormatter(this->key2()) << ","
+      << keyFormatter(this->key3()) << ","
+      << keyFormatter(this->key4()) << ","
+      << keyFormatter(this->key5()) << ")\n";
+  preintegratedMeasurements_.print("  preintegrated measurements:");
+  std::cout << "  gravity: [ " << gravity_.transpose() << " ]" << std::endl;
+  std::cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
+  this->noiseModel_->print("  noise model: ");
+  if(this->body_P_sensor_)
+    this->body_P_sensor_->print("  sensor pose in body frame: ");
+}
+
+//------------------------------------------------------------------------------
+bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const {
+  const This *e =  dynamic_cast<const This*> (&expected);
+  return e != NULL && Base::equals(*e, tol)
+  && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
+  && equal_with_abs_tol(gravity_, e->gravity_, tol)
+  && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
+  && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
+}
+
+//------------------------------------------------------------------------------
+Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
+    const imuBias::ConstantBias& bias,
+    boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2,
+    boost::optional<Matrix&> H3,  boost::optional<Matrix&> H4,
+    boost::optional<Matrix&> H5) const
+{
+
+  const double& deltaTij = preintegratedMeasurements_.deltaTij_;
+  const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
+  const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
+
+  // we give some shorter name to rotations and translations
+  const Rot3 Rot_i = pose_i.rotation();
+  const Rot3 Rot_j = pose_j.rotation();
+  const Vector3 pos_i = pose_i.translation().vector();
+  const Vector3 pos_j = pose_j.translation().vector();
+
+  // We compute factor's Jacobians
+  /* ---------------------------------------------------------------------------------------------------- */
+  const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
+  // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
+
+  Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
+
+  Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
+      Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
+
+  const Rot3 deltaRij_biascorrected_corioliscorrected =
+      Rot3::Expmap( theta_biascorrected_corioliscorrected );
+
+  const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
+
+  const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
+
+  const Matrix3 Jtheta = -Jr_theta_bcc  * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
+
+  const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
+
+  if(H1) {
+    H1->resize(9,6);
+
+    Matrix3 dfPdPi;
+    Matrix3 dfVdPi;
+    if(use2ndOrderCoriolis_){
+      dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
+      dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
+    }
+    else{
+      dfPdPi = - Rot_i.matrix();
+      dfVdPi = Z_3x3;
+    }
+
+    (*H1) <<
+        // dfP/dRi
+        Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
+        + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
+        // dfP/dPi
+        dfPdPi,
+        // dfV/dRi
+        Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
+        + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
+        // dfV/dPi
+        dfVdPi,
+        // dfR/dRi
+        Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
+        // dfR/dPi
+        Z_3x3;
+  }
+
+  if(H2) {
+    H2->resize(9,3);
+    (*H2) <<
+        // dfP/dVi
+        - I_3x3 * deltaTij
+        + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij,  // Coriolis term - we got rid of the 2 wrt ins paper
+        // dfV/dVi
+        - I_3x3
+        + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
+        // dfR/dVi
+        Z_3x3;
+  }
+
+  if(H3) {
+    H3->resize(9,6);
+    (*H3) <<
+        // dfP/dPosej
+        Z_3x3, Rot_j.matrix(),
+        // dfV/dPosej
+        Matrix::Zero(3,6),
+        // dfR/dPosej
+        Jrinv_fRhat *  ( I_3x3 ), Z_3x3;
+  }
+
+  if(H4) {
+    H4->resize(9,3);
+    (*H4) <<
+        // dfP/dVj
+        Z_3x3,
+        // dfV/dVj
+        I_3x3,
+        // dfR/dVj
+        Z_3x3;
+  }
+
+  if(H5) {
+    const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
+    const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
+    const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
+
+    H5->resize(9,6);
+    (*H5) <<
+        // dfP/dBias
+        - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
+        - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
+        // dfV/dBias
+        - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
+        - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
+        // dfR/dBias
+        Matrix::Zero(3,3),
+        Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
+  }
+
+  // Evaluate residual error, according to [3]
+  /* ---------------------------------------------------------------------------------------------------- */
+  const Vector3 fp =
+      pos_j - pos_i
+      - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
+          + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
+          + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
+          - vel_i * deltaTij
+          + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
+          - 0.5 * gravity_ * deltaTij*deltaTij;
+
+  const Vector3 fv =
+      vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
+          + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
+          + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
+          + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij  // Coriolis term
+          - gravity_ * deltaTij;
+
+  const Vector3 fR = Rot3::Logmap(fRhat);
+
+  Vector r(9); r << fp, fv, fR;
+  return r;
+}
+
+//------------------------------------------------------------------------------
+PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
+    const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
+    const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensore,
+    const bool use2ndOrderCoriolis)
+{
+
+  const double& deltaTij = preintegratedMeasurements.deltaTij_;
+  const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
+  const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
+
+  const Rot3 Rot_i = pose_i.rotation();
+  const Vector3 pos_i = pose_i.translation().vector();
+
+  // Predict state at time j
+  /* ---------------------------------------------------------------------------------------------------- */
+  Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
+      + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
+      + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
+      + vel_i * deltaTij
+      - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
+      + 0.5 * gravity * deltaTij*deltaTij;
+
+  Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
+      + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
+      + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
+      - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
+      + gravity * deltaTij);
+
+  if(use2ndOrderCoriolis){
+    pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;  // 2nd order coriolis term for position
+    vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
+  }
+
+  const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
+  // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
+  Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
+  Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
+      Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
+  const Rot3 deltaRij_biascorrected_corioliscorrected =
+      Rot3::Expmap( theta_biascorrected_corioliscorrected );
+  const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
+
+  Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
+  return PoseVelocity(pose_j, vel_j);
+}
+
+} /// namespace gtsam

gtsam/navigation/ImuFactor.h

@@ -28,21 +28,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/debug.h>
 
-/* External or standard includes */
-#include <ostream>
-
-
 namespace gtsam {
-/**
- * Struct to hold return variables by the Predict Function
- */
-struct PoseVelocity {
-  Pose3 pose;
-  Vector3 velocity;
-  PoseVelocity(const Pose3& _pose, const Vector3& _velocity) :
-      pose(_pose), velocity(_velocity) {
-  }
-};
 
 /**
  *
@@ -60,19 +46,34 @@ struct PoseVelocity {
  * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
  */
 
+/**
+ * Struct to hold return variables by the Predict Function
+ */
+struct PoseVelocity {
+  Pose3 pose;
+  Vector3 velocity;
+  PoseVelocity(const Pose3& _pose, const Vector3& _velocity) :
+    pose(_pose), velocity(_velocity) {
+  }
+};
+
 class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
 public:
 
-    /** Struct to store results of preintegrating IMU measurements.  Can be build
+  /**
-     * incrementally so as to avoid costly integration at time of factor construction. */
+   * PreintegratedMeasurements accumulates (integrates) the IMU measurements
-
+   * (rotation rates and accelerations) and the corresponding covariance matrix.
-    /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
+   * The measurements are then used to build the Preintegrated IMU factor (ImuFactor class).
-         * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/
+   * Can be built incrementally so as to avoid costly integration at time of
+   * factor construction.
+   */
   class PreintegratedMeasurements {
+
     friend class ImuFactor;
+
   protected:
     imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
-      Eigen::Matrix<double,9,9> measurementCovariance_; ///< (continuous-time uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
+    Eigen::Matrix<double,9,9> measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
 
     Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
     Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
@@ -84,64 +85,33 @@ struct PoseVelocity {
     Matrix3 delVdelBiasAcc_;   ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
     Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
     Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
+
     Eigen::Matrix<double,9,9> PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
+
     bool use2ndOrderIntegration_; ///< Controls the order of integration
+
     public:
-      /** Default constructor, initialize with no IMU measurements */
-      PreintegratedMeasurements(
-          const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
-          const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
-          const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measured Angular Rate
-          const Matrix3& integrationErrorCovariance, ///< Covariance matrix of integration errors
-          const bool use2ndOrderIntegration = false ///< Controls the order of integration
-      ) : biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
-      delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
-      delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
-      delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration)
-      {
-        measurementCovariance_ << integrationErrorCovariance , Z_3x3, Z_3x3,
-                                       Z_3x3, measuredAccCovariance,  Z_3x3,
-                                       Z_3x3,   Z_3x3, measuredOmegaCovariance;
-        PreintMeasCov_.setZero(9,9);
-      }
 
-      // TODO: in what context is this constructor used and why do you init to zero?
+    /**
-      PreintegratedMeasurements() :
+     *  Default constructor, initialize the class with no measurements
-      biasHat_(imuBias::ConstantBias()), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
+     *  @param bias Current estimate of acceleration and rotation rate biases
-      delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
+     *  @param measuredAccCovariance      Covariance matrix of measuredAcc
-      delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
+     *  @param measuredOmegaCovariance    Covariance matrix of measured Angular Rate
-      delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(false)
+     *  @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position)
-      {
+     *  @param use2ndOrderIntegration     Controls the order of integration
-          measurementCovariance_.setZero(9,9);
+     *  (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
-          PreintMeasCov_.setZero(9,9);
+     */
-      }
+    PreintegratedMeasurements(const imuBias::ConstantBias& bias,
+        const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance,
+        const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration = false);
 
-      /** print */
+    /// print
-      void print(const std::string& s = "Preintegrated Measurements:") const {
+    void print(const std::string& s = "Preintegrated Measurements:") const;
-        std::cout << s << std::endl;
-        biasHat_.print("  biasHat");
-        std::cout << "  deltaTij " << deltaTij_ << std::endl;
-        std::cout << "  deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
-        std::cout << "  deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
-        deltaRij_.print("  deltaRij ");
-        std::cout << "  measurementCovariance [ " << measurementCovariance_ << " ]" << std::endl;
-        std::cout << "  PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl;
-      }
 
-      /** equals */
+    /// equals
-      bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const {
+    bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const;
-        return biasHat_.equals(expected.biasHat_, tol)
+
-            && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
+    /// methods to access class variables
-            && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
-            && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
-            && deltaRij_.equals(expected.deltaRij_, tol)
-            && std::fabs(deltaTij_ - expected.deltaTij_) < tol
-            && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
-            && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
-            && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
-            && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
-            && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
-      }
     Matrix measurementCovariance() const {return measurementCovariance_;}
     Matrix deltaRij() const {return deltaRij_.matrix();}
     double deltaTij() const{return deltaTij_;}
@@ -155,121 +125,20 @@ struct PoseVelocity {
     Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
     Matrix preintMeasCov() const { return PreintMeasCov_;}
 
+    /// Re-initialize PreintegratedMeasurements
+    void resetIntegration();
 
-      void resetIntegration(){
+    /**
-        deltaPij_ = Vector3::Zero();
+     * Add a single IMU measurement to the preintegration.
-        deltaVij_ = Vector3::Zero();
+     * @param measuredAcc Measured acceleration (in body frame)
-        deltaRij_ = Rot3();
+     * @param measuredOmega Measured angular velocity
-        deltaTij_ = 0.0;
+     * @param deltaT Time interval between two consecutive IMU measurements
-        delPdelBiasAcc_ = Z_3x3;
+     * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
-        delPdelBiasOmega_ = Z_3x3;
+     */
-        delVdelBiasAcc_ = Z_3x3;
+    void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
-        delVdelBiasOmega_ = Z_3x3;
+        boost::optional<const Pose3&> body_P_sensor = boost::none);
-        delRdelBiasOmega_ = Z_3x3;
-        PreintMeasCov_ = Matrix::Zero(9,9);
-      }
-
-      /** Add a single IMU measurement to the preintegration. */
-      void integrateMeasurement(
-          const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame)
-          const Vector3& measuredOmega, ///< Measured angular velocity (in body frame)
-          double deltaT, ///< Time step
-          boost::optional<const Pose3&> body_P_sensor = boost::none ///< Sensor frame
-      ) {
-
-        // NOTE: order is important here because each update uses old values.
-        // First we compensate the measurements for the bias
-        Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
-        Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
-
-        // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
-        if(body_P_sensor){
-          Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
-          correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
-          Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
-          correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
-          // linear acceleration vector in the body frame
-        }
-
-        const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
-        const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
-
-        const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
-
-        // Update Jacobians
-        /* ----------------------------------------------------------------------------------------------------------------------- */
-        if(!use2ndOrderIntegration_){
-          delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
-          delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
-        }else{
-          delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
-          delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
-                                    * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
-        }
-        delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
-        delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
-        delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr  * deltaT;
-
-        // Update preintegrated measurements covariance
-        /* ----------------------------------------------------------------------------------------------------------------------- */
-        const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
-        const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
-
-        Rot3 Rot_j = deltaRij_ * Rincr;
-        const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
-        const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
-
-        // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
-        // can be seen as a prediction phase in an EKF framework
-        Matrix H_pos_pos    = I_3x3;
-        Matrix H_pos_vel    = I_3x3 * deltaT;
-        Matrix H_pos_angles = Z_3x3;
-
-        Matrix H_vel_pos    = Z_3x3;
-        Matrix H_vel_vel    = I_3x3;
-        Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
-        // analytic expression corresponding to the following numerical derivative
-        // Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
-
-        Matrix H_angles_pos   = Z_3x3;
-        Matrix H_angles_vel    = Z_3x3;
-        Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
-        // analytic expression corresponding to the following numerical derivative
-        // Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
-
-        // overall Jacobian wrt preintegrated measurements (df/dx)
-        Matrix F(9,9);
-        F << H_pos_pos, H_pos_vel,  H_pos_angles,
-            H_vel_pos, H_vel_vel, H_vel_angles,
-            H_angles_pos, H_angles_vel, H_angles_angles;
-
-        // first order uncertainty propagation:
-        // the deltaT allows to pass from continuous time noise to discrete time noise
-        // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
-        // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
-        PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ;
-
-        // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
-        //
-        //        Matrix G(9,9);
-        //        G << I_3x3 * deltaT, Z_3x3,  Z_3x3,
-        //            Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
-        //            Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
-        //
-        //        PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
-
-        // Update preintegrated measurements
-        /* ----------------------------------------------------------------------------------------------------------------------- */
-        if(!use2ndOrderIntegration_){
-          deltaPij_ += deltaVij_ * deltaT;
-        }else{
-          deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
-        }
-        deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
-        deltaRij_ = deltaRij_ * Rincr;
-        deltaTij_ += deltaT;
-      }
 
+    // TODO: move to testImuFactor
     /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
     // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
     static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
@@ -340,63 +209,38 @@ struct PoseVelocity {
 #endif
 
     /** Default constructor - only use for serialization */
-    ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3), use2ndOrderCoriolis_(false){}
+    ImuFactor();
 
-    /** Constructor */
+    /**
-    ImuFactor(
+     * Constructor
-      Key pose_i, ///< previous pose key
+     * @param pose_i Previous pose key
-      Key vel_i, ///< previous velocity key
+     * @param vel_i  Previous velocity key
-      Key pose_j, ///< current pose key
+     * @param pose_j Current pose key
-      Key vel_j, ///< current velocity key
+     * @param vel_j  Current velocity key
-      Key bias, ///< previous bias key
+     * @param bias   Previous bias key
-        const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements
+     * @param preintegratedMeasurements Preintegrated IMU measurements
-        const Vector3& gravity, ///< gravity vector
+     * @param gravity Gravity vector expressed in the global frame
-        const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame
+     * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
-        boost::optional<const Pose3&> body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame
+     * @param body_P_sensor Optional pose of the sensor frame in the body frame
-        const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect
+     * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
-    ) :
+     */
-      Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias),
+    ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
-      preintegratedMeasurements_(preintegratedMeasurements),
+        const PreintegratedMeasurements& preintegratedMeasurements,
-      gravity_(gravity),
+        const Vector3& gravity, const Vector3& omegaCoriolis,
-      omegaCoriolis_(omegaCoriolis),
+        boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false);
-      body_P_sensor_(body_P_sensor),
-      use2ndOrderCoriolis_(use2ndOrderCoriolis){
-    }
 
     virtual ~ImuFactor() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    virtual gtsam::NonlinearFactor::shared_ptr clone() const;
-      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
     /** implement functions needed for Testable */
 
-    /** print */
+    /// print
-    virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
-      std::cout << s << "ImuFactor("
-          << keyFormatter(this->key1()) << ","
-          << keyFormatter(this->key2()) << ","
-          << keyFormatter(this->key3()) << ","
-          << keyFormatter(this->key4()) << ","
-          << keyFormatter(this->key5()) << ")\n";
-      preintegratedMeasurements_.print("  preintegrated measurements:");
-      std::cout << "  gravity: [ " << gravity_.transpose() << " ]" << std::endl;
-      std::cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
-      this->noiseModel_->print("  noise model: ");
-      if(this->body_P_sensor_)
-        this->body_P_sensor_->print("  sensor pose in body frame: ");
-    }
 
-    /** equals */
+    /// equals
-    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const;
-      const This *e =  dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol)
-          && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
-          && equal_with_abs_tol(gravity_, e->gravity_, tol)
-          && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
-          && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
-    }
 
     /** Access the preintegrated measurements. */
     const PreintegratedMeasurements& preintegratedMeasurements() const {
@@ -408,205 +252,20 @@ struct PoseVelocity {
 
     /** implement functions needed to derive from Factor */
 
-    /** vector of errors */
+    /// 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,
         boost::optional<Matrix&> H1 = boost::none,
         boost::optional<Matrix&> H2 = boost::none,
         boost::optional<Matrix&> H3 = boost::none,
         boost::optional<Matrix&> H4 = boost::none,
-        boost::optional<Matrix&> H5 = boost::none) const
+        boost::optional<Matrix&> H5 = boost::none) const;
-    {
 
-      const double& deltaTij = preintegratedMeasurements_.deltaTij_;
+    // predicted states from IMU
-      const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
-      const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
-
-      // we give some shorter name to rotations and translations
-      const Rot3 Rot_i = pose_i.rotation();
-      const Rot3 Rot_j = pose_j.rotation();
-      const Vector3 pos_i = pose_i.translation().vector();
-      const Vector3 pos_j = pose_j.translation().vector();
-
-      // We compute factor's Jacobians
-      /* ---------------------------------------------------------------------------------------------------- */
-      const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
-      // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-
-      Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-
-      Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-          Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
-
-      const Rot3 deltaRij_biascorrected_corioliscorrected =
-          Rot3::Expmap( theta_biascorrected_corioliscorrected );
-
-      const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
-
-      const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
-
-      const Matrix3 Jtheta = -Jr_theta_bcc  * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
-
-      const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
-
-      if(H1) {
-        H1->resize(9,6);
-
-        Matrix3 dfPdPi;
-        Matrix3 dfVdPi;
-        if(use2ndOrderCoriolis_){
-          dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
-          dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
-        }
-        else{
-          dfPdPi = - Rot_i.matrix();
-          dfVdPi = Z_3x3;
-        }
-
-    (*H1) <<
-      // dfP/dRi
-      Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
-        + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
-      // dfP/dPi
-      dfPdPi,
-      // dfV/dRi
-      Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
-        + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
-      // dfV/dPi
-      dfVdPi,
-      // dfR/dRi
-      Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
-      // dfR/dPi
-      Z_3x3;
-      }
-
-      if(H2) {
-        H2->resize(9,3);
-        (*H2) <<
-            // dfP/dVi
-            - I_3x3 * deltaTij
-            + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij,  // Coriolis term - we got rid of the 2 wrt ins paper
-            // dfV/dVi
-            - I_3x3
-            + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
-            // dfR/dVi
-            Z_3x3;
-      }
-
-      if(H3) {
-
-        H3->resize(9,6);
-        (*H3) <<
-            // dfP/dPosej
-            Z_3x3, Rot_j.matrix(),
-            // dfV/dPosej
-            Matrix::Zero(3,6),
-            // dfR/dPosej
-            Jrinv_fRhat *  ( I_3x3 ), Z_3x3;
-      }
-
-      if(H4) {
-        H4->resize(9,3);
-        (*H4) <<
-            // dfP/dVj
-            Z_3x3,
-            // dfV/dVj
-            I_3x3,
-            // dfR/dVj
-            Z_3x3;
-      }
-
-      if(H5) {
-
-        const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
-        const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
-        const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
-
-        H5->resize(9,6);
-        (*H5) <<
-            // dfP/dBias
-            - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
-            - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
-            // dfV/dBias
-            - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
-            - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
-            // dfR/dBias
-            Matrix::Zero(3,3),
-            Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
-      }
-
-      // Evaluate residual error, according to [3]
-      /* ---------------------------------------------------------------------------------------------------- */
-      const Vector3 fp =
-          pos_j - pos_i
-          - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
-              + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
-              + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
-              - vel_i * deltaTij
-              + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-              - 0.5 * gravity_ * deltaTij*deltaTij;
-
-      const Vector3 fv =
-          vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
-              + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
-              + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
-              + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij  // Coriolis term
-              - gravity_ * deltaTij;
-
-      const Vector3 fR = Rot3::Logmap(fRhat);
-
-      Vector r(9); r << fp, fv, fR;
-      return r;
-    }
-
-
-    /** predicted states from IMU */
     static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i,
         const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
         const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
-        const bool use2ndOrderCoriolis = false)
+        const bool use2ndOrderCoriolis = false);
-    {
-
-      const double& deltaTij = preintegratedMeasurements.deltaTij_;
-      const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
-      const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
-
-      const Rot3 Rot_i = pose_i.rotation();
-      const Vector3 pos_i = pose_i.translation().vector();
-
-      // Predict state at time j
-      /* ---------------------------------------------------------------------------------------------------- */
-    Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
-      + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
-      + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
-      + vel_i * deltaTij
-      - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-      + 0.5 * gravity * deltaTij*deltaTij;
-
-    Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
-      + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
-      + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
-      - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
-      + gravity * deltaTij);
-
-      if(use2ndOrderCoriolis){
-        pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;  // 2nd order coriolis term for position
-        vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
-      }
-
-      const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
-      // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-      Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-      Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-          Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
-      const Rot3 deltaRij_biascorrected_corioliscorrected =
-          Rot3::Expmap( theta_biascorrected_corioliscorrected );
-      const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
-
-      Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
-      return PoseVelocity(pose_j, vel_j);
-    }
-
 
   private:
 
@@ -621,7 +280,7 @@ struct PoseVelocity {
       ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
       ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
     }
-  }; // \class ImuFactor
+  }; // class ImuFactor
 
   typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements;