From 8aca431913ca9a8ec2068c565740d9d31891582c Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Fri, 31 Jul 2015 15:31:45 -0700
Subject: [PATCH] const update method

---
 gtsam/navigation/ImuFactor.cpp          |  5 ++---
 gtsam/navigation/PreintegrationBase.cpp | 30 ++++++++++++++++---------
 gtsam/navigation/PreintegrationBase.h   |  4 ++++
 3 files changed, 26 insertions(+), 13 deletions(-)

diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp
index 2f5861de5..279c95d3c 100644
--- a/gtsam/navigation/ImuFactor.cpp
+++ b/gtsam/navigation/ImuFactor.cpp
@@ -67,11 +67,10 @@ void PreintegratedImuMeasurements::integrateMeasurement(
   static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished();
 
   // Update preintegrated measurements (also get Jacobian)
-  Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
   Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
   Matrix93 G1, G2;
-  updatePreintegratedMeasurements(measuredAcc, measuredOmega, dt,
-      &D_incrR_integratedOmega, &F, &G1, &G2);
+  Matrix3 D_incrR_integratedOmega;
+  updatePreintegratedMeasurements(measuredAcc, measuredOmega, dt, &D_incrR_integratedOmega, &F, &G1, &G2);
 
   // first order covariance propagation:
   // as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF
diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp
index 2373c474e..0d51e59f9 100644
--- a/gtsam/navigation/PreintegrationBase.cpp
+++ b/gtsam/navigation/PreintegrationBase.cpp
@@ -60,12 +60,10 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
       && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
 }
 
-void PreintegrationBase::updatePreintegratedMeasurements(
-    const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
-    Matrix3* D_incrR_integratedOmega, Matrix9* F, Matrix93* G1, Matrix93* G2) {
-
-  // 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).
+//------------------------------------------------------------------------------
+NavState PreintegrationBase::update(const Vector3& measuredAcc,
+    const Vector3& measuredOmega, const double dt, Matrix9* F, Matrix93* G1,
+    Matrix93* G2) const {
 
   // Correct for bias in the sensor frame
   Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
@@ -87,15 +85,28 @@ void PreintegrationBase::updatePreintegratedMeasurements(
     correctedAcc = bRs * correctedAcc - correctedOmega.cross(b_velocity_bs);
   }
 
+  // Do update in one fell swoop
+  return deltaXij_.update(correctedAcc, correctedOmega, dt, F, G1, G2);
+}
+
+//------------------------------------------------------------------------------
+void PreintegrationBase::updatePreintegratedMeasurements(
+    const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
+    Matrix3* D_incrR_integratedOmega, Matrix9* F, Matrix93* G1, Matrix93* G2) {
+
   // Save current rotation for updating Jacobians
   const Rot3 oldRij = deltaXij_.attitude();
 
-  // Do update in one fell swoop
+  // Do update
   deltaTij_ += dt;
-  deltaXij_ = deltaXij_.update(correctedAcc, correctedOmega, dt, F, G1, G2);
+  deltaXij_ = update(measuredAcc, measuredOmega, dt, F, G1, G2); // functional
 
   // Update Jacobians
   // TODO(frank): we are repeating some computation here: accessible in F ?
+  // Correct for bias in the sensor frame
+  Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
+  Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
+
   Matrix3 D_acc_R;
   oldRij.rotate(correctedAcc, D_acc_R);
   const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
@@ -103,8 +114,7 @@ void PreintegrationBase::updatePreintegratedMeasurements(
   const Vector3 integratedOmega = correctedOmega * dt;
   const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
   const Matrix3 incrRt = incrR.transpose();
-  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
-      - *D_incrR_integratedOmega * dt;
+  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - *D_incrR_integratedOmega * dt;
 
   double dt22 = 0.5 * dt * dt;
   const Matrix3 dRij = oldRij.matrix(); // expensive
diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h
index 6b4626654..95de5e935 100644
--- a/gtsam/navigation/PreintegrationBase.h
+++ b/gtsam/navigation/PreintegrationBase.h
@@ -188,6 +188,10 @@ public:
   /// check equality
   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;
+
   /// Update preintegrated measurements
   void updatePreintegratedMeasurements(const Vector3& measuredAcc,
       const Vector3& measuredOmega, const double deltaT,