Add ifdefs that restore the old IMU factor functionality

gtsam/navigation/CombinedImuFactor.cpp

@@ -70,7 +70,13 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
   // Update preintegrated measurements.
   Matrix9 A;  // overall Jacobian wrt preintegrated measurements (df/dx)
   Matrix93 B, C;
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
   PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
+#else
+  Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
+  PreintegrationBase::update(measuredAcc, measuredOmega, dt,
+      &D_incrR_integratedOmega, &A, &B, &C);
+#endif
 
   // Update preintegrated measurements covariance: as in [2] we consider a first
   // order propagation that can be seen as a prediction phase in an EKF
@@ -79,7 +85,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
   // and preintegrated measurements
 
   // Single Jacobians to propagate covariance
-  // TODO(frank): should we not also accout for bias on position?
+  // 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>();
 

gtsam/navigation/ImuFactor.cpp

@@ -55,7 +55,13 @@ void PreintegratedImuMeasurements::integrateMeasurement(
   // Update preintegrated measurements (also get Jacobian)
   Matrix9 A;  // overall Jacobian wrt preintegrated measurements (df/dx)
   Matrix93 B, C;
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
   PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
+#else
+  Matrix3 D_incrR_integratedOmega;
+  PreintegrationBase::update(measuredAcc, measuredOmega, dt,
+      &D_incrR_integratedOmega, &A, &B, &C);
+#endif
 
   // first order covariance propagation:
   // as in [2] we consider a first order propagation that can be seen as a
@@ -91,13 +97,14 @@ void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredA
 }
 
 //------------------------------------------------------------------------------
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
 void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12,  //
                                              Matrix9* H1, Matrix9* H2) {
   PreintegrationBase::mergeWith(pim12, H1, H2);
   preintMeasCov_ =
       *H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose();
 }
-
+#endif
 //------------------------------------------------------------------------------
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 PreintegratedImuMeasurements::PreintegratedImuMeasurements(
@@ -174,6 +181,7 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
 }
 
 //------------------------------------------------------------------------------
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
 PreintegratedImuMeasurements ImuFactor::Merge(
     const PreintegratedImuMeasurements& pim01,
     const PreintegratedImuMeasurements& pim12) {
@@ -216,6 +224,7 @@ ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01,
                                        f01->key5(),  // B
                                        pim02);
 }
+#endif
 
 //------------------------------------------------------------------------------
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4

gtsam/navigation/ImuFactor.h

@@ -124,8 +124,10 @@ public:
   /// Return pre-integrated measurement covariance
   Matrix preintMeasCov() const { return preintMeasCov_; }
 
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
   /// Merge in a different set of measurements and update bias derivatives accordingly
   void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2);
+#endif
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
   /// @deprecated constructor
@@ -230,6 +232,7 @@ public:
       boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
           boost::none, boost::optional<Matrix&> H5 = boost::none) const;
 
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
   /// Merge two pre-integrated measurement classes
   static PreintegratedImuMeasurements Merge(
       const PreintegratedImuMeasurements& pim01,
@@ -237,6 +240,7 @@ public:
 
   /// Merge two factors
   static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
+#endif
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
   /// @deprecated typename

gtsam/navigation/NavState.cpp

@@ -239,7 +239,7 @@ Matrix7 NavState::wedge(const Vector9& xi) {
 #define D_v_v(H) (H)->block<3,3>(6,6)
 
 //------------------------------------------------------------------------------
-#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_IMU_MANIFOLD_INTEGRATION)
 NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
     const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
     OptionalJacobian<9, 3> G2) const {

gtsam/navigation/NavState.h

@@ -203,7 +203,7 @@ public:
   /// @name Dynamics
   /// @{
 
-#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V4) || !defined(GTSAM_IMU_MANIFOLD_INTEGRATION)
   /// Integrate forward in time given angular velocity and acceleration in body frame
   /// Uses second order integration for position, returns derivatives except dt.
   NavState update(const Vector3& b_acceleration, const Vector3& b_omega,

gtsam/navigation/PreintegrationBase.cpp

@@ -37,15 +37,28 @@ PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p,
 //------------------------------------------------------------------------------
 void PreintegrationBase::resetIntegration() {
   deltaTij_ = 0.0;
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
   preintegrated_.setZero();
   preintegrated_H_biasAcc_.setZero();
   preintegrated_H_biasOmega_.setZero();
+#else
+  deltaXij_ = NavState();
+  delRdelBiasOmega_.setZero();
+  delPdelBiasAcc_.setZero();
+  delPdelBiasOmega_.setZero();
+  delVdelBiasAcc_.setZero();
+  delVdelBiasOmega_.setZero();
+#endif
 }
 
 //------------------------------------------------------------------------------
 ostream& operator<<(ostream& os, const PreintegrationBase& pim) {
   os << "    deltaTij " << pim.deltaTij_ << endl;
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
   os << "    deltaRij " << Point3(pim.theta()) << endl;
+#else
+  os << "    deltaRij.ypr = (" << pim.deltaRij().ypr().transpose() << ")" << endl;
+#endif
   os << "    deltaPij " << Point3(pim.deltaPij()) << endl;
   os << "    deltaVij " << Point3(pim.deltaVij()) << endl;
   os << "    gyrobias " << Point3(pim.biasHat_.gyroscope()) << endl;
@@ -64,9 +77,18 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
   return p_->equals(*other.p_, tol)
       && fabs(deltaTij_ - other.deltaTij_) < tol
       && biasHat_.equals(other.biasHat_, tol)
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
       && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol)
       && equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol)
       && equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol);
+#else
+      && deltaXij_.equals(other.deltaXij_, tol)
+      && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
+      && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
+      && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
+      && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
+      && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
+#endif
 }
 
 //------------------------------------------------------------------------------
