No more second-order integration flag

gtsam/navigation/CombinedImuFactor.cpp

@@ -145,15 +145,16 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
     const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
     const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit,
     const bool use2ndOrderIntegration) {
+  if (!use2ndOrderIntegration)
+    throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
   biasHat_ = biasHat;
-  boost::shared_ptr<Params> p = boost::make_shared<Params>();
+  boost::shared_ptr<Params> p = Params::MakeSharedFRD();
   p->gyroscopeCovariance = measuredOmegaCovariance;
   p->accelerometerCovariance = measuredAccCovariance;
   p->integrationCovariance = integrationErrorCovariance;
   p->biasAccCovariance = biasAccCovariance;
   p->biasOmegaCovariance = biasOmegaCovariance;
   p->biasAccOmegaInit = biasAccOmegaInit;
-  p->use2ndOrderIntegration = use2ndOrderIntegration;
   p_ = p;
   resetIntegration();
   preintMeasCov_.setZero();
@@ -259,7 +260,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
 //------------------------------------------------------------------------------
 CombinedImuFactor::CombinedImuFactor(
     Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
-    const CombinedPreintegratedMeasurements& pim, const Vector3& gravity,
+    const CombinedPreintegratedMeasurements& pim, const Vector3& b_gravity,
     const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
     const bool use2ndOrderCoriolis)
     : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
@@ -267,7 +268,7 @@ CombinedImuFactor::CombinedImuFactor(
       _PIM_(pim) {
   boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
       boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
-  p->gravity = gravity;
+  p->b_gravity = b_gravity;
   p->omegaCoriolis = omegaCoriolis;
   p->body_P_sensor = body_P_sensor;
   p->use2ndOrderCoriolis = use2ndOrderCoriolis;
@@ -278,11 +279,11 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
                                 Pose3& pose_j, Vector3& vel_j,
                                 const imuBias::ConstantBias& bias_i,
                                 CombinedPreintegratedMeasurements& pim,
-                                const Vector3& gravity,
+                                const Vector3& b_gravity,
                                 const Vector3& omegaCoriolis,
                                 const bool use2ndOrderCoriolis) {
   // use deprecated predict
-  PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, gravity,
+  PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, b_gravity,
       omegaCoriolis, use2ndOrderCoriolis);
   pose_j = pvb.pose;
   vel_j = pvb.velocity;

gtsam/navigation/CombinedImuFactor.h

@@ -66,9 +66,26 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase {
     Matrix3 biasOmegaCovariance;  ///< continuous-time "Covariance" describing gyroscope bias random walk
     Matrix6 biasAccOmegaInit;     ///< covariance of bias used for pre-integration
 
-    Params():biasAccCovariance(I_3x3), biasOmegaCovariance(I_3x3), biasAccOmegaInit(I_6x6) {}
+    /// See two named constructors below for good values of b_gravity in body frame
+    Params(const Vector3& b_gravity) :
+        PreintegrationBase::Params(b_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
+            I_3x3), biasAccOmegaInit(I_6x6) {
+    }
+
+    // Default Params for Front-Right-Down convention: b_gravity points along positive Z-axis
+    static boost::shared_ptr<Params> MakeSharedFRD(double g = 9.81) {
+      return boost::make_shared<Params>(Vector3(0, 0, g));
+    }
+
+    // Default Params for Front-Left-Up convention: b_gravity points along negative Z-axis
+    static boost::shared_ptr<Params> MakeSharedFLU(double g = 9.81) {
+      return boost::make_shared<Params>(Vector3(0, 0, -g));
+    }
 
    private:
+    /// Default constructor makes unitialized params struct
+    Params() {}
+
     /** Serialization function */
     friend class boost::serialization::access;
     template <class ARCHIVE>
@@ -134,12 +151,13 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase {
   Matrix preintMeasCov() const { return preintMeasCov_; }
 
   /// deprecated constructor
+  /// NOTE(frank): assumes FRD convention, only second order integration supported
   PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat,
       const Matrix3& measuredAccCovariance,
       const Matrix3& measuredOmegaCovariance,
       const Matrix3& integrationErrorCovariance,
       const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
-      const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = false);
+      const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true);
 
  private:
   /// Serialization function
@@ -245,7 +263,7 @@ public:
   /// @deprecated constructor
   CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i,
                     Key bias_j, const CombinedPreintegratedMeasurements& pim,
-                    const Vector3& gravity, const Vector3& omegaCoriolis,
+                    const Vector3& b_gravity, const Vector3& omegaCoriolis,
                     const boost::optional<Pose3>& body_P_sensor = boost::none,
                     const bool use2ndOrderCoriolis = false);
 
@@ -253,7 +271,7 @@ public:
   static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
                       Vector3& vel_j, const imuBias::ConstantBias& bias_i,
                       CombinedPreintegratedMeasurements& pim,
-                      const Vector3& gravity, const Vector3& omegaCoriolis,
+                      const Vector3& b_gravity, const Vector3& omegaCoriolis,
                       const bool use2ndOrderCoriolis = false);
 
 private:

gtsam/navigation/ImuFactor.cpp

@@ -38,8 +38,8 @@ void PreintegratedImuMeasurements::print(const string& s) const {
 //------------------------------------------------------------------------------
 bool PreintegratedImuMeasurements::equals(
     const PreintegratedImuMeasurements& other, double tol) const {
-  return PreintegrationBase::equals(other, tol) &&
+  return PreintegrationBase::equals(other, tol)
-         equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
+      && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
 }
 
 //------------------------------------------------------------------------------
@@ -51,8 +51,7 @@ void PreintegratedImuMeasurements::resetIntegration() {
 //------------------------------------------------------------------------------
 void PreintegratedImuMeasurements::integrateMeasurement(
     const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
-    OptionalJacobian<9, 9> F_test,
+    OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) {
-    OptionalJacobian<9, 9> G_test) {
 
   Vector3 correctedAcc, correctedOmega;
   correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
@@ -65,7 +64,8 @@ void PreintegratedImuMeasurements::integrateMeasurement(
   const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega);
 
   // Update Jacobians
-  updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
+  updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr,
+      deltaT);
 
   // Update preintegrated measurements (also get Jacobian)
   const Matrix3 dRij = deltaRij_.matrix(); // store this, which is useful to compute G_test
@@ -88,14 +88,13 @@ void PreintegratedImuMeasurements::integrateMeasurement(
       * p().gyroscopeCovariance * D_Rincr_integratedOmega.transpose() * deltaT;
 
   Matrix3 F_pos_noiseacc;
-  if (p().use2ndOrderIntegration) {
   F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT;
   preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc
       * p().accelerometerCovariance * F_pos_noiseacc.transpose();
-    Matrix3 temp = F_pos_noiseacc * p().accelerometerCovariance * dRij.transpose();  // has 1/deltaT
+  Matrix3 temp = F_pos_noiseacc * p().accelerometerCovariance
+      * dRij.transpose(); // has 1/deltaT
   preintMeasCov_.block<3, 3>(0, 3) += temp;
   preintMeasCov_.block<3, 3>(3, 0) += temp.transpose();
-  }
 
   // F_test and G_test are given as output for testing purposes and are not needed by the factor
   if (F_test) {
@@ -103,9 +102,6 @@ void PreintegratedImuMeasurements::integrateMeasurement(
   }
   if (G_test) {
     // This in only for testing & documentation, while the actual computation is done block-wise
-    if (!p().use2ndOrderIntegration)
-      F_pos_noiseacc = Z_3x3;
-
     //           intNoise          accNoise           omegaNoise
     (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos
     Z_3x3, dRij * deltaT, Z_3x3, // vel
@@ -117,12 +113,13 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements(
     const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
     const Matrix3& measuredOmegaCovariance,
     const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) {
+  if (!use2ndOrderIntegration)
+    throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
   biasHat_ = biasHat;
-  boost::shared_ptr<Params> p = boost::make_shared<Params>();
+  boost::shared_ptr<Params> p = Params::MakeSharedFRD();
   p->gyroscopeCovariance = measuredOmegaCovariance;
   p->accelerometerCovariance = measuredAccCovariance;
   p->integrationCovariance = integrationErrorCovariance;
-  p->use2ndOrderIntegration = use2ndOrderIntegration;
   p_ = p;
   resetIntegration();
 }
@@ -131,10 +128,10 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements(
 // ImuFactor methods
 //------------------------------------------------------------------------------
 ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
-                     const PreintegratedImuMeasurements& pim)
+    const PreintegratedImuMeasurements& pim) :
-    : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
+    Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
-           pose_j, vel_j, bias),
+        pose_j, vel_j, bias), _PIM_(pim) {
-      _PIM_(pim) {}
+}
 
 //------------------------------------------------------------------------------
 gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
@@ -151,7 +148,8 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
   Base::print("");
   _PIM_.print("  preintegrated measurements:");
   // Print standard deviations on covariance only
-  cout << "  noise model sigmas: " << this->noiseModel_->sigmas().transpose() << endl;
+  cout << "  noise model sigmas: " << this->noiseModel_->sigmas().transpose()
+      << endl;
 }
 
 //------------------------------------------------------------------------------
@@ -173,16 +171,14 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
 
 //------------------------------------------------------------------------------
 ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
-                     const PreintegratedMeasurements& pim,
+    const PreintegratedMeasurements& pim, const Vector3& b_gravity,
-                     const Vector3& gravity, const Vector3& omegaCoriolis,
+    const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
-                     const boost::optional<Pose3>& body_P_sensor,
+    const bool use2ndOrderCoriolis) :
-                     const bool use2ndOrderCoriolis)
+    Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
-    : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
+        pose_j, vel_j, bias), _PIM_(pim) {
-           pose_j, vel_j, bias),
+  boost::shared_ptr<PreintegratedMeasurements::Params> p = boost::make_shared<
-      _PIM_(pim) {
+      PreintegratedMeasurements::Params>(pim.p());
-  boost::shared_ptr<PreintegratedMeasurements::Params> p =
+  p->b_gravity = b_gravity;
-      boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
-  p->gravity = gravity;
   p->omegaCoriolis = omegaCoriolis;
   p->body_P_sensor = body_P_sensor;
   p->use2ndOrderCoriolis = use2ndOrderCoriolis;
@@ -191,13 +187,11 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
 
 //------------------------------------------------------------------------------
 void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
-                        Pose3& pose_j, Vector3& vel_j,
+    Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i,
-                        const imuBias::ConstantBias& bias_i,
+    PreintegratedMeasurements& pim, const Vector3& b_gravity,
-                        PreintegratedMeasurements& pim,
+    const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) {
-                        const Vector3& gravity, const Vector3& omegaCoriolis,
-                        const bool use2ndOrderCoriolis) {
   // use deprecated predict
-  PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, gravity,
+  PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, b_gravity,
       omegaCoriolis, use2ndOrderCoriolis);
   pose_j = pvb.pose;
   vel_j = pvb.velocity;

