New naming, old derivative code

gtsam/navigation/CombinedImuFactor.cpp

@@ -74,7 +74,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
   Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
   Matrix9 F_9x9; // overall Jacobian wrt preintegrated measurements (df/dx)
   Matrix93 G1,G2;
-  updatePreintegratedMeasurements(measuredAcc, measuredOmega, deltaT,
+  PreintegrationBase::update(measuredAcc, measuredOmega, deltaT,
       &D_incrR_integratedOmega, &F_9x9, &G1, &G2);
 
   // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that

gtsam/navigation/ImuFactor.cpp

@@ -58,7 +58,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
   Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
   Matrix93 G1, G2;
   Matrix3 D_incrR_integratedOmega;
-  updatePreintegratedMeasurements(measuredAcc, measuredOmega, dt,
+  PreintegrationBase::update(measuredAcc, measuredOmega, dt,
       &D_incrR_integratedOmega, &F, &G1, &G2);
 
   // first order covariance propagation:
@@ -67,10 +67,20 @@ void PreintegratedImuMeasurements::integrateMeasurement(
   // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G'
   // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise
   // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
+#ifdef OLD_JACOBIAN_CALCULATION
+  Matrix9 G;
+  G << G1, Gi, G2;
+  Matrix9 Cov;
+  Cov << p().accelerometerCovariance / dt, Z_3x3, Z_3x3,
+      Z_3x3, p().integrationCovariance * dt, Z_3x3,
+      Z_3x3, Z_3x3, p().gyroscopeCovariance / dt;
+  preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G * Cov * G.transpose();
+#else
   preintMeasCov_ = F * preintMeasCov_ * F.transpose()
       + G1 * (p().accelerometerCovariance / dt) * G1.transpose()
       + Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt)
       + G2 * (p().gyroscopeCovariance / dt) * G2.transpose();
+#endif
 }
 
 //------------------------------------------------------------------------------

gtsam/navigation/PreintegrationBase.cpp

@@ -61,9 +61,9 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
 }
 
 //------------------------------------------------------------------------------
-NavState PreintegrationBase::update(const Vector3& measuredAcc,
-    const Vector3& measuredOmega, const double dt, Matrix9* F, Matrix93* G1,
-    Matrix93* G2) const {
+NavState PreintegrationBase::updatedDeltaXij(const Vector3& measuredAcc,
+    const Vector3& measuredOmega, const double dt, OptionalJacobian<9, 9> F,
+    OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const {
 
   // Correct for bias in the sensor frame
   Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
@@ -90,7 +90,7 @@ NavState PreintegrationBase::update(const Vector3& measuredAcc,
 }
 
 //------------------------------------------------------------------------------
-void PreintegrationBase::updatePreintegratedMeasurements(
+void PreintegrationBase::update(
     const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
     Matrix3* D_incrR_integratedOmega, Matrix9* F, Matrix93* G1, Matrix93* G2) {
 
@@ -99,7 +99,7 @@ void PreintegrationBase::updatePreintegratedMeasurements(
 
   // Do update
   deltaTij_ += dt;
-  deltaXij_ = update(measuredAcc, measuredOmega, dt, F, G1, G2); // functional
+  deltaXij_ = updatedDeltaXij(measuredAcc, measuredOmega, dt, F, G1, G2); // functional
 
   // Update Jacobians
   // TODO(frank): we are repeating some computation here: accessible in F ?
@@ -139,7 +139,7 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
   Vector9 xi;
   Matrix3 D_dR_correctedRij;
   // TODO(frank): could line below be simplified? It is equivalent to
-  //   LogMap(deltaRij_.compose(Expmap(delRdelBiasOmega_ * biasIncr.gyroscope())))
+  //   LogMap(deltaRij_.compose(Expmap(biasInducedOmega)))
   NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0);
   NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer()
       + delPdelBiasOmega_ * biasIncr.gyroscope();

gtsam/navigation/PreintegrationBase.h

@@ -146,6 +146,9 @@ public:
   }
 
   /// getters
+  const NavState& deltaXij() const {
+    return deltaXij_;
+  }
   const double& deltaTij() const {
     return deltaTij_;
   }
@@ -189,13 +192,15 @@ public:
   bool equals(const PreintegrationBase& other, double tol) const;
 
   /// Calculate the updated preintegrated measurement, does not modify
-  NavState update(const Vector3& measuredAcc, const Vector3& measuredOmega,
-      const double dt, Matrix9* F, Matrix93* G1, Matrix93* G2) const;
+  NavState updatedDeltaXij(const Vector3& measuredAcc,
+      const Vector3& measuredOmega, const double dt, OptionalJacobian<9, 9> F =
+          boost::none, OptionalJacobian<9, 3> G1 = boost::none,
+      OptionalJacobian<9, 3> G2 = boost::none) const;
 
-  /// Update preintegrated measurements
-  void updatePreintegratedMeasurements(const Vector3& measuredAcc,
-      const Vector3& measuredOmega, const double deltaT,
-      Matrix3* D_incrR_integratedOmega, Matrix9* F, Matrix93* G1, Matrix93* G2);
+  /// Update preintegrated measurements and get derivatives
+  void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
+      const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* F,
+      Matrix93* G1, Matrix93* G2);
 
   /// Given the estimate of the bias, return a NavState tangent vector
   /// summarizing the preintegrated IMU measurements so far