@@ -105,7 +127,8 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
     // Update derivative: centrifugal causes the correlation between acc and omega!!!
     if (correctedAcc_H_unbiasedOmega) {
       double wdp = correctedOmega.dot(b_arm);
-      *correctedAcc_H_unbiasedOmega = -( (Matrix) Vector3::Constant(wdp).asDiagonal()
+      const Matrix3 diag_wdp = Vector3::Constant(wdp).asDiagonal();
+      *correctedAcc_H_unbiasedOmega = -( diag_wdp
           + correctedOmega * b_arm.transpose()) * bRs.matrix()
           + 2 * b_arm * unbiasedOmega.transpose();
     }
@@ -114,6 +137,7 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
   return make_pair(correctedAcc, correctedOmega);
 }
 
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
 //------------------------------------------------------------------------------
 // See extensive discussion in ImuFactor.lyx
 Vector9 PreintegrationBase::UpdatePreintegrated(
@@ -224,29 +248,150 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
   }
   return biasCorrected;
 }
+#else
+
+//------------------------------------------------------------------------------
+pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
+    const Vector3& measuredAcc, const Vector3& measuredOmega,
+    OptionalJacobian<3, 3> D_correctedAcc_measuredAcc,
+    OptionalJacobian<3, 3> D_correctedAcc_measuredOmega,
+    OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const {
+
+  // Correct for bias in the sensor frame
+  Vector3 unbiasedAcc = biasHat_.correctAccelerometer(measuredAcc);
+  Vector3 unbiasedOmega = biasHat_.correctGyroscope(measuredOmega);
+
+  return correctMeasurementsBySensorPose(unbiasedAcc, unbiasedOmega,
+      D_correctedAcc_measuredAcc, D_correctedAcc_measuredOmega,
+      D_correctedOmega_measuredOmega);
+}
+
+//------------------------------------------------------------------------------
+NavState PreintegrationBase::updatedDeltaXij(const Vector3& measuredAcc,
+    const Vector3& measuredOmega, const double dt,
+    OptionalJacobian<9, 9> D_updated_current,
+    OptionalJacobian<9, 3> D_updated_measuredAcc,
+    OptionalJacobian<9, 3> D_updated_measuredOmega) const {
+
+  Vector3 correctedAcc, correctedOmega;
+  Matrix3 D_correctedAcc_measuredAcc, //
+      D_correctedAcc_measuredOmega, //
+      D_correctedOmega_measuredOmega;
+  bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor;
+  boost::tie(correctedAcc, correctedOmega) =
+      correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
+          (needDerivs ? &D_correctedAcc_measuredAcc : 0),
+          (needDerivs ? &D_correctedAcc_measuredOmega : 0),
+          (needDerivs ? &D_correctedOmega_measuredOmega : 0));
+  // Do update in one fell swoop
+  Matrix93 D_updated_correctedAcc, D_updated_correctedOmega;
+  NavState updated = deltaXij_.update(correctedAcc, correctedOmega, dt, D_updated_current,
+              (needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc),
+              (needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega));
+  if (needDerivs) {
+    *D_updated_measuredAcc = D_updated_correctedAcc * D_correctedAcc_measuredAcc;
+    *D_updated_measuredOmega = D_updated_correctedOmega * D_correctedOmega_measuredOmega;
+    if (!p().body_P_sensor->translation().vector().isZero()) {
+      *D_updated_measuredOmega += D_updated_correctedAcc * D_correctedAcc_measuredOmega;
+    }
+  }
+  return updated;
+}
+
+//------------------------------------------------------------------------------
+void PreintegrationBase::update(const Vector3& measuredAcc,
+    const Vector3& measuredOmega, const double dt,
+    Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
+    Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) {
+
+  // Save current rotation for updating Jacobians
+  const Rot3 oldRij = deltaXij_.attitude();
+
+  // Do update
+  deltaTij_ += dt;
+  deltaXij_ = updatedDeltaXij(measuredAcc, measuredOmega, dt,
+      D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional
+
+  // Update Jacobians
+  // TODO(frank): we are repeating some computation here: accessible in F ?
+  Vector3 correctedAcc, correctedOmega;
+  boost::tie(correctedAcc, correctedOmega) =
+      correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega);
+
+  Matrix3 D_acc_R;
+  oldRij.rotate(correctedAcc, D_acc_R);
+  const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
+
+  const Vector3 integratedOmega = correctedOmega * dt;
+  const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
+  const Matrix3 incrRt = incrR.transpose();
+  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
+      - *D_incrR_integratedOmega * dt;
+
+  double dt22 = 0.5 * dt * dt;
+  const Matrix3 dRij = oldRij.matrix(); // expensive
+  delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
+  delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
+  delVdelBiasAcc_ += -dRij * dt;
+  delVdelBiasOmega_ += D_acc_biasOmega * dt;
+}
+
+//------------------------------------------------------------------------------
+Vector9 PreintegrationBase::biasCorrectedDelta(
+    const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
+  // Correct deltaRij, derivative is delRdelBiasOmega_
+  const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
+  Matrix3 D_correctedRij_bias;
+  const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
+  const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none,
+      H ? &D_correctedRij_bias : 0);
+  if (H)
+    D_correctedRij_bias *= delRdelBiasOmega_;
+
+  Vector9 xi;
+  Matrix3 D_dR_correctedRij;
+  // TODO(frank): could line below be simplified? It is equivalent to
+  //   LogMap(deltaRij_.compose(Expmap(biasInducedOmega)))
+  NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0);
+  NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer()
+      + delPdelBiasOmega_ * biasIncr.gyroscope();
+  NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer()
+      + delVdelBiasOmega_ * biasIncr.gyroscope();
+
+  if (H) {
+    Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
+    D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias;
+    D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_;
+    D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
+    (*H) << D_dR_bias, D_dP_bias, D_dV_bias;
+  }
+  return xi;
+}
+
+#endif
 
 //------------------------------------------------------------------------------
 NavState PreintegrationBase::predict(const NavState& state_i,
-                                     const imuBias::ConstantBias& bias_i,
+    const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
-                                     OptionalJacobian<9, 9> H1,
     OptionalJacobian<9, 6> H2) const {
   // TODO(frank): make sure this stuff is still correct
   Matrix96 D_biasCorrected_bias;
-  Vector9 biasCorrected =
+  Vector9 biasCorrected = biasCorrectedDelta(bias_i,
-      biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : 0);
+      H2 ? &D_biasCorrected_bias : 0);
 
   // Correct for initial velocity and gravity
   Matrix9 D_delta_state, D_delta_biasCorrected;
   Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