gtsam/navigation/ImuFactor.h

@@ -108,11 +108,12 @@ public:
   Matrix preintMeasCov() const { return preintMeasCov_; }
 
   /// @deprecated constructor
+  /// NOTE(frank): assumes FRD convention, only second order integration supported
   PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat,
       const Matrix3& measuredAccCovariance,
       const Matrix3& measuredOmegaCovariance,
       const Matrix3& integrationErrorCovariance,
-      bool use2ndOrderIntegration = false);
+      bool use2ndOrderIntegration = true);
 
 private:
 
@@ -209,14 +210,14 @@ public:
   /// @deprecated constructor
   ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
       const PreintegratedMeasurements& preintegratedMeasurements,
-      const Vector3& gravity, const Vector3& omegaCoriolis,
+      const Vector3& b_gravity, const Vector3& omegaCoriolis,
       const boost::optional<Pose3>& body_P_sensor = boost::none,
       const bool use2ndOrderCoriolis = false);
 
   /// @deprecated use PreintegrationBase::predict
   static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
       Vector3& vel_j, const imuBias::ConstantBias& bias_i,
-      PreintegratedMeasurements& pim, const Vector3& gravity,
+      PreintegratedMeasurements& pim, const Vector3& b_gravity,
       const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
 
 private:

gtsam/navigation/NavState.cpp

@@ -248,25 +248,19 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
 }
 
 //------------------------------------------------------------------------------
