Combined two methods and renamed static method

gtsam/navigation/CombinedImuFactor.cpp

@@ -70,7 +70,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
   // Update preintegrated measurements.
   Matrix9 A;  // overall Jacobian wrt preintegrated measurements (df/dx)
   Matrix93 B, C;
-  PreintegrationBase::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
+  updatedPreintegrated(measuredAcc, measuredOmega, dt, &A, &B, &C);
 
   // Update preintegrated measurements covariance: as in [2] we consider a first
   // order propagation that can be seen as a prediction phase in an EKF

gtsam/navigation/ImuFactor.cpp

@@ -55,7 +55,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
   // Update preintegrated measurements (also get Jacobian)
   Matrix9 A;  // overall Jacobian wrt preintegrated measurements (df/dx)
   Matrix93 B, C;
-  PreintegrationBase::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
+  updatedPreintegrated(measuredAcc, measuredOmega, dt, &A, &B, &C);
 
   // first order covariance propagation:
   // as in [2] we consider a first order propagation that can be seen as a

gtsam/navigation/PreintegrationBase.cpp

@@ -116,7 +116,7 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
 
 //------------------------------------------------------------------------------
 // See extensive discussion in ImuFactor.lyx
-Vector9 PreintegrationBase::UpdateEstimate(const Vector3& a_body,
+Vector9 PreintegrationBase::UpdatePreintegrated(const Vector3& a_body,
                                            const Vector3& w_body, double dt,
                                            const Vector9& preintegrated,
                                            OptionalJacobian<9, 9> A,
@@ -178,43 +178,31 @@ Vector9 PreintegrationBase::UpdateEstimate(const Vector3& a_body,
 }
 
 //------------------------------------------------------------------------------
-Vector9 PreintegrationBase::updatedPreintegrated(const Vector3& measuredAcc,
+void PreintegrationBase::updatedPreintegrated(const Vector3& measuredAcc,
-                                                 const Vector3& measuredOmega,
+                                              const Vector3& measuredOmega,
-                                                 double dt, Matrix9* A,
+                                              double dt, Matrix9* A,
-                                                 Matrix93* B, Matrix93* C) const {
+                                              Matrix93* B, Matrix93* C) {
   // Correct for bias in the sensor frame
-  Vector3 unbiasedAcc = biasHat_.correctAccelerometer(measuredAcc);
+  Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
-  Vector3 unbiasedOmega = biasHat_.correctGyroscope(measuredOmega);
+  Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
 
-  if (!p().body_P_sensor) {
+  // Possibly correct for sensor pose
-    return UpdateEstimate(unbiasedAcc, unbiasedOmega, dt, preintegrated_, A, B,
+  Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
-                          C);
+  if (p().body_P_sensor)
-  } else {
+    boost::tie(acc, omega) = correctMeasurementsBySensorPose( acc, omega,
-    // More complicated derivatives in case of sensor displacement
+        D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
-    Matrix3 D_correctedAcc_unbiasedAcc, D_correctedAcc_unbiasedOmega,
-        D_correctedOmega_unbiasedOmega;
-    auto corrected = correctMeasurementsBySensorPose(
-        unbiasedAcc, unbiasedOmega, D_correctedAcc_unbiasedAcc,
-        D_correctedAcc_unbiasedOmega, D_correctedOmega_unbiasedOmega);
 
-    const Vector9 updated = UpdateEstimate(corrected.first, corrected.second, dt,
+    // Do update
-                                           preintegrated_, A, B, C);
-
-    *C *= D_correctedOmega_unbiasedOmega;
-    if (!p().body_P_sensor->translation().vector().isZero())
-      *C += *B* D_correctedAcc_unbiasedOmega;
-    *B *= D_correctedAcc_unbiasedAcc;  // NOTE(frank): needs to be last
-    return updated;
-  }
-}
-
-//------------------------------------------------------------------------------
-void PreintegrationBase::update(const Vector3& measuredAcc,
-                                const Vector3& measuredOmega, double dt,
-                                Matrix9* A, Matrix93* B, Matrix93* C) {
-  // Do update
   deltaTij_ += dt;
-  preintegrated_ = updatedPreintegrated(measuredAcc, measuredOmega, dt, A, B, C);
+  preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C);
+
+  if (p().body_P_sensor) {
+    // More complicated derivatives in case of non-trivial sensor pose
+    *C *= D_correctedOmega_omega;
+    if (!p().body_P_sensor->translation().vector().isZero())
+      *C += *B* D_correctedAcc_omega;
+    *B *= D_correctedAcc_acc;  // NOTE(frank): needs to be last
+  }
 
   // D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias
   preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B);

gtsam/navigation/PreintegrationBase.h

@@ -172,23 +172,20 @@ public:
       OptionalJacobian<3, 3> D_correctedOmega_unbiasedOmega = boost::none) const;
 
   // Update integrated vector on tangent manifold preintegrated with acceleration
-  // readings a_body and gyro readings w_body, bias-corrected in body frame.
+  // Static, functional version.
-  static Vector9 UpdateEstimate(const Vector3& a_body, const Vector3& w_body,
+  static Vector9 UpdatePreintegrated(const Vector3& a_body,
-                                double dt, const Vector9& preintegrated,
+                                     const Vector3& w_body, double dt,
-                                OptionalJacobian<9, 9> A = boost::none,
+                                     const Vector9& preintegrated,
-                                OptionalJacobian<9, 3> B = boost::none,
+                                     OptionalJacobian<9, 9> A = boost::none,
-                                OptionalJacobian<9, 3> C = boost::none);
+                                     OptionalJacobian<9, 3> B = boost::none,
-
+                                     OptionalJacobian<9, 3> C = boost::none);
-  /// Calculate the updated preintegrated measurement, does not modify
-  /// It takes measured quantities in the j frame
-  Vector9 updatedPreintegrated(const Vector3& measuredAcc,
-                               const Vector3& measuredOmega, double dt,
-                               Matrix9* A, Matrix93* B, Matrix93* C) const;
 
   /// Update preintegrated measurements and get derivatives
   /// It takes measured quantities in the j frame
-  void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
+  /// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose
-              const double deltaT, Matrix9* A, Matrix93* B, Matrix93* C);
+  void updatedPreintegrated(const Vector3& measuredAcc,
+                            const Vector3& measuredOmega, const double deltaT,
+                            Matrix9* A, Matrix93* B, Matrix93* C);
 
   /// Given the estimate of the bias, return a NavState tangent vector
   /// summarizing the preintegrated IMU measurements so far

gtsam/navigation/tests/testPreintegrationBase.cpp

@@ -39,7 +39,7 @@ static boost::shared_ptr<PreintegrationBase::Params> defaultParams() {
 }
 
 Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) {
-  return PreintegrationBase::UpdateEstimate(a, w, kDt, zeta);
+  return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta);
 }
 
 /* ************************************************************************* */
@@ -50,7 +50,7 @@ TEST(PreintegrationBase, UpdateEstimate1) {
   zeta.setZero();
   Matrix9 aH1;
   Matrix93 aH2, aH3;
-  pim.UpdateEstimate(acc, omega, kDt, zeta, aH1, aH2, aH3);
+  pim.UpdatePreintegrated(acc, omega, kDt, zeta, aH1, aH2, aH3);
   EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-9));
   EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-9));
   EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
@@ -64,7 +64,7 @@ TEST(PreintegrationBase, UpdateEstimate2) {
   zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3;
   Matrix9 aH1;
   Matrix93 aH2, aH3;
-  pim.UpdateEstimate(acc, omega, kDt, zeta, aH1, aH2, aH3);
+  pim.UpdatePreintegrated(acc, omega, kDt, zeta, aH1, aH2, aH3);
   // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
   EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3));
   EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-8));