-                                  p().omegaCoriolis, p().use2ndOrderCoriolis,
+      p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
-                                  H1 ? &D_delta_state : 0,
       H2 ? &D_delta_biasCorrected : 0);
 
   // Use retract to get back to NavState manifold
   Matrix9 D_predict_state, D_predict_delta;
   NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
-  if (H1) *H1 = D_predict_state + D_predict_delta* D_delta_state;
+  if (H1)
-  if (H2) *H2 = D_predict_delta* D_delta_biasCorrected* D_biasCorrected_bias;
+    *H1 = D_predict_state + D_predict_delta * D_delta_state;
+  if (H2)
+    *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias;
   return state_j;
 }
 
@@ -306,6 +451,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
   return error;
 }
 
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
 //------------------------------------------------------------------------------
 // sugar for derivative blocks
 #define D_R_R(H) (H)->block<3,3>(0,0)
@@ -393,6 +539,7 @@ void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1,
   preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ +
                                (*H2) * pim12.preintegrated_H_biasOmega_;
 }
+#endif
 
 //------------------------------------------------------------------------------
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
@@ -408,6 +555,7 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
   p_ = q;
   return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
 }
+
 #endif
 //------------------------------------------------------------------------------
 

gtsam/navigation/PreintegrationBase.h