-Vector9 NavState::predictXi(const Vector9& pim, double dt,
+Vector9 NavState::integrateTangent(const Vector9& pim, double dt,
-    const Vector3& gravity, const boost::optional<Vector3>& omegaCoriolis,
+    const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis,
-    bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1,
+    OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
-    OptionalJacobian<9, 9> H2) const {
   const Rot3& nRb = R_;
   const Velocity3& n_v = v_; // derivative is Ri !
   const double dt2 = dt * dt;
 
   Vector9 delta;
-  Matrix3 D_dP_Ri, D_dP_dP, D_dV_Ri, D_dV_dV;
+  Matrix3 D_dP_Ri, D_dP_nv;
   dR(delta) = dR(pim);
-  // TODO(frank):
+  dP(delta) = dP(pim)
-  // - why do we integrate n_v here ?
+      + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri : 0, H2 ? &D_dP_nv : 0);
-  // - the dP and dV should be in body ! How come semi-retract works out ??
+  dV(delta) = dV(pim);
-  // - should we rename t_ to p_? if not, we should rename dP do dT
-  dP(delta) = nRb.rotate(dP(pim), H1 ? &D_dP_Ri : 0, H2 ? &D_dP_dP : 0)
-      + n_v * dt + 0.5 * gravity * dt2;
-  dV(delta) = nRb.rotate(dV(pim), H1 ? &D_dV_Ri : 0, H2 ? &D_dV_dV : 0)
-      + gravity * dt;
 
   if (omegaCoriolis) {
     delta += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1);
@@ -278,15 +272,11 @@ Vector9 NavState::predictXi(const Vector9& pim, double dt,
     if (H1) {
       if (!omegaCoriolis)
         H1->setZero(); // if coriolis H1 is already initialized
-      D_t_R(H1) += D_dP_Ri;
+      D_t_R(H1) += dt * D_dP_Ri;
-      D_t_v(H1) += I_3x3 * dt * Ri;
+      D_t_v(H1) += dt * D_dP_nv * Ri;
-      D_v_R(H1) += D_dV_Ri;
     }
     if (H2) {
-      H2->setZero();
+      H2->setIdentity();
-      D_R_R(H2) << I_3x3;
-      D_t_t(H2) << D_dP_dP;
-      D_v_v(H2) << D_dV_dV;
     }
   }
 
@@ -294,13 +284,12 @@ Vector9 NavState::predictXi(const Vector9& pim, double dt,
 }
 //------------------------------------------------------------------------------
 NavState NavState::predict(const Vector9& pim, double dt,
-    const Vector3& gravity, const boost::optional<Vector3>& omegaCoriolis,
+    const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis,
-    bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1,
+    OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
-    OptionalJacobian<9, 9> H2) const {
 
   Matrix9 D_delta_state, D_delta_pim;
-  Vector9 delta = predictXi(pim, dt, gravity, omegaCoriolis,
+  Vector9 delta = integrateTangent(pim, dt, omegaCoriolis, use2ndOrderCoriolis,
-      use2ndOrderCoriolis, H1 ? &D_delta_state : 0, H2 ? &D_delta_pim : 0);
+      H1 ? &D_delta_state : 0, H2 ? &D_delta_pim : 0);
 
   Matrix9 D_predicted_state, D_predicted_delta;
   NavState predicted = retract(delta, H1 ? &D_predicted_state : 0,

gtsam/navigation/NavState.h

@@ -33,6 +33,9 @@ typedef Vector3 Velocity3;
  */
 class NavState: public LieGroup<NavState, 9> {
 private:
+
+  // TODO(frank):
+  // - should we rename t_ to p_? if not, we should rename dP do dT
   Rot3 R_; ///< Rotation nRb, rotates points/velocities in body to points/velocities in nav
   Point3 t_; ///< position n_t, in nav frame
   Velocity3 v_; ///< velocity n_v in nav frame
@@ -207,16 +210,16 @@ public:
   Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
       OptionalJacobian<9, 9> H = boost::none) const;
 
-  /// Combine preintegrated measurements, in the form of a tangent space vector,
+  /// Integrate a tangent vector forwards on tangent space, taking into account
-  /// with gravity, velocity, and Coriolis forces into a tangent space update.
+  /// Coriolis forces if omegaCoriolis is given.
-  Vector9 predictXi(const Vector9& pim, double dt, const Vector3& gravity,
+  Vector9 integrateTangent(const Vector9& pim, double dt,
       const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
           false, OptionalJacobian<9, 9> H1 = boost::none,
       OptionalJacobian<9, 9> H2 = boost::none) const;
 
-  /// Combine preintegrated measurements, in the form of a tangent space vector,
+  /// Integrate a tangent vector forwards on tangent space, taking into account
-  /// with gravity, velocity, and Coriolis forces into a new state.
+  /// Coriolis forces if omegaCoriolis is given. Calls retract after to yield a NavState
-  NavState predict(const Vector9& pim, double dt, const Vector3& gravity,
+  NavState predict(const Vector9& pim, double dt,
       const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
           false, OptionalJacobian<9, 9> H1 = boost::none,
       OptionalJacobian<9, 9> H2 = boost::none) const;

gtsam/navigation/PreintegrationBase.cpp

@@ -64,14 +64,11 @@ void PreintegrationBase::updatePreintegratedMeasurements(
     OptionalJacobian<9, 9> F) {
 
   const Matrix3 dRij = deltaRij_.matrix(); // expensive
-  const Vector3 temp = dRij * correctedAcc * deltaT;
+  const Vector3 j_acc = dRij * correctedAcc; // acceleration in current frame
 
-  if (!p().use2ndOrderIntegration) {
+  double dt22 = 0.5 * deltaT * deltaT;
-    deltaPij_ += deltaVij_ * deltaT;
+  deltaPij_ += deltaVij_ * deltaT + dt22 * j_acc;
-  } else {
+  deltaVij_ += deltaT * j_acc;
-    deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
-  }
-  deltaVij_ += temp;
 
   Matrix3 R_i, F_angles_angles;
   if (F)
@@ -81,10 +78,7 @@ void PreintegrationBase::updatePreintegratedMeasurements(
   if (F) {
     const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT;
     Matrix3 F_pos_angles;
-    if (p().use2ndOrderIntegration)
     F_pos_angles = 0.5 * F_vel_angles * deltaT;
-    else
-      F_pos_angles = Z_3x3;
 
     //    pos  vel             angle
     *F << //
@@ -101,13 +95,8 @@ void PreintegrationBase::updatePreintegratedJacobians(
   const Matrix3 dRij = deltaRij_.matrix(); // expensive
   const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT
       * delRdelBiasOmega_;
-  if (!p().use2ndOrderIntegration) {
-    delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
-    delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
-  } else {
   delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT;
   delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5);
-  }
   delVdelBiasAcc_ += -dRij * deltaT;
   delVdelBiasOmega_ += temp;
   update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
@@ -135,10 +124,9 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
 //------------------------------------------------------------------------------
 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_deltaRij_bias;
+  Rot3 deltaRij = biascorrectedDeltaRij(biasIncr.gyroscope());
-  Rot3 deltaRij = PreintegratedRotation::biascorrectedDeltaRij(
-      biasIncr.gyroscope(), H ? &D_deltaRij_bias : 0);
 
   Vector9 xi;
   Matrix3 D_dR_deltaRij;
@@ -147,9 +135,10 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
       + 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_deltaRij * D_deltaRij_bias;
+    D_dR_bias << Z_3x3, D_dR_deltaRij * delRdelBiasOmega_;
     D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_;
     D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
     (*H) << D_dR_bias, D_dP_bias, D_dV_bias;
@@ -161,13 +150,23 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
 NavState PreintegrationBase::predict(const NavState& state_i,
     const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
     OptionalJacobian<9, 6> H2) const {
+  // correct for bias
   Matrix96 D_biasCorrected_bias;
   Vector9 biasCorrected = biasCorrectedDelta(bias_i,
       H2 ? &D_biasCorrected_bias : 0);
+
+  // integrate on tangent space
   Matrix9 D_delta_state, D_delta_biasCorrected;
-  Vector9 xi = state_i.predictXi(biasCorrected, deltaTij_, p().gravity,
+  Vector9 xi = state_i.integrateTangent(biasCorrected, deltaTij_,
       p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
       H2 ? &D_delta_biasCorrected : 0);
+
+  // Correct for gravity
+  double dt = deltaTij_, dt2 = dt * dt;
+  NavState::dP(xi) += 0.5 * p().b_gravity * dt2;
+  NavState::dV(xi) += p().b_gravity * dt;
+
+  // 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)
@@ -313,11 +312,11 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
 //------------------------------------------------------------------------------
 PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
     const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
-    const Vector3& gravity, const Vector3& omegaCoriolis,
+    const Vector3& b_gravity, const Vector3& omegaCoriolis,
     const bool use2ndOrderCoriolis) {
   // NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
   boost::shared_ptr<Params> q = boost::make_shared<Params>(p());
-  q->gravity = gravity;
+  q->b_gravity = b_gravity;
   q->omegaCoriolis = omegaCoriolis;
   q->use2ndOrderCoriolis = use2ndOrderCoriolis;
   p_ = q;

gtsam/navigation/PreintegrationBase.h

@@ -24,6 +24,7 @@
 #include <gtsam/navigation/PreintegratedRotation.h>
 #include <gtsam/navigation/NavState.h>
 #include <gtsam/navigation/ImuBias.h>
+#include <boost/make_shared.hpp>
 
 namespace gtsam {
 
@@ -32,11 +33,16 @@ struct PoseVelocityBias {
   Pose3 pose;
   Vector3 velocity;
   imuBias::ConstantBias bias;
-  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, const imuBias::ConstantBias _bias)
+  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
-      : pose(_pose), velocity(_velocity), bias(_bias) {}
+      const imuBias::ConstantBias _bias) :
-  PoseVelocityBias(const NavState& navState, const imuBias::ConstantBias _bias)
+      pose(_pose), velocity(_velocity), bias(_bias) {
-      : pose(navState.pose()), velocity(navState.velocity()), bias(_bias) {}
+  }
-  NavState navState() const { return NavState(pose,velocity);}
+  PoseVelocityBias(const NavState& navState, const imuBias::ConstantBias _bias) :
+      pose(navState.pose()), velocity(navState.velocity()), bias(_bias) {
+  }
+  NavState navState() const {
+    return NavState(pose, velocity);
+  }
 };
 
 /**
@@ -55,19 +61,31 @@ class PreintegrationBase : public PreintegratedRotation {
     Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
     Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
     /// (to compensate errors in Euler integration)
-    bool use2ndOrderIntegration;  ///< Controls the order of integration
     ///  (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)
     bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
-    Vector3 gravity;              ///< Gravity constant
+    Vector3 b_gravity; ///< Gravity constant in body frame
 
-    Params()
+    /// The Params constructor insists on the user passing in gravity in the *body* frame.
-        : accelerometerCovariance(I_3x3),
+    /// For convenience, two commonly used conventions are provided by named constructors below
-          integrationCovariance(I_3x3),
+    Params(const Vector3& b_gravity) :
-          use2ndOrderIntegration(false),
+        accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis(
-          use2ndOrderCoriolis(false),
+            false), b_gravity(b_gravity) {
-          gravity(0, 0, 9.8) {}
+    }
+
+    // Default Params for Front-Right-Down convention: gravity points along positive Z-axis
+    static boost::shared_ptr<Params> MakeSharedFRD(double g = 9.81) {
+      return boost::make_shared<Params>(Vector3(0, 0, g));
+    }
+
+    // Default Params for Front-Left-Up convention: gravity points along negative Z-axis
+    static boost::shared_ptr<Params> MakeSharedFLU(double g = 9.81) {
+      return boost::make_shared<Params>(Vector3(0, 0, -g));
+    }
+
+  protected:
+    /// Default constructor for serialization only: uninitialized!
+    Params();
 
-   private:
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
@@ -75,9 +93,8 @@ class PreintegrationBase : public PreintegratedRotation {
       ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
       ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
       ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
-      ar & BOOST_SERIALIZATION_NVP(use2ndOrderIntegration);
       ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
-      ar & BOOST_SERIALIZATION_NVP(gravity);
+      ar & BOOST_SERIALIZATION_NVP(b_gravity);
     }
   };
 
@@ -95,7 +112,8 @@ class PreintegrationBase : public PreintegratedRotation {
   Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
 
   /// Default constructor for serialization
-  PreintegrationBase() {}
+  PreintegrationBase() {
+  }
 
 public:
 
@@ -105,27 +123,45 @@ class PreintegrationBase : public PreintegratedRotation {
    *  @param p    Parameters, typically fixed in a single application
    */
   PreintegrationBase(const boost::shared_ptr<const Params>& p,
-                     const imuBias::ConstantBias& biasHat)
+      const imuBias::ConstantBias& biasHat) :
-      : PreintegratedRotation(p), biasHat_(biasHat) {
+      PreintegratedRotation(p), biasHat_(biasHat) {
     resetIntegration();
   }
 
   /// Re-initialize PreintegratedMeasurements
   void resetIntegration();
 
-  const Params& p() const { return *boost::static_pointer_cast<const Params>(p_);}
+  const Params& p() const {
+    return *boost::static_pointer_cast<const Params>(p_);
+  }
 
   /// getters
-  const imuBias::ConstantBias& biasHat() const { return biasHat_; }
+  const imuBias::ConstantBias& biasHat() const {
-  const Vector3& deltaPij() const { return deltaPij_; }
+    return biasHat_;
-  const Vector3& deltaVij() const { return deltaVij_; }
+  }
-  const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_; }
+  const Vector3& deltaPij() const {
-  const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_; }
+    return deltaPij_;
-  const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_; }
+  }
-  const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_; }
+  const Vector3& deltaVij() const {
+    return deltaVij_;
+  }
+  const Matrix3& delPdelBiasAcc() const {
+    return delPdelBiasAcc_;
+  }
+  const Matrix3& delPdelBiasOmega() const {
+    return delPdelBiasOmega_;
+  }
+  const Matrix3& delVdelBiasAcc() const {
+    return delVdelBiasAcc_;
+  }
+  const Matrix3& delVdelBiasOmega() const {
+    return delVdelBiasOmega_;
+  }
 
   // Exposed for MATLAB
-  Vector6 biasHatVector() const { return biasHat_.vector(); }
+  Vector6 biasHatVector() const {
+    return biasHat_.vector();
+  }
 
   /// print
   void print(const std::string& s) const;
@@ -134,17 +170,15 @@ class PreintegrationBase : public PreintegratedRotation {
   bool equals(const PreintegrationBase& other, double tol) const;
 
   /// Update preintegrated measurements
-  void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& incrR,
+  void updatePreintegratedMeasurements(const Vector3& correctedAcc,
-                                       const double deltaT, OptionalJacobian<9, 9> F);
+      const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F);
 
   /// Update Jacobians to be used during preintegration
   void updatePreintegratedJacobians(const Vector3& correctedAcc,
-                                    const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR,
+      const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT);
-                                    double deltaT);
 
   void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
-                                              const Vector3& measuredOmega,
+      const Vector3& measuredOmega, Vector3* correctedAcc,
-                                              Vector3* correctedAcc,
       Vector3* correctedOmega);
 
   /// Given the estimate of the bias, return a NavState tangent vector
@@ -154,21 +188,21 @@ class PreintegrationBase : public PreintegratedRotation {
 
   /// Predict state at time j
   NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
-      OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = boost::none) const;
+      OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 =
+          boost::none) const;
 
   /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
-  Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j,
+  Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
-                                   const Vector3& vel_j, const imuBias::ConstantBias& bias_i,
+      const Pose3& pose_j, const Vector3& vel_j,
-                                   OptionalJacobian<9, 6> H1 = boost::none,
+      const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1 =
-                                   OptionalJacobian<9, 3> H2 = boost::none,
+          boost::none, OptionalJacobian<9, 3> H2 = boost::none,
-                                   OptionalJacobian<9, 6> H3 = boost::none,
+      OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
-                                   OptionalJacobian<9, 3> H4 = boost::none,
+          boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
-                                   OptionalJacobian<9, 6> H5 = boost::none) const;
 
   /// @deprecated predict
   PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
-      const imuBias::ConstantBias& bias_i, const Vector3& gravity, const Vector3& omegaCoriolis,
+      const imuBias::ConstantBias& bias_i, const Vector3& b_gravity,
-      const bool use2ndOrderCoriolis = false);
+      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
 
 private:
   /** Serialization function */

