refactoring F and G

gtsam/navigation/CombinedImuFactor.cpp

@@ -50,18 +50,33 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
   preintMeasCov_.setZero();
 }
 
+//------------------------------------------------------------------------------
+// sugar for derivative blocks
+#define D_R_R(H) (H)->block<3,3>(0,0)
+#define D_R_t(H) (H)->block<3,3>(0,3)
+#define D_R_v(H) (H)->block<3,3>(0,6)
+#define D_t_R(H) (H)->block<3,3>(3,0)
+#define D_t_t(H) (H)->block<3,3>(3,3)
+#define D_t_v(H) (H)->block<3,3>(3,6)
+#define D_v_R(H) (H)->block<3,3>(6,0)
+#define D_v_t(H) (H)->block<3,3>(6,3)
+#define D_v_v(H) (H)->block<3,3>(6,6)
+#define D_a_a(H) (H)->block<3,3>(9,9)
+#define D_g_g(H) (H)->block<3,3>(12,12)
+
 //------------------------------------------------------------------------------
 void PreintegratedCombinedMeasurements::integrateMeasurement(
     const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
-    boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) {
+    OptionalJacobian<15, 15> F_test, OptionalJacobian<15, 21> G_test) {
 
-  const Matrix3 dRij = deltaRij_.matrix(); // expensive when quaternion
+  const Matrix3 dRij = deltaXij_.R(); // expensive when quaternion
 
   // Update preintegrated measurements.
   Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
   Matrix9 F_9x9; // overall Jacobian wrt preintegrated measurements (df/dx)
+  Matrix93 G1,G2;
   updatePreintegratedMeasurements(measuredAcc, measuredOmega, deltaT,
-      &D_incrR_integratedOmega, &F_9x9);
+      &D_incrR_integratedOmega, &F_9x9, &G1, &G2);
 
   // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
   // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
@@ -74,17 +89,11 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
 
   // overall Jacobian wrt preintegrated measurements (df/dx)
   Eigen::Matrix<double,15,15> F;
-  // for documentation:
-  //  F << I_3x3,    I_3x3 * deltaT,   Z_3x3,             Z_3x3,            Z_3x3,
-  //       Z_3x3,    I_3x3,            H_vel_angles,      H_vel_biasacc,    Z_3x3,
-  //       Z_3x3,    Z_3x3,            H_angles_angles,   Z_3x3,            H_angles_biasomega,
-  //       Z_3x3,    Z_3x3,            Z_3x3,             I_3x3,            Z_3x3,
-  //       Z_3x3,    Z_3x3,            Z_3x3,             Z_3x3,            I_3x3;
   F.setZero();
   F.block<9, 9>(0, 0) = F_9x9;
+  F.block<3, 3>(0, 12) = H_angles_biasomega;
+  F.block<3, 3>(6, 9) = H_vel_biasacc;
   F.block<6, 6>(9, 9) = I_6x6;
-  F.block<3, 3>(3, 9) = H_vel_biasacc;
-  F.block<3, 3>(6, 12) = H_angles_biasomega;
 
   // first order uncertainty propagation
   // Optimized matrix multiplication   (1/deltaT) * G * measurementCovariance * G.transpose()
@@ -92,38 +101,36 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
   G_measCov_Gt.setZero(15, 15);
 
   // BLOCK DIAGONAL TERMS
-  G_measCov_Gt.block<3, 3>(0, 0) = deltaT * p().integrationCovariance;
+  D_t_t(&G_measCov_Gt) = deltaT * p().integrationCovariance;
-  G_measCov_Gt.block<3, 3>(3, 3) = (1 / deltaT) * (H_vel_biasacc)
+  D_v_v(&G_measCov_Gt) = (1 / deltaT) * (H_vel_biasacc)
       * (p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0))
       * (H_vel_biasacc.transpose());
-  G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega)
+  D_R_R(&G_measCov_Gt) = (1 / deltaT) * (H_angles_biasomega)
       * (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3))
       * (H_angles_biasomega.transpose());
-  G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * p().biasAccCovariance;
+  D_a_a(&G_measCov_Gt) = (1 / deltaT) * p().biasAccCovariance;
-  G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * p().biasOmegaCovariance;
+  D_g_g(&G_measCov_Gt) = (1 / deltaT) * p().biasOmegaCovariance;
 
   // OFF BLOCK DIAGONAL TERMS
-  Matrix3 block23 = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0)
+  Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0)
       * H_angles_biasomega.transpose();
