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

@@ -213,26 +213,26 @@ double deltaT = 1.0;
 TEST( NavState, Lie ) {
   // origin and zero deltas
   NavState identity;
-  const double tol=1e-5;
+  const double tol = 1e-5;
   Rot3 rot = Rot3::RzRyRx(0.1, 0.2, 0.3);
   Point3 pt(1.0, 2.0, 3.0);
   Velocity3 vel(0.4, 0.5, 0.6);
 
-  EXPECT(assert_equal(identity, (NavState)identity.retract(zero(9)), tol));
+  EXPECT(assert_equal(identity, (NavState )identity.retract(zero(9)), tol));
   EXPECT(assert_equal(zero(9), identity.localCoordinates(identity), tol));
 
   NavState state1(rot, pt, vel);
-  EXPECT(assert_equal(state1, (NavState)state1.retract(zero(9)), tol));
+  EXPECT(assert_equal(state1, (NavState )state1.retract(zero(9)), tol));
   EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol));
 
   // Special retract
   Vector delta(9);
-  delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3;
+  delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
   Rot3 drot = Rot3::Expmap(delta.head<3>());
-  Point3 dt = Point3(delta.segment<3>(3));
+  Point3 dt = Point3(delta.segment < 3 > (3));
   Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3);
   NavState state2 = state1 * NavState(drot, dt, dvel);
-  EXPECT(assert_equal(state2, (NavState)state1.retract(delta), tol));
+  EXPECT(assert_equal(state2, (NavState )state1.retract(delta), tol));
   EXPECT(assert_equal(delta, state1.localCoordinates(state2), tol));
 
   // roundtrip from state2 to state3 and back
@@ -244,17 +244,19 @@ TEST( NavState, Lie ) {
   EXPECT(assert_equal(delta, state3.logmap(state4), tol));
 
   // For the expmap/logmap (not necessarily retract/local) -delta goes other way
-  EXPECT(assert_equal(state3, (NavState)state4.expmap(-delta), tol));
+  EXPECT(assert_equal(state3, (NavState )state4.expmap(-delta), tol));
   EXPECT(assert_equal(delta, -state4.logmap(state3), tol));
 
   // retract derivatives
   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;