gtsam/navigation/tests/testImuFactor.cpp

@@ -57,15 +57,11 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
 Vector updatePreintegratedPosVel(const Vector3 deltaPij_old,
     const Vector3& deltaVij_old, const Rot3& deltaRij_old,
     const Vector3& correctedAcc, const Vector3& correctedOmega,
-    const double deltaT, const bool use2ndOrderIntegration_) {
+    const double deltaT) {
   Matrix3 dRij = deltaRij_old.matrix();
   Vector3 temp = dRij * correctedAcc * deltaT;
   Vector3 deltaPij_new;
-  if (!use2ndOrderIntegration_) {
-    deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
-  } else {
   deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
-  }
   Vector3 deltaVij_new = deltaVij_old + temp;
 
   Vector result(6);
@@ -93,11 +89,9 @@ const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
 /* ************************************************************************* */
 PreintegratedImuMeasurements evaluatePreintegratedMeasurements(
     const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
-    const list<Vector3>& measuredOmegas, const list<double>& deltaTs,
+    const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
-    const bool use2ndOrderIntegration = false) {
   PreintegratedImuMeasurements result(bias, kMeasuredAccCovariance,
-      kMeasuredOmegaCovariance, kIntegrationErrorCovariance,
+      kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
-      use2ndOrderIntegration);
 
   list<Vector3>::const_iterator itAcc = measuredAccs.begin();
   list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@@ -142,7 +136,7 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
 } // namespace
 
 /* ************************************************************************* */
-namespace straight {
+TEST(ImuFactor, StraightLine) {
   // Set up IMU measurements
   static const double g = 10; // make this easy to check
   static const double a = 0.2, dt = 3.0;
@@ -154,15 +148,10 @@ Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0);
   static const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
   static const Point3 initial_position(10, 20, 0);
   static const NavState state1(nRb, initial_position, Velocity3(0, 0, 0));
-}
-
-TEST(ImuFactor, StraightLineSecondOrder) {
-  using namespace straight;
 
   imuBias::ConstantBias biasHat, bias;
   boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
       PreintegratedImuMeasurements::Params::MakeSharedFRD();
-  p->use2ndOrderIntegration = true;
   p->b_gravity = Vector3(0, 0, g); // FRD convention
 
   PreintegratedImuMeasurements pim(p, biasHat);
@@ -186,39 +175,6 @@ TEST(ImuFactor, StraightLineSecondOrder) {
   EXPECT(assert_equal(expected, pim.predict(state1, bias)));
 }
 
-TEST(ImuFactor, StraightLineFirstOrder) {
-  using namespace straight;
-
-  imuBias::ConstantBias biasHat, bias;
-  boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
-      PreintegratedImuMeasurements::Params::MakeSharedFRD();
-  p->use2ndOrderIntegration = false;
-  p->b_gravity = Vector3(0, 0, g); // FRD convention
-
-  // We do a rough approximation:
-  PreintegratedImuMeasurements pim(p, biasHat);
-  for (size_t i = 0; i < 10; i++)
-    pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10);
-
-  // Check integrated quantities in body frame: gravity measured by IMU is integrated!
-  const double approx = exact * 0.9; // approximation for dt split into 10 intervals
-  EXPECT(assert_equal(Rot3(), pim.deltaRij()));
-  EXPECT(assert_equal(Vector3(a * approx, 0, -g * approx), pim.deltaPij()));
-  EXPECT(assert_equal(Vector3(a * dt, 0, -g * dt), pim.deltaVij())); // still correct
-
-  // Check bias-corrected delta: also in body frame
-  Vector9 expectedBC;
-  expectedBC << 0, 0, 0, a * approx, 0, -g * approx, a * dt, 0, -g * dt;
-  EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(bias)));
-
-  // Check prediction, note we move along x in body, along y in nav
-  // In this instance the position is just an approximation,
-  // but gravity should be subtracted out exactly
-  NavState expected(nRb, Point3(10, 20 + a * approx, 0), Velocity3(0, a * dt, 0));
-  GTSAM_PRINT(pim);
-  EXPECT(assert_equal(expected, pim.predict(state1, bias)));
-}
-
 /* ************************************************************************* */
 TEST(ImuFactor, PreintegratedMeasurements) {
   // Linearization point
@@ -236,11 +192,9 @@ TEST(ImuFactor, PreintegratedMeasurements) {
   Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0);
   double expectedDeltaT1(0.5);
 
-  bool use2ndOrderIntegration = true;
   // Actual preintegrated values
   PreintegratedImuMeasurements actual1(bias, kMeasuredAccCovariance,
-      kMeasuredOmegaCovariance, kIntegrationErrorCovariance,
+      kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
-      use2ndOrderIntegration);
   actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
   EXPECT(
@@ -299,7 +253,6 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
   p->omegaCoriolis = Vector3(0.02, 0.03, 0.04);
   p->accelerometerCovariance = kMeasuredAccCovariance;
   p->integrationCovariance = kIntegrationErrorCovariance;
-  p->use2ndOrderIntegration = false;
   p->use2ndOrderCoriolis = true;
 
   PreintegratedImuMeasurements pim(p, bias);
@@ -332,10 +285,8 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
 /* ************************************************************************* */
 TEST(ImuFactor, ErrorAndJacobians) {
   using namespace common;
-  bool use2ndOrderIntegration = true;
   PreintegratedImuMeasurements pim(bias, kMeasuredAccCovariance,
-      kMeasuredOmegaCovariance, kIntegrationErrorCovariance,
+      kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
-      use2ndOrderIntegration);
 
   pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
   EXPECT(assert_equal(state2, pim.predict(state1, bias), 1e-6));
@@ -631,11 +582,10 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
         Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
     deltaTs.push_back(0.01);
   }
-  bool use2ndOrderIntegration = false;
   // Actual preintegrated values
   PreintegratedImuMeasurements preintegrated =
       evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
-          deltaTs, use2ndOrderIntegration);
+          deltaTs);
 
   // so far we only created a nontrivial linearization point for the preintegrated measurements
   // Now we add a new measurement and ask for Jacobians
