Now uses NavState::predict !

gtsam/navigation/PreintegrationBase.cpp

@@ -63,7 +63,7 @@ void PreintegrationBase::updatePreintegratedMeasurements(
     const Vector3& correctedAcc, const Rot3& incrR, const double deltaT,
     OptionalJacobian<9, 9> F) {
 
-  const Matrix3 dRij = deltaRij().matrix(); // expensive
+  const Matrix3 dRij = deltaRij_.matrix(); // expensive
   const Vector3 temp = dRij * correctedAcc * deltaT;
 
   if (!p().use2ndOrderIntegration) {
@@ -98,9 +98,9 @@ void PreintegrationBase::updatePreintegratedMeasurements(
 void PreintegrationBase::updatePreintegratedJacobians(
     const Vector3& correctedAcc, const Matrix3& D_Rincr_integratedOmega,
     const Rot3& incrR, double deltaT) {
-  const Matrix3 dRij = deltaRij().matrix(); // expensive
+  const Matrix3 dRij = deltaRij_.matrix(); // expensive
   const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT
-      * delRdelBiasOmega();
+      * delRdelBiasOmega_;
   if (!p().use2ndOrderIntegration) {
     delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
     delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
@@ -157,48 +157,6 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
   return xi;
 }
 
-//------------------------------------------------------------------------------
-Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i,
-    const Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1,
-    OptionalJacobian<9, 9> H2) const {
-
-  const Rot3& rot_i = state_i.attitude();
-  const Vector3& vel_i = state_i.velocity();
-  const double dt = deltaTij(), dt2 = dt * dt;
-
-  // Rotation, position, and velocity:
-  Vector9 xi;
-  Matrix3 D_dP_Ri, D_dP_bc, D_dV_Ri, D_dV_bc;
-  NavState::dR(xi) = NavState::dR(biasCorrectedDelta);
-  NavState::dP(xi) = rot_i.rotate(NavState::dP(biasCorrectedDelta), D_dP_Ri,
-      D_dP_bc) + vel_i * dt + 0.5 * p().gravity * dt2;
-  NavState::dV(xi) = rot_i.rotate(NavState::dV(biasCorrectedDelta), D_dV_Ri,
-      D_dV_bc) + p().gravity * dt;
-
-  Matrix9 Hcoriolis;
-  if (p().omegaCoriolis) {
-    state_i.addCoriolis(&xi, *(p().omegaCoriolis), deltaTij(),
-        p().use2ndOrderCoriolis, H1 ? &Hcoriolis : 0);
-  }
-  if (H1) {
-    H1->setZero();
-    H1->block<3, 3>(3, 0) = D_dP_Ri;
-    H1->block<3, 3>(3, 6) = I_3x3 * dt;
-    H1->block<3, 3>(6, 0) = D_dV_Ri;
-    if (p().omegaCoriolis)
-      *H1 += Hcoriolis;
-  }
-  if (H2) {
-    H2->setZero();
-    Matrix3 Ri = rot_i.matrix();
-    H2->block<3, 3>(0, 0) = I_3x3;
-    H2->block<3, 3>(3, 3) = Ri;
-    H2->block<3, 3>(6, 6) = Ri;
-  }
-
-  return xi;
-}
-
 //------------------------------------------------------------------------------
 NavState PreintegrationBase::predict(const NavState& state_i,
     const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
@@ -207,8 +165,9 @@ NavState PreintegrationBase::predict(const NavState& state_i,
   Vector9 biasCorrected = biasCorrectedDelta(bias_i,
       H2 ? &D_biasCorrected_bias : 0);
   Matrix9 D_delta_state, D_delta_biasCorrected;
-  Vector9 xi = recombinedPrediction(state_i, biasCorrected,
+  Vector9 xi = state_i.predictXi(biasCorrected, deltaTij_, p().gravity,
-      H1 ? &D_delta_state : 0, H2 ? &D_delta_biasCorrected : 0);
+      p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
+      H2 ? &D_delta_biasCorrected : 0);
   Matrix9 D_predict_state, D_predict_delta;
   NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
   if (H1)
@@ -300,7 +259,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
   Matrix3 D_fR_fRrot;
   Rot3::Logmap(fRrot, H1 || H3 || H5 ? &D_fR_fRrot : 0);
 
-  const double dt = deltaTij(), dt2 = dt * dt;
+  const double dt = deltaTij_, dt2 = dt * dt;
   Matrix3 RitOmegaCoriolisHat = Z_3x3;
   if ((H1 || H2) && p().omegaCoriolis)
     RitOmegaCoriolisHat = Ri.transpose() * skewSymmetric(*p().omegaCoriolis);

gtsam/navigation/PreintegrationBase.h

@@ -152,11 +152,6 @@ class PreintegrationBase : public PreintegratedRotation {
   Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
       OptionalJacobian<9, 6> H = boost::none) const;
 
-  /// Recombine the preintegration, gravity, and coriolis in a single NavState tangent vector
-  Vector9 recombinedPrediction(const NavState& state_i,
-      const Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1 = boost::none,
-      OptionalJacobian<9, 9> H2 = boost::none) const;
-
   /// 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;

gtsam/navigation/tests/testImuFactor.cpp

@@ -251,10 +251,12 @@ TEST( NavState, Lie ) {
   Matrix9 aH1, aH2;
   state1.retract(delta, aH1, aH2);
   Matrix eH1 = numericalDerivative11<NavState, NavState>(
-      boost::bind(&NavState::retract, _1, delta, boost::none, boost::none),state1);
+      boost::bind(&NavState::retract, _1, delta, boost::none, boost::none),
+      state1);
   EXPECT(assert_equal(eH1, aH1));
   Matrix eH2 = numericalDerivative11<NavState, Vector9>(
-      boost::bind(&NavState::retract, state1, _1, boost::none, boost::none), delta);
+      boost::bind(&NavState::retract, state1, _1, boost::none, boost::none),
+      delta);
   EXPECT(assert_equal(eH2, aH2));
 }
 
@@ -282,19 +284,6 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
             boost::none), bias);
     EXPECT(assert_equal(expectedH, actualH));
   }
