added unit test, improved computation

gtsam/navigation/PreintegrationBase.h

@@ -178,7 +178,9 @@ public:
   //------------------------------------------------------------------------------
   PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
       const imuBias::ConstantBias& bias_i, const Vector3& gravity,
-      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const {
+      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
+      boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
+      boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) const {
 
     const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer();
     const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
@@ -188,16 +190,20 @@ public:
 
     // Predict state at time j
     /* ---------------------------------------------------------------------------------------------------- */
-    Vector3 pos_j =  pos_i + Rot_i.matrix() * (deltaPij()
+    Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr;
-        + delPdelBiasAcc() * biasAccIncr
+    if(deltaPij_biascorrected_out)// if desired, store this
-        + delPdelBiasOmega() * biasOmegaIncr)
+      *deltaPij_biascorrected_out = deltaPij_biascorrected;
+
+    Vector3 pos_j =  pos_i + Rot_i.matrix() * deltaPij_biascorrected
         + vel_i * deltaTij()
         - omegaCoriolis.cross(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() * (deltaVij()
+    Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr;
-        + delVdelBiasAcc() * biasAccIncr
+    if(deltaVij_biascorrected_out)// if desired, store this
-        + delVdelBiasOmega() * biasOmegaIncr)
+      *deltaVij_biascorrected_out = deltaVij_biascorrected;
+
+    Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected
         - 2 * omegaCoriolis.cross(vel_i) * deltaTij()  // Coriolis term
         + gravity * deltaTij());
 
@@ -231,15 +237,28 @@ public:
       boost::optional<Matrix&> H5) const {
 
     // We need the mismatch w.r.t. the biases used for preintegration
-    const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer();
+    // const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); // this is not necessary
     const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
 
     // we give some shorter name to rotations and translations
     const Rot3& Ri = pose_i.rotation();
     const Rot3& Rj = pose_j.rotation();
-    const Vector3& pos_i = pose_i.translation().vector();
     const Vector3& pos_j = pose_j.translation().vector();
 
+    // Evaluate residual error, according to [3]
+    /* ---------------------------------------------------------------------------------------------------- */
+    Vector3 deltaPij_biascorrected, deltaVij_biascorrected;
+    PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity,
+        omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected);
+
+    // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
+    const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() );
+
+    // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
+    const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity );
+
+    // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j)
+
     // Jacobian computation
     /* ---------------------------------------------------------------------------------------------------- */
     // Get Get so<3> version of bias corrected rotation
@@ -248,7 +267,7 @@ public:
     Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5);
 
     // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
-    const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis);
+    const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis);
     const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis);
     Vector3 correctedOmega = biascorrectedOmega  - coriolis;
 
@@ -271,27 +290,23 @@ public:
       fRrot = correctedDeltaRij.between(Ri.between(Rj));
       fR = Rot3::Logmap(fRrot);
     }
-
     if(H1) {
       H1->resize(9,6);
-
       Matrix3 dfPdPi = -I_3x3;
       Matrix3 dfVdPi = Z_3x3;
       if(use2ndOrderCoriolis){
-        dfPdPi += 0.5 * Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij()*deltaTij();
+        // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix()
-        dfVdPi += Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij();
+        Matrix3 temp = Ritranspose_omegaCoriolisHat * (-Ritranspose_omegaCoriolisHat.transpose());
+        dfPdPi += 0.5 * temp * deltaTij()*deltaTij();
+        dfVdPi += temp * deltaTij();
       }
       (*H1) <<
           // dfP/dRi
-          skewSymmetric( Ri.inverse().matrix() * (pos_j - pos_i - vel_i * deltaTij()
+          skewSymmetric(fp + deltaPij_biascorrected),
-          + omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij()  // Coriolis term - we got rid of the 2 wrt ins paper
-          - 0.5 * gravity *deltaTij()*deltaTij())),
           // dfP/dPi
           dfPdPi,
           // dfV/dRi
-          skewSymmetric( Ri.inverse().matrix() *
+          skewSymmetric(fv + deltaVij_biascorrected),
-              (vel_j - vel_i - gravity * deltaTij() + 2 * omegaCoriolis.cross(vel_i) * deltaTij()  // Coriolis term
-           ) ),
           // dfV/dPi
           dfVdPi,
           // dfR/dRi
@@ -304,10 +319,10 @@ public:
       (*H2) <<
           // dfP/dVi
           - Ri.transpose() * deltaTij()
-          + Ri.transpose() * omegaCoriolisHat * deltaTij() * deltaTij(),  // Coriolis term - we got rid of the 2 wrt ins paper
+          + Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(),  // Coriolis term - we got rid of the 2 wrt ins paper
           // dfV/dVi
           - Ri.transpose()
-          + 2 * Ri.transpose() * omegaCoriolisHat * deltaTij(), // Coriolis term
+          + 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term
           // dfR/dVi
           Z_3x3;
     }