@@ -81,10 +81,18 @@ class PreintegrationBase {
    * Note: relative position does not take into account velocity at time i, see deltap+, in [2]
    * Note: velocity is now also in frame i, as opposed to deltaVij in [2]
    */
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
   Vector9 preintegrated_;
-
   Matrix93 preintegrated_H_biasAcc_;    ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias
   Matrix93 preintegrated_H_biasOmega_;  ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias
+#else
+  NavState deltaXij_;
+  Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
+  Matrix3 delPdelBiasAcc_;   ///< Jacobian of preintegrated position w.r.t. acceleration bias
+  Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
+  Matrix3 delVdelBiasAcc_;   ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
+  Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
+#endif
 
   /// Default constructor for serialization
   PreintegrationBase() {
@@ -140,17 +148,22 @@ public:
   const imuBias::ConstantBias& biasHat() const { return biasHat_; }
   double deltaTij() const { return deltaTij_; }
 
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
   const Vector9& preintegrated() const { return preintegrated_; }
-
   Vector3 theta() const     { return preintegrated_.head<3>(); }
   Vector3 deltaPij() const  { return preintegrated_.segment<3>(3); }
   Vector3 deltaVij() const  { return preintegrated_.tail<3>(); }
-
   Rot3 deltaRij() const     { return Rot3::Expmap(theta()); }
   NavState deltaXij() const { return NavState::Retract(preintegrated_); }
 
   const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; }
   const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; }
+#else
+  const NavState& deltaXij() const { return deltaXij_; }
+  const Rot3& deltaRij() const { return deltaXij_.attitude(); }
+  Vector3 deltaPij() const     { return deltaXij_.position().vector(); }
+  Vector3 deltaVij() const     { return deltaXij_.velocity(); }
+#endif
 
   // Exposed for MATLAB
   Vector6 biasHatVector() const { return biasHat_.vector(); }
@@ -175,6 +188,8 @@ public:
       OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none,
       OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const;
 
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
+
   // Update integrated vector on tangent manifold preintegrated with acceleration
   // Static, functional version.
   static Vector9 UpdatePreintegrated(const Vector3& a_body,
@@ -200,6 +215,38 @@ public:
   Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
       OptionalJacobian<9, 6> H = boost::none) const;
 
+#else
+
+  /// Subtract estimate and correct for sensor pose
+  /// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc)
+  /// Ignore D_correctedOmega_measuredAcc as it is trivially zero
+  std::pair<Vector3, Vector3> correctMeasurementsByBiasAndSensorPose(
+      const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
+      OptionalJacobian<3, 3> D_correctedAcc_measuredAcc = boost::none,
+      OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none,
+      OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const;
+
+  /// Calculate the updated preintegrated measurement, does not modify
+  /// It takes measured quantities in the j frame
+  NavState updatedDeltaXij(const Vector3& j_measuredAcc,
+      const Vector3& j_measuredOmega, const double dt,
+      OptionalJacobian<9, 9> D_updated_current = boost::none,
+      OptionalJacobian<9, 3> D_updated_measuredAcc = boost::none,
+      OptionalJacobian<9, 3> D_updated_measuredOmega = boost::none) const;
+
+  /// Update preintegrated measurements and get derivatives
+  /// It takes measured quantities in the j frame
+  void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
+      const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
+      Matrix93* D_udpated_measuredAcc, Matrix93* D_updated_measuredOmega);
+
+  /// Given the estimate of the bias, return a NavState tangent vector
+  /// summarizing the preintegrated IMU measurements so far
+  Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
+      OptionalJacobian<9, 6> H = boost::none) const;
+
+#endif
+
   /// Predict state at time j
   NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
                    OptionalJacobian<9, 9> H1 = boost::none,
@@ -219,6 +266,7 @@ public:
       OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
           boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
 
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
   // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives
   static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
                          double deltaT12,
@@ -229,13 +277,13 @@ public:
   /// The derivatives apply to the preintegrated Vector9
   void mergeWith(const PreintegrationBase& pim, Matrix9* H1, Matrix9* H2);
   /// @}
