Use PreintegratedRotation::integrateMeasurement

gtsam/navigation/AHRSFactor.cpp

@@ -50,31 +50,9 @@ void PreintegratedAhrsMeasurements::resetIntegration() {
 void PreintegratedAhrsMeasurements::integrateMeasurement(
     const Vector3& measuredOmega, double deltaT) {
 
-  // First we compensate the measurements for the bias
-  Vector3 correctedOmega = measuredOmega - biasHat_;
-
-  // Then compensate for sensor-body displacement: we express the quantities
-  // (originally in the IMU frame) into the body frame
-  if (p().body_P_sensor) {
-    Matrix3 body_R_sensor = p().body_P_sensor->rotation().matrix();
-    // rotation rate vector in the body frame
-    correctedOmega = body_R_sensor * correctedOmega;
-  }
-
-  // rotation vector describing rotation increment computed from the
-  // current rotation rate measurement
-  const Vector3 theta_incr = correctedOmega * deltaT;
-  Matrix3 D_incrR_integratedOmega;
-  const Rot3 incrR = Rot3::Expmap(theta_incr, D_incrR_integratedOmega); // expensive !!
-
-  // Update Jacobian
-  const Matrix3 incrRt = incrR.transpose();
-  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT;
-
-  // Update rotation and deltaTij.
-  Matrix3 Fr; // Jacobian of the update
-  deltaRij_ = deltaRij_.compose(incrR, Fr);
-  deltaTij_ += deltaT;
+  Matrix3 D_incrR_integratedOmega, Fr;
+  PreintegratedRotation::integrateMeasurement(measuredOmega,
+      biasHat_, deltaT, &D_incrR_integratedOmega, &Fr);
 
   // first order uncertainty propagation
   // the deltaT allows to pass from continuous time noise to discrete time noise

gtsam/navigation/ImuFactor.cpp

@@ -48,6 +48,7 @@ void PreintegratedImuMeasurements::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)

gtsam/navigation/PreintegrationBase.cpp

@@ -63,27 +63,40 @@ void PreintegrationBase::updatePreintegratedMeasurements(
     const Vector3& measuredAcc, const Vector3& measuredOmega,
     const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* F) {
 
-  Matrix3 D_Rij_incrR;
-
   // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
   // (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
 
-  Vector3 correctedAcc, correctedOmega;
-  boost::tie(correctedAcc, correctedOmega) =
-      correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega);
+  // Correct for bias in the sensor frame
+  Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
 
-  // rotation vector describing rotation increment computed from the current rotation rate measurement
-  const Vector3 integratedOmega = correctedOmega * deltaT;
-  const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // rotation increment computed from the current rotation rate measurement
+  // Compensate for sensor-body displacement if needed: we express the quantities
+  // (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
+    Matrix3 bRs = p().body_P_sensor->rotation().matrix();
+    Vector3 s_correctedOmega = biasHat_.correctGyroscope(measuredOmega);
+    Vector3 b_correctedOmega = bRs * s_correctedOmega; // rotation rate vector in the body frame
+
+    // Correct acceleration
+    Vector3 b_arm = p().body_P_sensor->translation().vector();
+    Vector3 b_velocity_bs = b_correctedOmega.cross(b_arm); // magnitude: omega * arm
+    // Subtract out the the centripetal acceleration from the measured one
+    // to get linear acceleration vector in the body frame:
+    correctedAcc = bRs * correctedAcc - b_correctedOmega.cross(b_velocity_bs);
+  }
 
   // Calculate acceleration in *current* i frame, i.e., before rotation update below
   Matrix3 D_acc_R;
   const Matrix3 dRij = deltaRij_.matrix(); // expensive
   const Vector3 i_acc = deltaRij_.rotate(correctedAcc, F ? &D_acc_R : 0);
+  const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * delRdelBiasOmega_;
+
+  Matrix3 D_Rij_incrR;
+  PreintegratedRotation::integrateMeasurement(measuredOmega,
+      biasHat_.gyroscope(), deltaT, D_incrR_integratedOmega, &D_Rij_incrR);
 
   double dt22 = 0.5 * deltaT * deltaT;
-  deltaTij_ += deltaT;
-  deltaRij_ = deltaRij_.compose(incrR, F ? &D_Rij_incrR : 0);
   deltaPij_ += dt22 * i_acc + deltaT * deltaVij_;
   deltaVij_ += deltaT * i_acc;
 
@@ -92,38 +105,10 @@ void PreintegrationBase::updatePreintegratedMeasurements(
   dt22 * D_acc_R, I_3x3, I_3x3 * deltaT, // pos
   deltaT * D_acc_R, Z_3x3, I_3x3; // vel
 
-  const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * delRdelBiasOmega_;
   delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - dt22 * dRij;
   delPdelBiasOmega_ += deltaT * delVdelBiasOmega_ + dt22 * temp;
   delVdelBiasAcc_ += -dRij * deltaT;
   delVdelBiasOmega_ += temp * deltaT;
-  const Matrix3 incrRt = incrR.transpose();
-  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
-      - *D_incrR_integratedOmega * deltaT;
-}
-
-std::pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
-    const Vector3& measuredAcc, const Vector3& measuredOmega) const {
-  // Correct for bias in the sensor frame
-  Vector3 s_correctedAcc, s_correctedOmega;
-  s_correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
-  s_correctedOmega = biasHat_.correctGyroscope(measuredOmega);
-
-  // Compensate for sensor-body displacement if needed: we express the quantities
-  // (originally in the IMU frame) into the body frame
-  // Equations below assume the "body" frame is the CG
-  if (p().body_P_sensor) {
-    Matrix3 bRs = p().body_P_sensor->rotation().matrix();
-    Vector3 b_arm = p().body_P_sensor->translation().vector();
-    Vector3 b_correctedOmega = bRs * s_correctedOmega; // rotation rate vector in the body frame
-    Vector3 b_velocity_bs = b_correctedOmega.cross(b_arm); // magnitude: omega * arm
-    // Subtract out the the centripetal acceleration from the measured one
-    // to get linear acceleration vector in the body frame:
-    Vector3 b_correctedAcc = bRs * s_correctedAcc
-        - b_correctedOmega.cross(b_velocity_bs);
-    return std::make_pair(b_correctedAcc, b_correctedOmega);
-  } else
-    return std::make_pair(s_correctedAcc, s_correctedOmega);
 }
 
 //------------------------------------------------------------------------------

gtsam/navigation/PreintegrationBase.h

@@ -174,11 +174,7 @@ public:
       const Vector3& measuredOmega, const double deltaT,
       Matrix3* D_incrR_integratedOmega, Matrix9* F);
 
-  std::pair<Vector3, Vector3>
-  correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
-      const Vector3& measuredOmega) const;
-
-    /// Given the estimate of the bias, return a NavState tangent vector
+  /// Given the estimate of the bias, return a NavState tangent vector
   /// summarizing the preintegrated IMU measurements so far
   Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
       OptionalJacobian<9, 6> H = boost::none) const;