added comments, made more elegant error evaluation for CombinedImuFactor

gtsam/navigation/CombinedImuFactor.cpp

@@ -206,21 +206,17 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
     boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
     boost::optional<Matrix&> H5, boost::optional<Matrix&> H6) const {
 
-  // Bias evolution model: random walk
-  const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer();
-  const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope();
-
-  // error wrt preintegrated position, velocity, rotation
-  Vector r_pvR(9);
-
   // if we need the jacobians
   if(H1 || H2 || H3 || H4 || H5 || H6){
-    Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR; // pvR = mnemonic: position (p), velocity (v), rotation (R)
+    Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR, Hbias_i, Hbias_j; // pvR = mnemonic: position (p), velocity (v), rotation (R)
 
-    // include errors wrt preintegrated measurements
+    // error wrt preintegrated measurements
-    r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i,
+    Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i,
         H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR);
 
+    // error wrt bias evolution model (random walk)
+    Vector6 fbias = bias_j.between(bias_i, Hbias_j, Hbias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr]
+
     if(H1) {
       H1->resize(15,6);
       H1->block<9,6>(0,0) = H1_pvR;
@@ -249,22 +245,25 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
       H5->resize(15,6);
       H5->block<9,6>(0,0) = H5_pvR;
       // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i]
-      H5->block<6,6>(0,9) = - Matrix::Identity(6,6);
+      H5->block<6,6>(0,9) = Hbias_i;
     }
     if(H6) {
       H6->resize(15,6);
       H6->block<9,6>(0,0) = Matrix::Zero(6,6);
       // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j]
-      H6->block<6,6>(0,9) = Matrix::Identity(6,6);
+      H6->block<6,6>(0,9) = Hbias_j;
     }
-    Vector r(15); r << r_pvR, fbiasAcc, fbiasOmega; // vector of size 15
+    Vector r(15); r << r_pvR, fbias; // vector of size 15
     return r;
   }
   // else, only compute the error vector:
-  // Evaluate residual error, including the model for bias evolution
+  // error wrt preintegrated measurements
-  r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i,
+  Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i,
       boost::none, boost::none, boost::none, boost::none, boost::none);
-  Vector r(15); r << r_pvR, fbiasAcc, fbiasOmega; // vector of size 15
+  // error wrt bias evolution model (random walk)
+  Vector6 fbias = bias_j.between(bias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr]
+  // overall error
+  Vector r(15); r << r_pvR, fbias; // vector of size 15
   return r;
 }
 

gtsam/navigation/ImuFactorBase.h

@@ -52,15 +52,24 @@ protected:
 
 public:
 
+  /** Default constructor - only use for serialization */
   ImuFactorBase() :
     gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)),
     body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {}
 
+  /**
+   *  Default constructor, stores basic quantities required by the Imu factors
+   * @param gravity Gravity vector expressed in the global frame
+   * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
+   * @param body_P_sensor Optional pose of the sensor frame in the body frame
+   * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
+   */
   ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis,
       boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) :
         gravity_(gravity), omegaCoriolis_(omegaCoriolis),
         body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {}
 
+  /// Methods to access class variables
   const Vector3& gravity() const { return gravity_; }
   const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
 
@@ -91,6 +100,7 @@ public:
       boost::optional<Matrix&> H3,  boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const{
 
     const double& deltaTij = preintegratedMeasurements_.deltaTij_;
+    // We need the mistmatch w.r.t. the biases used for preintegration
     const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
     const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
 
@@ -100,7 +110,7 @@ public:
     const Vector3 pos_i = pose_i.translation().vector();
     const Vector3 pos_j = pose_j.translation().vector();
 
-    // We compute factor's Jacobians
+    // Jacobian computation
     /* ---------------------------------------------------------------------------------------------------- */
     const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
     // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
@@ -114,11 +124,8 @@ public:
         Rot3::Expmap( theta_biascorrected_corioliscorrected );
 
     const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
-
     const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
-
     const Matrix3 Jtheta = -Jr_theta_bcc  * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
-
     const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
 
     if(H1) {
@@ -134,7 +141,6 @@ public:
         dfPdPi = - Rot_i.matrix();
         dfVdPi = Z_3x3;
       }
-
       (*H1) <<
           // dfP/dRi
           Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
@@ -151,7 +157,6 @@ public:
           // dfR/dPi
           Z_3x3;
     }
-
     if(H2) {
       H2->resize(9,3);
       (*H2) <<
@@ -164,7 +169,6 @@ public:
           // dfR/dVi
           Z_3x3;
     }
-
     if(H3) {
       H3->resize(9,6);
       (*H3) <<
@@ -175,7 +179,6 @@ public:
           // dfR/dPosej
           Jrinv_fRhat *  ( I_3x3 ), Z_3x3;
     }
-
     if(H4) {
       H4->resize(9,3);
       (*H4) <<
@@ -186,7 +189,6 @@ public:
           // dfR/dVj
           Z_3x3;
     }
-
     if(H5) {
       const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
       const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);