+#endif
 
   /** Dummy clone for MATLAB */
   virtual boost::shared_ptr<PreintegrationBase> clone() const {
     return boost::shared_ptr<PreintegrationBase>();
   }
 
-
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
   /// @name Deprecated
   /// @{
@@ -257,11 +305,22 @@ private:
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     namespace bs = ::boost::serialization;
     ar & BOOST_SERIALIZATION_NVP(p_);
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
     ar & BOOST_SERIALIZATION_NVP(biasHat_);
     ar & BOOST_SERIALIZATION_NVP(deltaTij_);
     ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size()));
     ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size()));
     ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size()));
+#else
+    ar & BOOST_SERIALIZATION_NVP(deltaTij_);
+    ar & BOOST_SERIALIZATION_NVP(deltaXij_);
+    ar & BOOST_SERIALIZATION_NVP(biasHat_);
+    ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size()));
+    ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size()));
+    ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size()));
+    ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size()));
+    ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size()));
+#endif
   }
 };
 

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -132,6 +132,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) {
 }
 
 /* ************************************************************************* */
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
 TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
   auto p = testing::Params();
   testing::SomeMeasurements measurements;
@@ -151,6 +152,7 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
   EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1),
                       pim.preintegrated_H_biasOmega(), 1e-3));
 }
+#endif
 
 /* ************************************************************************* */
 TEST(CombinedImuFactor, PredictPositionAndVelocity) {

gtsam/navigation/tests/testImuFactor.cpp

@@ -94,13 +94,13 @@ TEST(ImuFactor, PreintegratedMeasurements) {
 
   // Actual pre-integrated values
   PreintegratedImuMeasurements actual(testing::Params());
-  EXPECT(assert_equal(kZero, actual.theta()));
+  EXPECT(assert_equal(Rot3(), actual.deltaRij()));
   EXPECT(assert_equal(kZero, actual.deltaPij()));
   EXPECT(assert_equal(kZero, actual.deltaVij()));
   DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9);
 
   actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
-  EXPECT(assert_equal(expectedDeltaR1, actual.theta()));
+  EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR1), actual.deltaRij()));
   EXPECT(assert_equal(expectedDeltaP1, actual.deltaPij()));
   EXPECT(assert_equal(expectedDeltaV1, actual.deltaVij()));
   DOUBLES_EQUAL(0.5, actual.deltaTij(), 1e-9);
@@ -129,7 +129,7 @@ TEST(ImuFactor, PreintegratedMeasurements) {
 
   // Actual pre-integrated values
   actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
-  EXPECT(assert_equal(expectedDeltaR2, actual.theta()));
+  EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR2), actual.deltaRij()));
   EXPECT(assert_equal(expectedDeltaP2, actual.deltaPij()));
   EXPECT(assert_equal(expectedDeltaV2, actual.deltaVij()));
   DOUBLES_EQUAL(1.0, actual.deltaTij(), 1e-9);
@@ -439,6 +439,7 @@ TEST(ImuFactor, fistOrderExponential) {
 }
 
 /* ************************************************************************* */
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
 TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
   testing::SomeMeasurements measurements;
 
@@ -458,7 +459,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
   EXPECT(assert_equal(numericalDerivative22(preintegrated, kZero, kZero),
                       pim.preintegrated_H_biasOmega(), 1e-3));
 }
-
+#endif
 /* ************************************************************************* */
 Vector3 correctedAcc(const PreintegratedImuMeasurements& pim,
     const Vector3& measuredAcc, const Vector3& measuredOmega) {
@@ -789,6 +790,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
 }
 
 /* ************************************************************************* */
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
 static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6;
 
 struct ImuFactorMergeTest {
@@ -883,6 +885,7 @@ TEST(ImuFactor, MergeWithCoriolis) {
   mergeTest.p_->omegaCoriolis = Vector3(0.1, 0.2, -0.1);
   mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4);
 }
+#endif
 
 /* ************************************************************************* */
 // Same values as pre-integration test but now testing covariance

gtsam/navigation/tests/testPreintegrationBase.cpp

@@ -28,6 +28,7 @@
 
 static const double kDt = 0.1;
 
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
 Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) {
   return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta);
 }
@@ -140,6 +141,7 @@ TEST(PreintegrationBase, MergedBiasDerivatives) {
   EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(f, Z_3x1, Z_3x1),
                       expected_pim02.preintegrated_H_biasOmega(), 1e-7));
 }
+#endif
 
 /* ************************************************************************* */
 int main() {