@@ -658,20 +608,17 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
   // Compute expected f_pos_vel wrt positions
   Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>(
       boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old,
-          newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
+          newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaPij_old);
-      deltaPij_old);
 
   // Compute expected f_pos_vel wrt velocities
   Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>(
       boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old,
-          newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
+          newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaVij_old);
-      deltaVij_old);
 
   // Compute expected f_pos_vel wrt angles
   Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>(
       boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1,
-          newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
+          newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaRij_old);
-      deltaRij_old);
 
   Matrix FexpectedTop6(6, 9);
   FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
@@ -698,14 +645,12 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
   // Compute jacobian wrt acc noise
   Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>(
       boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
-          deltaRij_old, _1, newMeasuredOmega, newDeltaT,
+          deltaRij_old, _1, newMeasuredOmega, newDeltaT), newMeasuredAcc);
-          use2ndOrderIntegration), newMeasuredAcc);
 
   // Compute expected F wrt gyro noise
   Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>(
       boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
-          deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration),
+          deltaRij_old, newMeasuredAcc, _1, newDeltaT), newMeasuredOmega);
-      newMeasuredOmega);
   Matrix GexpectedTop6(6, 9);
   GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
 
@@ -754,11 +699,10 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
         Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
     deltaTs.push_back(0.01);
   }
