From ac6fb495a6536fe021f65af9b05483ac5b16190d Mon Sep 17 00:00:00 2001
From: Frank <frank@skyd.io>
Date: Wed, 27 Jan 2016 14:16:18 -0800
Subject: [PATCH] Full optimization

---
 python/gtsam_examples/ImuFactorExample.py     | 62 +++++++++++++++++--
 .../gtsam_examples/PreintegrationExample.py   |  6 +-
 2 files changed, 60 insertions(+), 8 deletions(-)

diff --git a/python/gtsam_examples/ImuFactorExample.py b/python/gtsam_examples/ImuFactorExample.py
index 6ab5c1211..c018dc7a7 100644
--- a/python/gtsam_examples/ImuFactorExample.py
+++ b/python/gtsam_examples/ImuFactorExample.py
@@ -2,6 +2,7 @@
 A script validating the ImuFactor inference.
 """
 
+from __future__ import print_function
 import math
 import matplotlib.pyplot as plt
 import numpy as np
@@ -10,21 +11,72 @@ from mpl_toolkits.mplot3d import Axes3D
 
 import gtsam
 from gtsam_utils import plotPose3
-from PreintegrationExample import PreintegrationExample
+from PreintegrationExample import PreintegrationExample, POSES_FIG
+
+# shorthand symbols:
+BIAS_KEY = int(gtsam.Symbol('b', 0))
+V = lambda j: int(gtsam.Symbol('v', j))
+X = lambda i: int(gtsam.Symbol('x', i))
 
 class ImuFactorExample(PreintegrationExample):
 
     def run(self):
-        # simulate the loop up to the top
-        T = self.timeForOneLoop
+        graph = gtsam.NonlinearFactorGraph()
+        for i in [0, 12]:
+            priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
+            graph.push_back(gtsam.PriorFactorPose3(X(i), gtsam.Pose3(), priorNoise))
+            velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
+            graph.push_back(gtsam.PriorFactorVector3(V(i), np.array([2, 0, 0]), velNoise))
+        
+        i = 0  # state index
+        
+        # initialize data structure for pre-integrated IMU measurements 
         pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
-        for i, t in enumerate(np.arange(0, T, self.dt)):
+        
+        # simulate the loop
+        T = self.timeForOneLoop
+        for k, t in enumerate(np.arange(0, T, self.dt)):
+            # get measurements and add them to PIM
             measuredOmega = self.runner.measuredAngularVelocity(t)
             measuredAcc = self.runner.measuredSpecificForce(t)
-            if i % 25 == 0:
+            pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
+            
+            # Plot every second
+            if k % 100 == 0:
                 self.plotImu(t, measuredOmega, measuredAcc)
                 self.plotGroundTruthPose(t)
+            
+            # create factor every second
+            if (k + 1) % 100 == 0:
+                factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim)
+                graph.push_back(factor)
+                pim.resetIntegration()
+                i += 1
 
+        graph.print()
+        num_poses = i + 1
+
+        initial = gtsam.Values()
+        initial.insert(BIAS_KEY, gtsam.ConstantBias())
+        for i in range(num_poses):
+            initial.insert(X(i), gtsam.Pose3())
+            initial.insert(V(i), np.zeros(3, np.float))
+        
+        # optimize using Levenberg-Marquardt optimization
+        params = gtsam.LevenbergMarquardtParams()
+        params.setVerbosityLM("SUMMARY")
+        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
+        result = optimizer.optimize()
+        # result.print("\Result:\n")
+        print(self.actualBias)
+
+        # Plot cameras
+        i = 0
+        while result.exists(X(i)):
+            pose_i = result.pose3_at(X(i))
+            plotPose3(POSES_FIG, pose_i, 0.1)
+            i += 1
+            
         plt.ioff()
         plt.show()
 
diff --git a/python/gtsam_examples/PreintegrationExample.py b/python/gtsam_examples/PreintegrationExample.py
index db0ce4363..1cf96b9cd 100644
--- a/python/gtsam_examples/PreintegrationExample.py
+++ b/python/gtsam_examples/PreintegrationExample.py
@@ -12,7 +12,7 @@ import gtsam
 from gtsam_utils import plotPose3
 
 IMU_FIG = 1
-GROUND_TRUTH_FIG = 2
+POSES_FIG = 2
 
 class PreintegrationExample(object):
 
@@ -92,7 +92,7 @@ class PreintegrationExample(object):
     def plotGroundTruthPose(self, t):
         # plot ground truth pose, as well as prediction from integrated IMU measurements
         actualPose = self.scenario.pose(t)
-        plotPose3(GROUND_TRUTH_FIG, actualPose, 0.3)
+        plotPose3(POSES_FIG, actualPose, 0.3)
         ax = plt.gca()
         ax.set_xlim3d(-self.radius, self.radius)
         ax.set_ylim3d(-self.radius, self.radius)
@@ -111,7 +111,7 @@ class PreintegrationExample(object):
                 self.plotGroundTruthPose(t)
                 pim = self.runner.integrate(t, self.actualBias, True)
                 predictedNavState = self.runner.predict(pim, self.actualBias)
-                plotPose3(GROUND_TRUTH_FIG, predictedNavState.pose(), 0.1)
+                plotPose3(POSES_FIG, predictedNavState.pose(), 0.1)
 
         plt.ioff()
         plt.show()