-  {
-    Matrix9 aH1, aH2;
-    Vector9 biasCorrectedDelta = pim.biasCorrectedDelta(bias);
-    pim.recombinedPrediction(state1, biasCorrectedDelta, aH1, aH2);
-    Matrix eH1 = numericalDerivative11<Vector9, NavState>(
-        boost::bind(&PreintegrationBase::recombinedPrediction, pim, _1,
-            biasCorrectedDelta, boost::none, boost::none), state1);
-    EXPECT(assert_equal(eH1, aH1));
-    Matrix eH2 = numericalDerivative11<Vector9, Vector9>(
-        boost::bind(&PreintegrationBase::recombinedPrediction, pim, state1, _1,
-            boost::none, boost::none), biasCorrectedDelta);
-    EXPECT(assert_equal(eH2, aH2));
-  }
   {
     Matrix9 aH1;
     Matrix96 aH2;
@@ -302,11 +291,11 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
     Matrix eH1 = numericalDerivative11<NavState, NavState>(
         boost::bind(&PreintegrationBase::predict, pim, _1, bias, boost::none,
             boost::none), state1);
-    EXPECT(assert_equal(eH1, aH1));
+    EXPECT(assert_equal(eH1, aH1, 1e-8));
     Matrix eH2 = numericalDerivative11<NavState, imuBias::ConstantBias>(
         boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none,
             boost::none), bias);
-    EXPECT(assert_equal(eH2, aH2));
+    EXPECT(assert_equal(eH2, aH2, 1e-8));
   }
 }
 
@@ -314,9 +303,9 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
 TEST(ImuFactor, ErrorAndJacobians) {
   using namespace common;
   bool use2ndOrderIntegration = true;
-  PreintegratedImuMeasurements pim(bias,
+  PreintegratedImuMeasurements pim(bias, kMeasuredAccCovariance,
-      kMeasuredAccCovariance, kMeasuredOmegaCovariance,
+      kMeasuredOmegaCovariance, kIntegrationErrorCovariance,
-      kIntegrationErrorCovariance, use2ndOrderIntegration);
+      use2ndOrderIntegration);
   pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
   // Create factor
@@ -933,7 +922,8 @@ TEST(ImuFactor, PredictRotation) {
     pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
   // Create factor
-  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity, kZeroOmegaCoriolis);
+  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
+      kZeroOmegaCoriolis);
 
   // Predict
   Pose3 x1, x2;