@@ -343,20 +358,6 @@ public:
           // dfR/dBias
           Z_3x3,               D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega);
     }
-
-    // Evaluate residual error, according to [3]
-    /* ---------------------------------------------------------------------------------------------------- */
-    PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity,
-        omegaCoriolis, use2ndOrderCoriolis);
-
-    // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
-    const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() );
-
-    // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
-    const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity );
-
-    // fR was computes earlier. Note: it is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j)
-
     Vector r(9); r << fp, fv, fR;
     return r;
   }

gtsam/navigation/tests/testImuFactor.cpp

@@ -315,6 +315,71 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases )
   EXPECT(assert_equal(H5e, H5a));
 }
 
+/* ************************************************************************* */
+TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis )
+{
+  imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
+  Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0));
+  Vector3 v1(Vector3(0.5, 0.0, 0.0));
+  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
+  Vector3 v2(Vector3(0.5, 0.0, 0.0));
+
+  // Measurements
+  Vector3 gravity; gravity << 0, 0, 9.81;
+  Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
+  Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
+  Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
+  double deltaT = 1.0;
+
+  ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
+      Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
+  pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
+
+  // Create factor
+  Pose3 bodyPsensor = Pose3();
+  bool use2ndOrderCoriolis = true;
+  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis);
+
+  SETDEBUG("ImuFactor evaluateError", false);
+  Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
+  SETDEBUG("ImuFactor evaluateError", false);
+
+  // Expected error (should not be zero in this test, as we want to evaluate Jacobians
+  // at a nontrivial linearization point)
+  // Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
+  // EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
+
+  // Expected Jacobians
+  Matrix H1e = numericalDerivative11<Vector,Pose3>(
+      boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
+  Matrix H2e = numericalDerivative11<Vector,Vector3>(
+      boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
+  Matrix H3e = numericalDerivative11<Vector,Pose3>(
+      boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
+  Matrix H4e = numericalDerivative11<Vector,Vector3>(
+      boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
+  Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
+      boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
+
+  // Check rotation Jacobians
+  Matrix RH1e = numericalDerivative11<Rot3,Pose3>(
+      boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
+  Matrix RH3e = numericalDerivative11<Rot3,Pose3>(
+      boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);
+  Matrix RH5e = numericalDerivative11<Rot3,imuBias::ConstantBias>(
+      boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias);
+
+  // Actual Jacobians
+  Matrix H1a, H2a, H3a, H4a, H5a;
+  (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
+
+  EXPECT(assert_equal(H1e, H1a));
+  EXPECT(assert_equal(H2e, H2a));
+  EXPECT(assert_equal(H3e, H3a));
+  EXPECT(assert_equal(H4e, H4a));
+  EXPECT(assert_equal(H5e, H5a));
+}
+
 /* ************************************************************************* */
 TEST( ImuFactor, PartialDerivative_wrt_Bias )
 {