-  bool use2ndOrderIntegration = true;
   // Actual preintegrated values
   PreintegratedImuMeasurements preintegrated =
       evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
-          deltaTs, use2ndOrderIntegration);
+          deltaTs);
 
   // so far we only created a nontrivial linearization point for the preintegrated measurements
   // Now we add a new measurement and ask for Jacobians
@@ -781,20 +725,17 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
   // Compute expected f_pos_vel wrt positions
   Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>(
       boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old,
-          newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
+          newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaPij_old);
-      deltaPij_old);
 
   // Compute expected f_pos_vel wrt velocities
   Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>(
       boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old,
-          newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
+          newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaVij_old);
-      deltaVij_old);
 
   // Compute expected f_pos_vel wrt angles
   Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>(
       boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1,
-          newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
+          newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaRij_old);
-      deltaRij_old);
 
   Matrix FexpectedTop6(6, 9);
   FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
@@ -821,14 +762,12 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
   // Compute jacobian wrt acc noise
   Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>(
       boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
-          deltaRij_old, _1, newMeasuredOmega, newDeltaT,
+          deltaRij_old, _1, newMeasuredOmega, newDeltaT), newMeasuredAcc);
-          use2ndOrderIntegration), newMeasuredAcc);
 
   // Compute expected F wrt gyro noise
   Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>(
       boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
-          deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration),
+          deltaRij_old, newMeasuredAcc, _1, newDeltaT), newMeasuredOmega);
-      newMeasuredOmega);
   Matrix GexpectedTop6(6, 9);
   GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
 

