Moved two very large methods from ImuFactorBase to PreintegrationBase

gtsam/navigation/CombinedImuFactor.cpp

@@ -209,7 +209,9 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
     Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR, Hbias_i, Hbias_j; // pvR = mnemonic: position (p), velocity (v), rotation (R)
 
     // error wrt preintegrated measurements
-    Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i,
+    Vector r_pvR(9);
+    r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
+        gravity_, omegaCoriolis_, use2ndOrderCoriolis_, //
         H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR);
 
     // error wrt bias evolution model (random walk)
@@ -256,7 +258,9 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
   }
   // else, only compute the error vector:
   // error wrt preintegrated measurements
-  Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i,
+  Vector r_pvR(9);
+  r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
+      gravity_, omegaCoriolis_, use2ndOrderCoriolis_, //
       boost::none, boost::none, boost::none, boost::none, boost::none);
   // 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]

gtsam/navigation/CombinedImuFactor.h

@@ -212,14 +212,6 @@ public:
       boost::optional<Matrix&> H5 = boost::none,
       boost::optional<Matrix&> H6 = boost::none) const;
 
-  /// predicted states from IMU
-  static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
-      const imuBias::ConstantBias& bias_i,
-      const PreintegrationBase& preintegratedMeasurements,
-      const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false){
-    return ImuFactorBase::predict(pose_i, vel_i, bias_i, preintegratedMeasurements, gravity, omegaCoriolis, use2ndOrderCoriolis);
-  }
-
 private:
 
   /** Serialization function */

gtsam/navigation/ImuFactor.cpp

@@ -148,7 +148,8 @@ ImuFactor::ImuFactor(
     const Vector3& gravity, const Vector3& omegaCoriolis,
     boost::optional<const Pose3&> body_P_sensor,
     const bool use2ndOrderCoriolis) :
-        Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias),
+        Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_),
+            pose_i, vel_i, pose_j, vel_j, bias),
         ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
         _PIM_(preintegratedMeasurements) {}
 
@@ -180,13 +181,14 @@ bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const {
 }
 
 //------------------------------------------------------------------------------
-Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
+Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
-    const imuBias::ConstantBias& bias_i,
+    const Pose3& pose_j, const Vector3& vel_j,
-    boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2,
+    const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H3,  boost::optional<Matrix&> H4,
+    boost::optional<Matrix&> H2, boost::optional<Matrix&> H3,
-    boost::optional<Matrix&> H5) const{
+    boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const {
 
-  return ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i, H1, H2, H3, H4, H5);
+  return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
+      gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5);
 }
 
 } /// namespace gtsam

gtsam/navigation/ImuFactor.h

@@ -197,13 +197,6 @@ public:
       boost::optional<Matrix&> H4 = boost::none,
       boost::optional<Matrix&> H5 = boost::none) const;
 
-  /// predicted states from IMU
-  static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
-      const imuBias::ConstantBias& bias_i, const PreintegrationBase& preintegratedMeasurements,
-      const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false){
-    return ImuFactorBase::predict(pose_i, vel_i, bias_i, preintegratedMeasurements, gravity, omegaCoriolis, use2ndOrderCoriolis);
-  }
-
   private:
 
   /** Serialization function */

gtsam/navigation/ImuFactorBase.h

@@ -27,20 +27,6 @@
 
 namespace gtsam {
 
-/**
- * Struct to hold all state variables of returned by Predict function
- */
-struct PoseVelocityBias {
-  Pose3 pose;
-  Vector3 velocity;
-  imuBias::ConstantBias bias;
-
-  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
-      const imuBias::ConstantBias _bias) :
-        pose(_pose), velocity(_velocity), bias(_bias) {
-  }
-};
-
 class ImuFactorBase {
 
 protected:
@@ -93,184 +79,6 @@ public:
         (body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_)));
   }
 