-  G_measCov_Gt.block<3, 3>(3, 6) = block23;
+  D_v_R(&G_measCov_Gt) = temp;
-  G_measCov_Gt.block<3, 3>(6, 3) = block23.transpose();
+  D_R_v(&G_measCov_Gt) = temp.transpose();
   preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
 
   // F_test and G_test are used for testing purposes and are not needed by the factor
   if (F_test) {
-    F_test->resize(15, 15);
     (*F_test) << F;
   }
   if (G_test) {
-    G_test->resize(15, 21);
     // This is for testing & documentation
     ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
     (*G_test) << //
-        I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, //
+        -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, //
-    Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, //
+        Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, //
-    Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, //
+        Z_3x3, Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, //
-    Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, //
+        Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, //
-    Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
+        Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
   }
 }
 
@@ -198,7 +205,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
   Matrix93 D_r_vel_i, D_r_vel_j;
 
   // error wrt preintegrated measurements
-  Vector9 r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
+  Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
       H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
       H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0);
 
@@ -242,7 +249,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
 
   // overall error
   Vector r(15);
-  r << r_pvR, fbias; // vector of size 15
+  r << r_Rpv, fbias; // vector of size 15
   return r;
 }
 

gtsam/navigation/CombinedImuFactor.h

@@ -142,10 +142,10 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase {
    * @param body_P_sensor Optional sensor frame (pose of the IMU in the body
    * frame)
    */
-  void integrateMeasurement(
+  void integrateMeasurement(const Vector3& measuredAcc,
-      const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
+      const Vector3& measuredOmega, double deltaT,
-      boost::optional<Matrix&> F_test = boost::none,
+      OptionalJacobian<15, 15> F_test = boost::none,
-      boost::optional<Matrix&> G_test = boost::none);
+      OptionalJacobian<15, 21> G_test = boost::none);
 
   /// methods to access class variables
   Matrix preintMeasCov() const { return preintMeasCov_; }

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -47,36 +47,30 @@ namespace {
 Vector updatePreintegratedMeasurementsTest(const Vector3 deltaPij_old,
     const Vector3& deltaVij_old, const Rot3& deltaRij_old,
     const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc,
-    const Vector3& correctedOmega, const double deltaT,
+    const Vector3& correctedOmega, const double deltaT) {
-    const bool use2ndOrderIntegration) {
 
   Matrix3 dRij = deltaRij_old.matrix();
   Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT;
   Vector3 deltaPij_new;
-  if (!use2ndOrderIntegration) {
+  deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
-    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;
   Rot3 deltaRij_new = deltaRij_old
       * Rot3::Expmap((correctedOmega - bias_old.gyroscope()) * deltaT);
   Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more
   imuBias::ConstantBias bias_new(bias_old);
   Vector result(15);
-  result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector();
+  result << logDeltaRij_new, deltaPij_new, deltaVij_new, bias_new.vector();
   return result;
 }
 
 Rot3 updatePreintegratedMeasurementsRot(const Vector3 deltaPij_old,
     const Vector3& deltaVij_old, const Rot3& deltaRij_old,
     const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc,
-    const Vector3& correctedOmega, const double deltaT,
+    const Vector3& correctedOmega, const double deltaT) {
-    const bool use2ndOrderIntegration) {
 
   Vector result = updatePreintegratedMeasurementsTest(deltaPij_old,
       deltaVij_old, deltaRij_old, bias_old, correctedAcc, correctedOmega,
-      deltaT, use2ndOrderIntegration);
+      deltaT);
 
   return Rot3::Expmap(result.segment < 3 > (6));
 }
@@ -377,10 +371,8 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
   Matrix oldPreintCovariance = pim.preintMeasCov();
 
   Matrix Factual, Gactual;
-  pim.integrateMeasurement(newMeasuredAcc, newMeasuredOmega,
+  pim.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, Factual,
-      newDeltaT, Factual, Gactual);
+      Gactual);
-
-  bool use2ndOrderIntegration = false;
 
   //////////////////////////////////////////////////////////////////////////////////////////////
   // COMPUTE NUMERICAL DERIVATIVES FOR F
@@ -388,51 +380,51 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
   // Compute expected F wrt positions (15,3)
   Matrix df_dpos = numericalDerivative11<Vector, Vector3>(
       boost::bind(&updatePreintegratedMeasurementsTest, _1, deltaVij_old,
-          deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
+          deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT
-          use2ndOrderIntegration), deltaPij_old);
+          ), deltaPij_old);
   // rotation part has to be done properly, on manifold
