Refactored arm correction but there is still a difference in regression values. Did I introduce a bug?

gtsam/navigation/ImuFactor.cpp

@@ -103,7 +103,8 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements(
 void PreintegratedImuMeasurements::integrateMeasurement(
     const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
     const Pose3& body_P_sensor) {
-  p_->body_P_sensor = body_P_sensor;
+  // modify parameters to accommodate deprecated method:-(
+  p_->body_P_sensor.reset(body_P_sensor);
   integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 }
 

gtsam/navigation/PreintegrationBase.cpp

@@ -61,9 +61,8 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
 }
 
 //------------------------------------------------------------------------------
-NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
+std::pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
-    const Vector3& j_measuredOmega, const double dt, OptionalJacobian<9, 9> F,
+    const Vector3& j_measuredAcc, const Vector3& j_measuredOmega) const {
-    OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const {
 
   // Correct for bias in the sensor frame
   Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc);
@@ -73,17 +72,34 @@ NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
   // (originally in the IMU frame) into the body frame
   // Equations below assume the "body" frame is the CG
   if (p().body_P_sensor) {
-    // Correct omega: slight duplication as this is also done in integrateMeasurement below
+    // Correct omega to rotation rate vector in the body frame
-    Matrix3 bRs = p().body_P_sensor->rotation().matrix();
+    const Matrix3 bRs = p().body_P_sensor->rotation().matrix();
-    j_correctedOmega = bRs * j_correctedOmega; // rotation rate vector in the body frame
+    j_correctedOmega = bRs * j_correctedOmega;
 
     // Correct acceleration
-    Vector3 b_arm = p().body_P_sensor->translation().vector();
+    j_correctedAcc = bRs * j_correctedAcc;
-    Vector3 b_velocity_bs = j_correctedOmega.cross(b_arm); // magnitude: omega * arm
+    const Vector3 b_arm = p().body_P_sensor->translation().vector();
+    if (!b_arm.isZero()) {
       // Subtract out the the centripetal acceleration from the measured one
       // to get linear acceleration vector in the body frame:
-    j_correctedAcc = bRs * j_correctedAcc - j_correctedOmega.cross(b_velocity_bs);
+      const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega);
+      const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
+      j_correctedAcc -= body_Omega_body * b_velocity_bs;
     }
+  }
+
+  // Do update in one fell swoop
+  return make_pair(j_correctedAcc, j_correctedOmega);
+}
+
+//------------------------------------------------------------------------------
+NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
+    const Vector3& j_measuredOmega, const double dt, OptionalJacobian<9, 9> F,
+    OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const {
+
+  Vector3 j_correctedAcc, j_correctedOmega;
+  boost::tie(j_correctedAcc, j_correctedOmega) =
+      correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega);
 
   // Do update in one fell swoop
   return deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, F, G1, G2);
@@ -103,9 +119,9 @@ void PreintegrationBase::update(
 
   // Update Jacobians
   // TODO(frank): we are repeating some computation here: accessible in F ?
-  // Correct for bias in the sensor frame
+  Vector3 j_correctedAcc, j_correctedOmega;
-  Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc);
+  boost::tie(j_correctedAcc, j_correctedOmega) =
-  Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega);
+      correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega);
 
   Matrix3 D_acc_R;
   oldRij.rotate(j_correctedAcc, D_acc_R);

gtsam/navigation/PreintegrationBase.h

@@ -191,6 +191,10 @@ public:
   /// check equality
   bool equals(const PreintegrationBase& other, double tol) const;
 
+  /// Subtract estimate and correct for sensor pose
+  std::pair<Vector3, Vector3> correctMeasurementsByBiasAndSensorPose(
+      const Vector3& j_measuredAcc, const Vector3& j_measuredOmega) const;
+
   /// Calculate the updated preintegrated measurement, does not modify
   /// It takes measured quantities in the j frame
   NavState updatedDeltaXij(const Vector3& j_measuredAcc,