-  /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
-  //------------------------------------------------------------------------------
-  Vector computeErrorAndJacobians(const PreintegrationBase& _PIM, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
-      const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2,
-      boost::optional<Matrix&> H3,  boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const{
-
-    const double& deltaTij = _PIM.deltaTij();
-    // We need the mistmatch w.r.t. the biases used for preintegration
-    const Vector3 biasAccIncr = bias_i.accelerometer() - _PIM.biasHat().accelerometer();
-    const Vector3 biasOmegaIncr = bias_i.gyroscope() - _PIM.biasHat().gyroscope();
-
-    // we give some shorter name to rotations and translations
-    const Rot3& Rot_i = pose_i.rotation();
-    const Rot3& Rot_j = pose_j.rotation();
-    const Vector3& pos_j = pose_j.translation().vector();
-
-    // Jacobian computation
-    /* ---------------------------------------------------------------------------------------------------- */
-    // Get Get so<3> version of bias corrected rotation
-    // If H5 is asked for, we will need the Jacobian, which we store in H5
-    // H5 will then be corrected below to take into account the Coriolis effect
-    Vector3 theta_biascorrected =
-        _PIM.biascorrectedThetaRij(biasOmegaIncr, H5);
-
-    Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-        Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
-
-    const Rot3 deltaRij_biascorrected_corioliscorrected =
-        Rot3::Expmap( theta_biascorrected_corioliscorrected );
-
-    // TODO: these are not always needed
-    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) {
-      H1->resize(9,6);
-
-      Matrix3 dfPdPi;
-      Matrix3 dfVdPi;
-      if(use2ndOrderCoriolis_){
-        dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
-        dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
-      }
-      else{
-        dfPdPi = - Rot_i.matrix();
-        dfVdPi = Z_3x3;
-      }
-      (*H1) <<
-          // dfP/dRi
-          Rot_i.matrix() * skewSymmetric(_PIM.deltaPij()
-          + _PIM.delPdelBiasOmega() * biasOmegaIncr + _PIM.delPdelBiasAcc() * biasAccIncr),
-          // dfP/dPi
-          dfPdPi,
-          // dfV/dRi
-          Rot_i.matrix() * skewSymmetric(_PIM.deltaVij()
-          + _PIM.delVdelBiasOmega() * biasOmegaIncr + _PIM.delVdelBiasAcc() * biasAccIncr),
-          // dfV/dPi
-          dfVdPi,
-          // dfR/dRi
-          Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
-          // dfR/dPi
-          Z_3x3;
-    }
-    if(H2) {
-      H2->resize(9,3);
-      (*H2) <<
-          // dfP/dVi
-          - I_3x3 * deltaTij
-          + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij,  // Coriolis term - we got rid of the 2 wrt ins paper
-          // dfV/dVi
-          - I_3x3
-          + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
-          // dfR/dVi
-          Z_3x3;
-    }
-    if(H3) {
-      H3->resize(9,6);
-      (*H3) <<
-          // dfP/dPosej
-          Z_3x3, Rot_j.matrix(),
-          // dfV/dPosej
-          Matrix::Zero(3,6),
-          // dfR/dPosej
-          Jrinv_fRhat *  ( I_3x3 ), Z_3x3;
-    }
-    if(H4) {
-      H4->resize(9,3);
-      (*H4) <<
-          // dfP/dVj
-          Z_3x3,
-          // dfV/dVj
-          I_3x3,
-          // dfR/dVj
-          Z_3x3;
-    }
-    if(H5) {
-      // H5 by this point already contains 3*3 biascorrectedThetaRij derivative
-      const Matrix3 JbiasOmega = Jr_theta_bcc * (*H5);
-      H5->resize(9,6);
-      (*H5) <<
-          // dfP/dBias
-          - Rot_i.matrix() * _PIM.delPdelBiasAcc(),
-          - Rot_i.matrix() * _PIM.delPdelBiasOmega(),
-          // dfV/dBias
-          - Rot_i.matrix() * _PIM.delVdelBiasAcc(),
-          - Rot_i.matrix() * _PIM.delVdelBiasOmega(),
-          // dfR/dBias
-          Matrix::Zero(3,3),
-          Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
-    }
-
-    // Evaluate residual error, according to [3]
-    /* ---------------------------------------------------------------------------------------------------- */
-    PoseVelocityBias predictedState_j = ImuFactorBase::predict(pose_i, vel_i, bias_i, _PIM,
-        gravity_, omegaCoriolis_, use2ndOrderCoriolis_);
-
-    const Vector3 fp = pos_j - predictedState_j.pose.translation().vector();
-
-    const Vector3 fv = vel_j - predictedState_j.velocity;
-
-    // This is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j)
-    const Vector3 fR = Rot3::Logmap(fRhat);
-
-    Vector r(9); r << fp, fv, fR;
-    return r;
-  }
-
-  /// Predict state at time j
-  //------------------------------------------------------------------------------
-  static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
-      const imuBias::ConstantBias& bias_i,
-      const PreintegrationBase& _PIM,
-      const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){
-
-    const double& deltaTij = _PIM.deltaTij();
-    const Vector3 biasAccIncr = bias_i.accelerometer() - _PIM.biasHat().accelerometer();
-    const Vector3 biasOmegaIncr = bias_i.gyroscope() - _PIM.biasHat().gyroscope();
-
-    const Rot3& Rot_i = pose_i.rotation();
-    const Vector3& pos_i = pose_i.translation().vector();
-
-    // Predict state at time j
-    /* ---------------------------------------------------------------------------------------------------- */
-    Vector3 pos_j =  pos_i + Rot_i.matrix() * (_PIM.deltaPij()
-        + _PIM.delPdelBiasAcc() * biasAccIncr
-        + _PIM.delPdelBiasOmega() * biasOmegaIncr)
-        + vel_i * deltaTij
-        - skewSymmetric(omegaCoriolis) * 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() * (_PIM.deltaVij()
-        + _PIM.delVdelBiasAcc() * biasAccIncr
-        + _PIM.delVdelBiasOmega() * biasOmegaIncr)
-        - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
-        + gravity * deltaTij);
-
-    if(use2ndOrderCoriolis){
-      pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;  // 2nd order coriolis term for position
-      vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
-    }
-
-    const Rot3 deltaRij_biascorrected = _PIM.biascorrectedDeltaRij(biasOmegaIncr);
-    // TODO Frank says comment below does not reflect what was in code
-    // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-
-    Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-    Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-        Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
-    const Rot3 deltaRij_biascorrected_corioliscorrected =
-        Rot3::Expmap( theta_biascorrected_corioliscorrected );
-    const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
-
-    Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
-    return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
-  }
-
 };
 
 } /// namespace gtsam

