From 9e99f88473939a70cd030cef50df7530c0c43194 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Mon, 28 Dec 2015 10:00:53 -0800
Subject: [PATCH] Prediction works !

---
 gtsam/navigation/ScenarioRunner.cpp | 35 +++++++++++++++++------------
 gtsam/navigation/ScenarioRunner.h   | 10 ++++++++-
 2 files changed, 30 insertions(+), 15 deletions(-)

diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp
index 40b683f9f..781f72aa5 100644
--- a/gtsam/navigation/ScenarioRunner.cpp
+++ b/gtsam/navigation/ScenarioRunner.cpp
@@ -35,12 +35,21 @@ using symbol_shorthand::V;
 
 static const Symbol kBiasKey('B', 0);
 
+Vector9 PreintegratedMeasurements2::currentEstimate() const {
+  VectorValues biasValues;
+  biasValues.insert(kBiasKey, estimatedBias_.vector());
+  VectorValues zetaValues = posterior_k_->solve(biasValues);
+  Vector9 zeta;
+  zeta << zetaValues.at(T(k_)), Vector3::Zero(), Vector3::Zero();
+  return zeta;
+}
+
 void PreintegratedMeasurements2::integrateMeasurement(
     const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
   typedef map<Key, Matrix> Terms;
   static const Matrix36 omega_D_bias = (Matrix36() << Z_3x3, I_3x3).finished();
 
-  // Correct measurements by subtractig bias
+  // Correct measurements by subtracting bias
   Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope();
 
   GaussianFactorGraph graph;
@@ -49,10 +58,9 @@ void PreintegratedMeasurements2::integrateMeasurement(
 
   // Handle first time differently
   if (k_ == 0) {
-    // theta(1) = measuredOmega - (bias + bias_delta)
+    // theta(1) = (measuredOmega - (bias + bias_delta)) * dt
     graph.add<Terms>({{T(k_ + 1), I_3x3}, {kBiasKey, omega_D_bias}},
-                     correctedOmega, gyroscopeNoiseModel_);
-    GTSAM_PRINT(graph);
+                     dt * correctedOmega, gyroscopeNoiseModel_);
 
     // eliminate all but biases
     Ordering keys = list_of(T(k_ + 1));
@@ -60,14 +68,14 @@ void PreintegratedMeasurements2::integrateMeasurement(
         graph.eliminatePartialSequential(keys, EliminateQR);
   } else {
     // add previous posterior
-    graph.add(posterior_k_);
+    graph.add(boost::static_pointer_cast<GaussianFactor>(posterior_k_));
 
-    // theta(k+1) = theta(k) + inverse(H)*(measuredOmega - (bias + bias_delta))
-    // =>  H*theta(k+1) - H*theta(k) + bias_delta = measuredOmega - bias
+    // theta(k+1) = theta(k) + inverse(H)*(measuredOmega - bias - bias_delta) dt
+    // =>  H*theta(k+1) - H*theta(k) + bias_delta dt = (measuredOmega - bias) dt
     Matrix3 H = Rot3::ExpmapDerivative(theta_);
-    graph.add<Terms>({{T(k_ + 1), H}, {T(k_), -H}, {kBiasKey, omega_D_bias}},
-                     correctedOmega, gyroscopeNoiseModel_);
-    GTSAM_PRINT(graph);
+    graph.add<Terms>(
+        {{T(k_ + 1), H}, {T(k_), -H}, {kBiasKey, omega_D_bias * dt}},
+        dt * correctedOmega, gyroscopeNoiseModel_);
 
     // eliminate all but biases
     Ordering keys = list_of(T(k_))(T(k_ + 1));
@@ -75,9 +83,6 @@ void PreintegratedMeasurements2::integrateMeasurement(
         graph.eliminatePartialSequential(keys, EliminateQR);
   }
 
-  GTSAM_PRINT(*bayesNet);
-  GTSAM_PRINT(*shouldBeEmpty);
-
   // The bayesNet now contains P(zeta(k)|zeta(k+1),bias) P(zeta(k+1)|bias)
   // We marginalize zeta(k) by dropping the first factor
   posterior_k_ = bayesNet->back();
@@ -87,7 +92,9 @@ void PreintegratedMeasurements2::integrateMeasurement(
 NavState PreintegratedMeasurements2::predict(
     const NavState& state_i, const imuBias::ConstantBias& bias_i,
     OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const {
-  return NavState();
+  // TODO(frank): handle bias
+  Vector9 zeta = currentEstimate();
+  return state_i.expmap(zeta);
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h
index f469e29f5..f8f580ac0 100644
--- a/gtsam/navigation/ScenarioRunner.h
+++ b/gtsam/navigation/ScenarioRunner.h
@@ -41,9 +41,12 @@ class PreintegratedMeasurements2 {
  private:
   SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_;
   const imuBias::ConstantBias estimatedBias_;
+
   size_t k_;       ///< index/count of measurements integrated
   Vector3 theta_;  ///< current estimate for theta
-  GaussianFactor::shared_ptr posterior_k_;  ///< posterior on current iterate
+
+  /// posterior on current iterate, as a conditional P(zeta|bias_delta):
+  boost::shared_ptr<GaussianConditional> posterior_k_;
 
  public:
   typedef ImuFactor::PreintegratedMeasurements::Params Params;
@@ -71,6 +74,11 @@ class PreintegratedMeasurements2 {
   NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
                    OptionalJacobian<9, 9> H1 = boost::none,
                    OptionalJacobian<9, 6> H2 = boost::none) const;
+
+ private:
+  // estimate zeta given estimated biases
+  // calculates conditional mean of P(zeta|bias_delta)
+  Vector9 currentEstimate() const;
 };
 
 /*