Rationalize predict

gtsam/navigation/CombinedImuFactor.h

@@ -226,17 +226,13 @@ public:
       boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
           boost::none, boost::optional<Matrix&> H6 = boost::none) const;
 
-  /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */
+  // @deprecated The following function has been deprecated, use PreintegrationBase::predict
   static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
       Vector3& vel_j, const imuBias::ConstantBias& bias_i,
       const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity,
-      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
+      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) {
-      boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
+    PoseVelocityBias PVB = PIM.predict(pose_i, vel_i, bias_i, gravity,
-      boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
+        omegaCoriolis, use2ndOrderCoriolis);
-    PoseVelocityBias PVB(
-        PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis,
-            use2ndOrderCoriolis, deltaPij_biascorrected_out,
-            deltaVij_biascorrected_out));
     pose_j = PVB.pose;
     vel_j = PVB.velocity;
   }

gtsam/navigation/ImuFactor.h

@@ -206,17 +206,13 @@ public:
       boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
           boost::none, boost::optional<Matrix&> H5 = boost::none) const;
 
-  /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */
+  /// @deprecated The following function has been deprecated, use PreintegrationBase::predict
   static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
       Vector3& vel_j, const imuBias::ConstantBias& bias_i,
       const PreintegratedMeasurements& PIM, const Vector3& gravity,
-      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
+      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) {
-      boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
+    PoseVelocityBias PVB = PIM.predict(pose_i, vel_i, bias_i, gravity,
-      boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
+        omegaCoriolis, use2ndOrderCoriolis);
-    PoseVelocityBias PVB(
-        PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis,
-            use2ndOrderCoriolis, deltaPij_biascorrected_out,
-            deltaVij_biascorrected_out));
     pose_j = PVB.pose;
     vel_j = PVB.velocity;
   }

gtsam/navigation/PreintegrationBase.cpp

@@ -146,57 +146,50 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
 
 /// Predict state at time j
 //------------------------------------------------------------------------------
-PoseVelocityBias PreintegrationBase::predict(
+PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
-    const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
+    const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
-    const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis,
+    const Vector3& gravity, const Vector3& omegaCoriolis,
-    boost::optional<Vector3&> deltaPij_biascorrected_out,
+    const Rot3& deltaRij_biascorrected, const Vector3& deltaPij_biascorrected,
-    boost::optional<Vector3&> deltaVij_biascorrected_out) const {
+    const Vector3& deltaVij_biascorrected,
-
+    const bool use2ndOrderCoriolis) const {
-  const imuBias::ConstantBias biasIncr = bias_i - biasHat();
-  const Vector3& biasAccIncr = biasIncr.accelerometer();
-  const Vector3& biasOmegaIncr = biasIncr.gyroscope();
-
-  const Rot3& Rot_i = pose_i.rotation();
-  const Matrix3 Rot_i_matrix = Rot_i.matrix();
-  const Vector3 pos_i = pose_i.translation().vector();
-
-  // Predict state at time j
-  /* ---------------------------------------------------------------------------------------------*/
-  const Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr
-      + delPdelBiasOmega() * biasOmegaIncr;
-  if (deltaPij_biascorrected_out)  // if desired, store this
-    *deltaPij_biascorrected_out = deltaPij_biascorrected;
 
   const double dt = deltaTij(), dt2 = dt * dt;
-  Vector3 pos_j = pos_i + Rot_i_matrix * deltaPij_biascorrected + vel_i * dt
-      - omegaCoriolis.cross(vel_i) * dt2  // Coriolis term - we got rid of the 2 wrt ins paper
-  + 0.5 * gravity * dt2;
 
-  const Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr
+  // Rotation
-      + delVdelBiasOmega() * biasOmegaIncr;
+  const Matrix3 Ri = pose_i.rotation().matrix();
-  if (deltaVij_biascorrected_out)  // if desired, store this
+  const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected);
-    *deltaVij_biascorrected_out = deltaVij_biascorrected;
+  const Vector3 dR = biascorrectedOmega
+      - Ri.transpose() * omegaCoriolis * dt; // Coriolis term
 
-  Vector3 vel_j = Vector3(vel_i + Rot_i_matrix * deltaVij_biascorrected -
+  // Translation
-                          2 * omegaCoriolis.cross(vel_i) * dt  // Coriolis term
+  Vector3 dP = Ri * deltaPij_biascorrected + vel_i * dt + 0.5 * gravity * dt2
-                          + gravity * dt);
+      - omegaCoriolis.cross(vel_i) * dt2; // Coriolis term - we got rid of the 2 wrt INS paper
+
+  // Velocity
+  Vector3 dV = Ri * deltaVij_biascorrected + gravity * dt
+      - 2 * omegaCoriolis.cross(vel_i) * dt; // Coriolis term
 
   if (use2ndOrderCoriolis) {
-    pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt2;  // for position
+    Vector3 v = omegaCoriolis.cross(omegaCoriolis.cross(pose_i.translation().vector()));
-    vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt;  // for velocity
+    dP -= 0.5 * v * dt2;
+    dV -= v * dt;
   }
 