-  df_dpos.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
+  df_dpos.block<3, 3>(0, 0) = numericalDerivative11<Rot3, Vector3>(
       boost::bind(&updatePreintegratedMeasurementsRot, _1, deltaVij_old,
-          deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
+          deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT
-          use2ndOrderIntegration), deltaPij_old);
+          ), deltaPij_old);
+  EXPECT(assert_equal(df_dpos, Factual.block<15, 3>(0, 3), 1e-5));
 
   // Compute expected F wrt velocities (15,3)
   Matrix df_dvel = numericalDerivative11<Vector, Vector3>(
       boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, _1,
-          deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
+          deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT
-          use2ndOrderIntegration), deltaVij_old);
+          ), deltaVij_old);
   // rotation part has to be done properly, on manifold
-  df_dvel.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
+  df_dvel.block<3, 3>(0, 0) = numericalDerivative11<Rot3, Vector3>(
       boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, _1,
-          deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
+          deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT
-          use2ndOrderIntegration), deltaVij_old);
+          ), deltaVij_old);
+  EXPECT(assert_equal(df_dvel, Factual.block<15, 3>(0, 6), 1e-5));
 
   // Compute expected F wrt angles (15,3)
   Matrix df_dangle = numericalDerivative11<Vector, Rot3>(
       boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
           deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega,
-          newDeltaT, use2ndOrderIntegration), deltaRij_old);
+          newDeltaT), deltaRij_old);
   // rotation part has to be done properly, on manifold
-  df_dangle.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Rot3>(
+  df_dangle.block<3, 3>(0, 0) = numericalDerivative11<Rot3, Rot3>(
       boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
           deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega,
-          newDeltaT, use2ndOrderIntegration), deltaRij_old);
+          newDeltaT), deltaRij_old);
+  EXPECT(assert_equal(df_dangle, Factual.block<15, 3>(0, 0), 1e-5));
 
   // Compute expected F wrt biases (15,6)
   Matrix df_dbias = numericalDerivative11<Vector, imuBias::ConstantBias>(
       boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
           deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
-          newDeltaT, use2ndOrderIntegration), bias_old);
+          newDeltaT), bias_old);
   // rotation part has to be done properly, on manifold
-  df_dbias.block<3, 6>(6, 0) =
+  df_dbias.block<3, 6>(0, 0) =
       numericalDerivative11<Rot3, imuBias::ConstantBias>(
           boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
               deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
-              newDeltaT, use2ndOrderIntegration), bias_old);
+              newDeltaT), bias_old);
-
+  EXPECT(assert_equal(df_dbias, Factual.block<15, 6>(0, 9), 1e-5));
-  Matrix Fexpected(15, 15);
-  Fexpected << df_dpos, df_dvel, df_dangle, df_dbias;
-  EXPECT(assert_equal(Fexpected, Factual,1e-5));
 
   //////////////////////////////////////////////////////////////////////////////////////////////
   // COMPUTE NUMERICAL DERIVATIVES FOR G
@@ -444,24 +436,24 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
   // Compute expected G wrt acc noise (15,3)
   Matrix df_daccNoise = numericalDerivative11<Vector, Vector3>(
       boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
-          deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT,
+          deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT
-          use2ndOrderIntegration), newMeasuredAcc);
+          ), newMeasuredAcc);
   // rotation part has to be done properly, on manifold
   df_daccNoise.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
       boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
-          deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT,
+          deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT
-          use2ndOrderIntegration), newMeasuredAcc);
+          ), newMeasuredAcc);
 
   // Compute expected G wrt gyro noise (15,3)
   Matrix df_domegaNoise = numericalDerivative11<Vector, Vector3>(
       boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
-          deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT,
+          deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT
-          use2ndOrderIntegration), newMeasuredOmega);
+          ), newMeasuredOmega);
   // rotation part has to be done properly, on manifold
   df_domegaNoise.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
       boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
-          deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT,
+          deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT
-          use2ndOrderIntegration), newMeasuredOmega);
+          ), newMeasuredOmega);
 
   // Compute expected G wrt bias random walk noise (15,6)
   Matrix df_rwBias(15, 6); // random walk on the bias does not appear in the first 9 entries
@@ -472,13 +464,13 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
   Matrix df_dinitBias = numericalDerivative11<Vector, imuBias::ConstantBias>(
       boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
           deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
-          newDeltaT, use2ndOrderIntegration), bias_old);
+          newDeltaT), bias_old);
   // rotation part has to be done properly, on manifold
   df_dinitBias.block<3, 6>(6, 0) = numericalDerivative11<Rot3,
       imuBias::ConstantBias>(
       boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
           deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
-          newDeltaT, use2ndOrderIntegration), bias_old);
+          newDeltaT), bias_old);
   df_dinitBias.block<6, 6>(9, 0) = Z_6x6; // only has to influence first 9 rows
 
   Matrix Gexpected(15, 21);