From cf07c22c2c61e6e1a62485e093ce6fb2fe87b6c5 Mon Sep 17 00:00:00 2001
From: Frank <frank@skyd.io>
Date: Tue, 26 Jan 2016 15:43:46 -0800
Subject: [PATCH] Showing trajectory and ground truth quantities

---
 gtsam/navigation/Scenario.h               |  8 ++-
 python/gtsam_examples/ImuFactorExample.py | 60 +++++++++++++++--------
 2 files changed, 46 insertions(+), 22 deletions(-)

diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h
index 2b2b3fddf..a6c005d7c 100644
--- a/gtsam/navigation/Scenario.h
+++ b/gtsam/navigation/Scenario.h
@@ -46,7 +46,13 @@ class Scenario {
   }
 };
 
-/// Scenario with constant twist 3D trajectory.
+/**
+ * Scenario with constant twist 3D trajectory.
+ * Note that a plane flying level does not feel any acceleration: gravity is
+ * being perfectly compensated by the lift generated by the wings, and drag is
+ * compensated by thrust. The accelerometer will add the gravity component back
+ * in, as modeled by the specificForce method in ScenarioRunner.
+ */
 class ConstantTwistScenario : public Scenario {
  public:
   /// Construct scenario with constant twist [w,v]
diff --git a/python/gtsam_examples/ImuFactorExample.py b/python/gtsam_examples/ImuFactorExample.py
index 2f64f5a1b..74daf1f47 100644
--- a/python/gtsam_examples/ImuFactorExample.py
+++ b/python/gtsam_examples/ImuFactorExample.py
@@ -9,6 +9,7 @@ import numpy as np
 from mpl_toolkits.mplot3d import Axes3D
 
 import gtsam
+from gtsam_utils import plotPose3
 
 class ImuFactorExample(object):
 
@@ -24,37 +25,54 @@ class ImuFactorExample(object):
         W = np.array([0, -w, 0])
         V = np.array([v, 0, 0])
         self.scenario = gtsam.ConstantTwistScenario(W, V)
-
+        self.dt = 0.5
+        self.realTimeFactor = 10.0
+        
         # Calculate time to do 1 loop
-        self.T = 2 * math.pi / w
+        self.radius = v / w
+        self.timeForOneLoop = 2 * math.pi / w
+        self.labels = list('xyz')
+        self.colors = list('rgb')
 
-    def plot(self, t, pose):
-        # plot IMU    
+    def plot(self, t, pose, omega_b, acceleration_n, acceleration_b):
+        # plot angular velocity    
         plt.figure(1)
-        times = np.arange(0, 10, 0.1)
-        shape = len(times), 1
-        labels = list('xyz')
-        colors = list('rgb')
-        plt.clf()
-        for i, (label, color) in enumerate(zip(labels, colors)):
+        for i, (label, color) in enumerate(zip(self.labels, self.colors)):
             plt.subplot(3, 1, i + 1)
-            imu = np.random.randn(len(times), 1)
-            plt.plot(times, imu, color=color)
-#             plt.axis([tmin, tmax, min,max])
+            plt.scatter(t, omega_b[i], color=color, marker='.')
+            plt.xlabel(label)
+            
+        # plot acceleration in nav
+        plt.figure(2)
+        for i, (label, color) in enumerate(zip(self.labels, self.colors)):
+            plt.subplot(3, 1, i + 1)
+            plt.scatter(t, acceleration_n[i], color=color, marker='.')
+            plt.xlabel(label)
+            
+        # plot acceleration in body
+        plt.figure(3)
+        for i, (label, color) in enumerate(zip(self.labels, self.colors)):
+            plt.subplot(3, 1, i + 1)
+            plt.scatter(t, acceleration_b[i], color=color, marker='.')
             plt.xlabel(label)
             
         # plot ground truth
-        fig = plt.figure(2)
-        ax = fig.gca(projection='3d')
-        p = pose.translation()
-        ax.scatter(p.x(), p.y(), p.z())
+        plotPose3(4, pose, 1.0)
+        ax = plt.gca()
+        ax.set_xlim3d(-self.radius, self.radius)
+        ax.set_ylim3d(-self.radius, self.radius)
+        ax.set_zlim3d(0, self.radius * 2)
 
-        plt.pause(0.1)
+        plt.pause(self.dt / self.realTimeFactor)
 
     def run(self):
-        for t in np.arange(0, self.T / 2, 1):
-            pose = self.scenario.pose(t)
-            self.plot(t, pose)
+        # simulate the loop up to the top
+        for t in np.arange(0, self.timeForOneLoop, self.dt):
+            self.plot(t,
+                      self.scenario.pose(t),
+                      self.scenario.omega_b(t),
+                      self.scenario.acceleration_n(t),
+                      self.scenario.acceleration_b(t))
 
         plt.ioff()
         plt.show()