-  const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
+  // TODO(frank): pose update below is separate expmap for R,t. Is that kosher?
-  // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr)
+  const Pose3 pose_j = Pose3(pose_i.rotation().expmap(dR), pose_i.translation() + Point3(dP));
+  return PoseVelocityBias(pose_j, vel_i + dV, bias_i); // bias is predicted as a constant
+}
 
-  const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected);
+/// Predict state at time j
-  const Vector3 correctedOmega = biascorrectedOmega
+//------------------------------------------------------------------------------
-      - Rot_i_matrix.transpose() * omegaCoriolis * dt;  // Coriolis term
+PoseVelocityBias PreintegrationBase::predict(
-  const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
+    const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
-  const Rot3 Rot_j = Rot_i.compose(correctedDeltaRij);
+    const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) const {
-
+  const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
-  const Pose3 pose_j = Pose3(Rot_j, Point3(pos_j));
+  return predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis,
-  return PoseVelocityBias(pose_j, vel_j, bias_i);  // bias is predicted as a constant
+      biascorrectedDeltaRij(biasIncr.gyroscope()),
+      biascorrectedDeltaPij(biasIncr), biascorrectedDeltaVij(biasIncr),
+      use2ndOrderCoriolis);
 }
 
 /// Compute errors w.r.t. preintegrated measurements and Jacobians wrpt pose_i, vel_i, bias_i, pose_j, bias_j
@@ -213,9 +206,6 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
                                                      OptionalJacobian<9, 3> H4,
                                                      OptionalJacobian<9, 6> H5) const {
 
-  // We need the mismatch w.r.t. the biases used for preintegration
-  const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
-
   // we give some shorter name to rotations and translations
   const Rot3& Ri = pose_i.rotation();
   const Matrix3 Ri_transpose_matrix = Ri.transpose();
@@ -225,10 +215,13 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
 
   // Evaluate residual error, according to [3]
   /* ---------------------------------------------------------------------------------------------------- */
-  Vector3 deltaPij_biascorrected, deltaVij_biascorrected;
+  const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
-  PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis,
+  const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasIncr.gyroscope());
-                                              use2ndOrderCoriolis, deltaPij_biascorrected,
+  const Vector3 deltaPij_biascorrected = biascorrectedDeltaPij(biasIncr);
-                                              deltaVij_biascorrected);
+  const Vector3 deltaVij_biascorrected = biascorrectedDeltaVij(biasIncr);
+  PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity,
+      omegaCoriolis, deltaRij_biascorrected, deltaPij_biascorrected,
+      deltaVij_biascorrected, use2ndOrderCoriolis);
 
   // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
   const Vector3 fp = Ri_transpose_matrix * (pos_j - predictedState_j.pose.translation().vector());
@@ -242,8 +235,9 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
   // 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
+  // TODO(frank): biascorrectedThetaRij calculates deltaRij_biascorrected, which we already have
   Matrix3 D_cThetaRij_biasOmegaIncr;
-  const Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr,
+  const Vector3 biascorrectedOmega = biascorrectedThetaRij(biasIncr.gyroscope(),
                                                            H5 ? &D_cThetaRij_biasOmegaIncr : 0);
 
   // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration

gtsam/navigation/PreintegrationBase.h

@@ -138,12 +138,27 @@ class PreintegrationBase : public PreintegratedRotation {
                                               Vector3& correctedOmega,
                                               boost::optional<const Pose3&> body_P_sensor);
 
+  Vector3 biascorrectedDeltaPij(const imuBias::ConstantBias& biasIncr) const {
+    return deltaPij_ + delPdelBiasAcc_ * biasIncr.accelerometer()
+        + delPdelBiasOmega_ * biasIncr.gyroscope();
+  }
+
+  Vector3 biascorrectedDeltaVij(const imuBias::ConstantBias& biasIncr) const {
+    return deltaVij_ + delVdelBiasAcc_ * biasIncr.accelerometer()
+        + delVdelBiasOmega_ * biasIncr.gyroscope();
+  }
+
+  /// Predict state at time j, with bias-corrected quantities given
+  PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
+      const imuBias::ConstantBias& bias_i, const Vector3& gravity, const Vector3& omegaCoriolis,
+      const Rot3& deltaRij_biascorrected,
+      const Vector3& deltaPij_biascorrected, const Vector3& deltaVij_biascorrected,
+      const bool use2ndOrderCoriolis = false) const;
+
   /// Predict state at time j
-  PoseVelocityBias predict(
+  PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
-      const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
+      const imuBias::ConstantBias& bias_i, const Vector3& gravity,
-      const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
+      const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const;
-      boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
-      boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) const;
 
   /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
   Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j,