gtsam/navigation/tests/testNavState.cpp

@@ -178,12 +178,12 @@ TEST(NavState, PredictXi) {
   xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
   double dt = 0.5;
   Matrix9 actualH1, actualH2;
-  boost::function<Vector9(const NavState&, const Vector9&)> predictXi =
+  boost::function<Vector9(const NavState&, const Vector9&)> integrateTangent =
-      boost::bind(&NavState::predictXi, _1, _2, dt, kGravity, kOmegaCoriolis,
+      boost::bind(&NavState::integrateTangent, _1, _2, dt, kGravity, kOmegaCoriolis,
           false, boost::none, boost::none);
-  kState1.predictXi(xi, dt, kGravity, kOmegaCoriolis, false, actualH1, actualH2);
+  kState1.integrateTangent(xi, dt, kGravity, kOmegaCoriolis, false, actualH1, actualH2);
-  EXPECT(assert_equal(numericalDerivative21(predictXi, kState1, xi), actualH1));
+  EXPECT(assert_equal(numericalDerivative21(integrateTangent, kState1, xi), actualH1));
-  EXPECT(assert_equal(numericalDerivative22(predictXi, kState1, xi), actualH2));
+  EXPECT(assert_equal(numericalDerivative22(integrateTangent, kState1, xi), actualH2));
 }
 
 /* ************************************************************************* */