Imu Examples Refactor

gtsam/navigation/navigation.i

@@ -88,6 +88,8 @@ virtual class PreintegratedRotationParams {
 virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
   PreintegrationParams(Vector n_gravity);
 
+  gtsam::Vector n_gravity;
+
   static gtsam::PreintegrationParams* MakeSharedD(double g);
   static gtsam::PreintegrationParams* MakeSharedU(double g);
   static gtsam::PreintegrationParams* MakeSharedD();  // default g = 9.81

python/gtsam/examples/ImuFactorExample.py

@@ -10,20 +10,19 @@
 Author: Frank Dellaert, Varun Agrawal
 """
 
-# pylint: disable=no-name-in-module,unused-import,arguments-differ
+# pylint: disable=no-name-in-module,unused-import,arguments-differ,import-error,wrong-import-order
 
 from __future__ import print_function
 
 import argparse
 import math
 
+import gtsam
 import matplotlib.pyplot as plt
 import numpy as np
-from mpl_toolkits.mplot3d import Axes3D
-
-import gtsam
 from gtsam.symbol_shorthand import B, V, X
 from gtsam.utils.plot import plot_pose3
+from mpl_toolkits.mplot3d import Axes3D
 
 from PreintegrationExample import POSES_FIG, PreintegrationExample
 
@@ -32,9 +31,29 @@
 np.set_printoptions(precision=3, suppress=True)
 
 
+def parse_args() -> argparse.Namespace:
+    """Parse command line arguments."""
+    parser = argparse.ArgumentParser("ImuFactorExample.py")
+    parser.add_argument("--twist_scenario",
+                        default="sick_twist",
+                        choices=("zero_twist", "forward_twist", "loop_twist",
+                                 "sick_twist"))
+    parser.add_argument("--time",
+                        "-T",
+                        default=12,
+                        type=int,
+                        help="Total time in seconds")
+    parser.add_argument("--compute_covariances",
+                        default=False,
+                        action='store_true')
+    parser.add_argument("--verbose", default=False, action='store_true')
+    args = parser.parse_args()
+    return args
+
+
 class ImuFactorExample(PreintegrationExample):
     """Class to run example of the Imu Factor."""
-    def __init__(self, twist_scenario="sick_twist"):
+    def __init__(self, twist_scenario: str = "sick_twist"):
         self.velocity = np.array([2, 0, 0])
         self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
         self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
@@ -51,43 +70,64 @@ def __init__(self, twist_scenario="sick_twist"):
         gyroBias = np.array([0.1, 0.3, -0.1])
         bias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
 
+        g = 9.81
+        params = gtsam.PreintegrationParams.MakeSharedU(g)
+
+        # Some arbitrary noise sigmas
+        gyro_sigma = 1e-3
+        accel_sigma = 1e-3
+        I_3x3 = np.eye(3)
+        params.setGyroscopeCovariance(gyro_sigma**2 * I_3x3)
+        params.setAccelerometerCovariance(accel_sigma**2 * I_3x3)
+        params.setIntegrationCovariance(1e-7**2 * I_3x3)
+
         dt = 1e-2
         super(ImuFactorExample, self).__init__(twist_scenarios[twist_scenario],
-                                               bias, dt)
+                                               bias, params, dt)
 
-    def addPrior(self, i, graph):
-        """Add priors at time step `i`."""
+    def addPrior(self, i: int, graph: gtsam.NonlinearFactorGraph):
+        """Add a prior on the navigation state at time `i`."""
         state = self.scenario.navState(i)
         graph.push_back(
             gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise))
         graph.push_back(
             gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise))
 
-    def optimize(self, graph, initial):
+    def optimize(self, graph: gtsam.NonlinearFactorGraph,
+                 initial: gtsam.Values):
         """Optimize using Levenberg-Marquardt optimization."""
         params = gtsam.LevenbergMarquardtParams()
         params.setVerbosityLM("SUMMARY")
         optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
         result = optimizer.optimize()
         return result
 
-    def plot(self, result):
-        """Plot resulting poses."""
+    def plot(self,
+             values: gtsam.Values,
+             title: str = "Estimated Trajectory",
+             fignum: int = POSES_FIG + 1,
+             show: bool = False):
+        """Plot poses in values."""
         i = 0
-        while result.exists(X(i)):
-            pose_i = result.atPose3(X(i))
-            plot_pose3(POSES_FIG + 1, pose_i, 1)
+        while values.exists(X(i)):
+            pose_i = values.atPose3(X(i))
+            plot_pose3(fignum, pose_i, 1)
             i += 1
-        plt.title("Estimated Trajectory")
+        plt.title(title)
 
-        gtsam.utils.plot.set_axes_equal(POSES_FIG + 1)
+        gtsam.utils.plot.set_axes_equal(fignum)
 
-        print("Bias Values", result.atConstantBias(BIAS_KEY))
+        print("Bias Values", values.atConstantBias(BIAS_KEY))
 
         plt.ioff()
-        plt.show()
 
-    def run(self, T=12, compute_covariances=False, verbose=True):
+        if show:
+            plt.show()
+
+    def run(self,
+            T: int = 12,
+            compute_covariances: bool = False,
+            verbose: bool = True):
         """Main runner."""
         graph = gtsam.NonlinearFactorGraph()
 
@@ -173,25 +213,11 @@ def run(self, T=12, compute_covariances=False, verbose=True):
                 print("Covariance on vel {}:\n{}\n".format(
                     i, marginals.marginalCovariance(V(i))))
 
-        self.plot(result)
+        self.plot(result, show=True)
 
 
 if __name__ == '__main__':
-    parser = argparse.ArgumentParser("ImuFactorExample.py")
-    parser.add_argument("--twist_scenario",
-                        default="sick_twist",
-                        choices=("zero_twist", "forward_twist", "loop_twist",
-                                 "sick_twist"))
-    parser.add_argument("--time",
-                        "-T",
-                        default=12,
-                        type=int,
-                        help="Total time in seconds")
-    parser.add_argument("--compute_covariances",
-                        default=False,
-                        action='store_true')
-    parser.add_argument("--verbose", default=False, action='store_true')
-    args = parser.parse_args()
+    args = parse_args()
 
     ImuFactorExample(args.twist_scenario).run(args.time,
                                               args.compute_covariances,

python/gtsam/examples/PreintegrationExample.py

@@ -5,10 +5,14 @@
 
 See LICENSE for the license information
 
-A script validating the Preintegration of IMU measurements
+A script validating the Preintegration of IMU measurements.
+
+Authors: Frank Dellaert, Varun Agrawal.
 """
 
-import math
+# pylint: disable=invalid-name,unused-import,wrong-import-order
+
+from typing import Optional, Sequence
 
 import gtsam
 import matplotlib.pyplot as plt
@@ -20,23 +24,25 @@
 POSES_FIG = 2
 
 
-class PreintegrationExample(object):
-
+class PreintegrationExample:
+    """Base class for all preintegration examples."""
     @staticmethod
-    def defaultParams(g):
+    def defaultParams(g: float):
         """Create default parameters with Z *up* and realistic noise parameters"""
         params = gtsam.PreintegrationParams.MakeSharedU(g)
-        kGyroSigma = math.radians(0.5) / 60  # 0.5 degree ARW
+        kGyroSigma = np.radians(0.5) / 60  # 0.5 degree ARW
         kAccelSigma = 0.1 / 60  # 10 cm VRW
-        params.setGyroscopeCovariance(
-            kGyroSigma ** 2 * np.identity(3, float))
-        params.setAccelerometerCovariance(
-            kAccelSigma ** 2 * np.identity(3, float))
-        params.setIntegrationCovariance(
-            0.0000001 ** 2 * np.identity(3, float))
+        params.setGyroscopeCovariance(kGyroSigma**2 * np.identity(3, float))
+        params.setAccelerometerCovariance(kAccelSigma**2 *
+                                          np.identity(3, float))
+        params.setIntegrationCovariance(0.0000001**2 * np.identity(3, float))
         return params
 
-    def __init__(self, twist=None, bias=None, dt=1e-2):
+    def __init__(self,
+                 twist: Optional[np.ndarray] = None,
+                 bias: Optional[gtsam.imuBias.ConstantBias] = None,
+                 params: Optional[gtsam.PreintegrationParams] = None,
+                 dt: float = 1e-2):
         """Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
 
         # setup interactive plotting
@@ -48,7 +54,7 @@ def __init__(self, twist=None, bias=None, dt=1e-2):
         else:
             # default = loop with forward velocity 2m/s, while pitching up
             # with angular velocity 30 degree/sec (negative in FLU)
-            W = np.array([0, -math.radians(30), 0])
+            W = np.array([0, -np.radians(30), 0])
             V = np.array([2, 0, 0])
 
         self.scenario = gtsam.ConstantTwistScenario(W, V)
@@ -58,9 +64,11 @@ def __init__(self, twist=None, bias=None, dt=1e-2):
         self.labels = list('xyz')
         self.colors = list('rgb')
 
-        # Create runner
-        self.g = 10  # simple gravity constant
-        self.params = self.defaultParams(self.g)
+        if params:
+            self.params = params
+        else:
+            # Default params with simple gravity constant
+            self.params = self.defaultParams(g=10)
 
         if bias is not None:
             self.actualBias = bias
@@ -69,13 +77,16 @@ def __init__(self, twist=None, bias=None, dt=1e-2):
             gyroBias = np.array([0, 0, 0])
             self.actualBias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
 
-        self.runner = gtsam.ScenarioRunner(
-            self.scenario, self.params, self.dt, self.actualBias)
+        # Create runner
+        self.runner = gtsam.ScenarioRunner(self.scenario, self.params, self.dt,
+                                           self.actualBias)
 
         fig, self.axes = plt.subplots(4, 3)
         fig.set_tight_layout(True)
 
-    def plotImu(self, t, measuredOmega, measuredAcc):
+    def plotImu(self, t: float, measuredOmega: Sequence,
+                measuredAcc: Sequence):
+        """Plot IMU measurements."""
         plt.figure(IMU_FIG)
 
         # plot angular velocity
@@ -108,21 +119,24 @@ def plotImu(self, t, measuredOmega, measuredAcc):
             ax.scatter(t, measuredAcc[i], color=color, marker='.')
             ax.set_xlabel('specific force ' + label)
 
-    def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01):
-        # plot ground truth pose, as well as prediction from integrated IMU measurements
+    def plotGroundTruthPose(self,
+                            t: float,
+                            scale: float = 0.3,
+                            time_interval: float = 0.01):
+        """Plot ground truth pose, as well as prediction from integrated IMU measurements."""
         actualPose = self.scenario.pose(t)
         plot_pose3(POSES_FIG, actualPose, scale)
-        t = actualPose.translation()
-        self.maxDim = max([max(np.abs(t)), self.maxDim])
+        translation = actualPose.translation()
+        self.maxDim = max([max(np.abs(translation)), self.maxDim])
         ax = plt.gca()
         ax.set_xlim3d(-self.maxDim, self.maxDim)
         ax.set_ylim3d(-self.maxDim, self.maxDim)
         ax.set_zlim3d(-self.maxDim, self.maxDim)
 
         plt.pause(time_interval)
 
-    def run(self, T=12):
-        # simulate the loop
+    def run(self, T: int = 12):
+        """Simulate the loop."""
         for i, t in enumerate(np.arange(0, T, self.dt)):
             measuredOmega = self.runner.measuredAngularVelocity(t)
             measuredAcc = self.runner.measuredSpecificForce(t)