gtsam/navigation/PreintegrationBase.h

@@ -26,6 +26,20 @@
 
 namespace gtsam {
 
+/**
+ * Struct to hold all state variables of returned by Predict function
+ */
+struct PoseVelocityBias {
+  Pose3 pose;
+  Vector3 velocity;
+  imuBias::ConstantBias bias;
+
+  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
+      const imuBias::ConstantBias _bias) :
+        pose(_pose), velocity(_velocity), bias(_bias) {
+  }
+};
+
 /**
  * PreintegrationBase is the base class for PreintegratedMeasurements
  * (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor).
@@ -150,6 +164,185 @@ public:
     }
   }
 
+  /// Predict state at time j
+  //------------------------------------------------------------------------------
+  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 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer();
+    const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
+
+    const Rot3& Rot_i = pose_i.rotation();
+    const Vector3& pos_i = pose_i.translation().vector();
+
+    // Predict state at time j
+    /* ---------------------------------------------------------------------------------------------------- */
+    Vector3 pos_j =  pos_i + Rot_i.matrix() * (deltaPij()
+        + delPdelBiasAcc() * biasAccIncr
+        + delPdelBiasOmega() * biasOmegaIncr)
+        + vel_i * deltaTij()
+        - skewSymmetric(omegaCoriolis) * 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()
+        + delVdelBiasAcc() * biasAccIncr
+        + delVdelBiasOmega() * biasOmegaIncr)
+        - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij()  // Coriolis term
+        + gravity * deltaTij());
+
+    if(use2ndOrderCoriolis){
+      pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij()*deltaTij();  // 2nd order coriolis term for position
+      vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij(); // 2nd order term for velocity
+    }
+
+    const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
+    // TODO Frank says comment below does not reflect what was in code
+    // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
+
+    Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
+    Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
+        Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term
+    const Rot3 deltaRij_biascorrected_corioliscorrected =
+        Rot3::Expmap( theta_biascorrected_corioliscorrected );
+    const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
+
+    Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
+    return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
+  }
+
+  /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
+  //------------------------------------------------------------------------------
+  Vector computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
+      const Pose3& pose_j, const Vector3& vel_j,
+      const imuBias::ConstantBias& bias_i, const Vector3& gravity,
+      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis,
+      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
+      boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
+      boost::optional<Matrix&> H5) const {
+
+    // We need the mistmatch w.r.t. the biases used for preintegration
+    const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer();
+    const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
+
+    // we give some shorter name to rotations and translations
+    const Rot3& Rot_i = pose_i.rotation();
+    const Rot3& Rot_j = pose_j.rotation();
+    const Vector3& pos_j = pose_j.translation().vector();
+
+    // Jacobian computation
+    /* ---------------------------------------------------------------------------------------------------- */
+    // Get Get so<3> version of bias corrected rotation
+    // If H5 is asked for, we will need the Jacobian, which we store in H5
+    // H5 will then be corrected below to take into account the Coriolis effect
+    Vector3 theta_biascorrected =
+        biascorrectedThetaRij(biasOmegaIncr, H5);
+
+    Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
+        Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term
+
+    const Rot3 deltaRij_biascorrected_corioliscorrected =
+        Rot3::Expmap( theta_biascorrected_corioliscorrected );
+
+    // TODO: these are not always needed
+    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) {
+      H1->resize(9,6);
+
+      Matrix3 dfPdPi;
+      Matrix3 dfVdPi;
+      if(use2ndOrderCoriolis){
+        dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * Rot_i.matrix() * deltaTij()*deltaTij();
+        dfVdPi = skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * Rot_i.matrix() * deltaTij();
+      }
+      else{
+        dfPdPi = - Rot_i.matrix();
+        dfVdPi = Z_3x3;
+      }
+      (*H1) <<
+          // dfP/dRi
+          Rot_i.matrix() * skewSymmetric(deltaPij()
+          + delPdelBiasOmega() * biasOmegaIncr + delPdelBiasAcc() * biasAccIncr),
+          // dfP/dPi
+          dfPdPi,
+          // dfV/dRi
+          Rot_i.matrix() * skewSymmetric(deltaVij()
+          + delVdelBiasOmega() * biasOmegaIncr + delVdelBiasAcc() * biasAccIncr),
+          // dfV/dPi
+          dfVdPi,
+          // dfR/dRi
+          Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
+          // dfR/dPi
+          Z_3x3;
+    }
+    if(H2) {
+      H2->resize(9,3);
+      (*H2) <<
+          // dfP/dVi
+          - I_3x3 * deltaTij()
+          + skewSymmetric(omegaCoriolis) * deltaTij() * deltaTij(),  // Coriolis term - we got rid of the 2 wrt ins paper
+          // dfV/dVi
+          - I_3x3
+          + 2 * skewSymmetric(omegaCoriolis) * deltaTij(), // Coriolis term
+          // dfR/dVi
+          Z_3x3;
+    }
+    if(H3) {
+      H3->resize(9,6);
+      (*H3) <<
+          // dfP/dPosej
+          Z_3x3, Rot_j.matrix(),
+          // dfV/dPosej
+          Matrix::Zero(3,6),
+          // dfR/dPosej
+          Jrinv_fRhat *  ( I_3x3 ), Z_3x3;
+    }
+    if(H4) {
+      H4->resize(9,3);
+      (*H4) <<
+          // dfP/dVj
+          Z_3x3,
+          // dfV/dVj
+          I_3x3,
+          // dfR/dVj
+          Z_3x3;
+    }
+    if(H5) {
+      // H5 by this point already contains 3*3 biascorrectedThetaRij derivative
+      const Matrix3 JbiasOmega = Jr_theta_bcc * (*H5);
+      H5->resize(9,6);
+      (*H5) <<
+          // dfP/dBias
+          - Rot_i.matrix() * delPdelBiasAcc(),
+          - Rot_i.matrix() * delPdelBiasOmega(),
+          // dfV/dBias
+          - Rot_i.matrix() * delVdelBiasAcc(),
+          - Rot_i.matrix() * delVdelBiasOmega(),
+          // dfR/dBias
+          Matrix::Zero(3,3),
+          Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
+    }
+
+    // Evaluate residual error, according to [3]
+    /* ---------------------------------------------------------------------------------------------------- */
+    PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity,
+        omegaCoriolis, use2ndOrderCoriolis);
+
+    const Vector3 fp = pos_j - predictedState_j.pose.translation().vector();
+
+    const Vector3 fv = vel_j - predictedState_j.velocity;
+
+    // This is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j)
+    const Vector3 fR = Rot3::Logmap(fRhat);
+
+    Vector r(9); r << fp, fv, fR;
+    return r;
+  }
+
   /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
   // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
   static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -290,7 +290,7 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){
     // Predict
     Pose3 x1;
     Vector3 v1(0, 0.0, 0.0);
-    PoseVelocityBias poseVelocityBias = Combinedfactor.predict(x1, v1, bias, Combined_pre_int_data, gravity, omegaCoriolis);
+    PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
     Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
     Vector3 expectedVelocity; expectedVelocity<<0,1,0;
     EXPECT(assert_equal(expectedPose, poseVelocityBias.pose));
@@ -319,7 +319,7 @@ TEST(CombinedImuFactor, PredictRotation) {
   // Predict
   Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0));
   Vector3 v(0,0,0);
-  PoseVelocityBias poseVelocityBias = Combinedfactor.predict(x,v,bias, Combined_pre_int_data, gravity, omegaCoriolis);
+  PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x,v,bias, gravity, omegaCoriolis);
   Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0));
   EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol));
 }

gtsam/navigation/tests/testImuFactor.cpp

@@ -561,7 +561,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){
     // Predict
     Pose3 x1;
     Vector3 v1(0, 0.0, 0.0);
-    PoseVelocityBias poseVelocity = factor.predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis);
+    PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
     Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
     Vector3 expectedVelocity; expectedVelocity<<0,1,0;
     EXPECT(assert_equal(expectedPose, poseVelocity.pose));
@@ -593,7 +593,7 @@ TEST(ImuFactor, PredictRotation) {
     // Predict
     Pose3 x1;
     Vector3 v1(0, 0.0, 0.0);
-    PoseVelocityBias poseVelocity = factor.predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis);
+    PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
     Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0));
     Vector3 expectedVelocity; expectedVelocity<<0,0,0;
     EXPECT(assert_equal(expectedPose, poseVelocity.pose));