update ImuFactorExample2.py

python/gtsam_py/python/examples/ImuFactorExample2.py

@@ -1,6 +1,6 @@
 """
 iSAM2 example with ImuFactor.
-Author: Robert Truax (C++), Frank Dellaert (Python)
+Author: Frank Dellaert, Varun Agrawal
 """
 # pylint: disable=invalid-name, E1101
 
@@ -8,17 +8,18 @@ from __future__ import print_function
 
 import math
 
-import gtsam
-import gtsam.utils.plot as gtsam_plot
 import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
 import numpy as np
+
+import gtsam
 from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
                    ConstantTwistScenario, ImuFactor, NonlinearFactorGraph,
                    PinholeCameraCal3_S2, Point3, Pose3,
                    PriorFactorConstantBias, PriorFactorPose3,
                    PriorFactorVector, Rot3, Values)
 from gtsam.gtsam.symbol_shorthand import B, V, X
-from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
+from gtsam.utils import plot
 
 
 def vector3(x, y, z):
@@ -26,46 +27,45 @@ def vector3(x, y, z):
     return np.array([x, y, z], dtype=np.float)
 
 
-def ISAM2_plot(values, fignum=0):
-    """Plot poses."""
-    fig = plt.figure(fignum)
-    axes = fig.gca(projection='3d')
-    plt.cla()
-
-    i = 0
-    min_bounds = 0, 0, 0
-    max_bounds = 0, 0, 0
-    while values.exists(X(i)):
-        pose_i = values.atPose3(X(i))
-        gtsam_plot.plot_pose3(fignum, pose_i, 10)
-        position = pose_i.translation()
-        min_bounds = [min(v1, v2) for (v1, v2) in zip(position, min_bounds)]
-        max_bounds = [max(v1, v2) for (v1, v2) in zip(position, max_bounds)]
-        # max_bounds = min(pose_i.x(), max_bounds[0]), 0, 0
-        i += 1
-
-    # draw
-    axes.set_xlim3d(min_bounds[0]-1, max_bounds[0]+1)
-    axes.set_ylim3d(min_bounds[1]-1, max_bounds[1]+1)
-    axes.set_zlim3d(min_bounds[2]-1, max_bounds[2]+1)
-    plt.pause(1)
-
-
-# IMU preintegration parameters
-# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
 g = 9.81
 n_gravity = vector3(0, 0, -g)
-PARAMS = gtsam.PreintegrationParams.MakeSharedU(g)
-I = np.eye(3)
-PARAMS.setAccelerometerCovariance(I * 0.1)
-PARAMS.setGyroscopeCovariance(I * 0.1)
-PARAMS.setIntegrationCovariance(I * 0.1)
-PARAMS.setUse2ndOrderCoriolis(False)
-PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
 
-BIAS_COVARIANCE = gtsam.noiseModel.Isotropic.Variance(6, 0.1)
+
-DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
+def preintegration_parameters():
-              Point3(0.05, -0.10, 0.20))
+    # IMU preintegration parameters
+    # Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
+    PARAMS = gtsam.PreintegrationParams.MakeSharedU(g)
+    I = np.eye(3)
+    PARAMS.setAccelerometerCovariance(I * 0.1)
+    PARAMS.setGyroscopeCovariance(I * 0.1)
+    PARAMS.setIntegrationCovariance(I * 0.1)
+    PARAMS.setUse2ndOrderCoriolis(False)
+    PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
+
+    BIAS_COVARIANCE = gtsam.noiseModel.Isotropic.Variance(6, 0.1)
+    DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
+                  Point3(0.05, -0.10, 0.20))
+
+    return PARAMS, BIAS_COVARIANCE, DELTA
+
+
+def get_camera(radius):
+    up = Point3(0, 0, 1)
+    target = Point3(0, 0, 0)
+    position = Point3(radius, 0, 0)
+    camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
+    return camera
+
+
+def get_scenario(radius, pose_0, angular_velocity, delta_t):
+    """Create the set of ground-truth landmarks and poses"""
+
+    angular_velocity_vector = vector3(0, -angular_velocity, 0)
+    linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
+    scenario = ConstantTwistScenario(
+        angular_velocity_vector, linear_velocity_vector, pose_0)
+
+    return scenario
 
 
 def IMU_example():
@@ -73,23 +73,17 @@ def IMU_example():
 
     # Start with a camera on x-axis looking at origin
     radius = 30
-    up = Point3(0, 0, 1)
+    camera = get_camera(radius)
-    target = Point3(0, 0, 0)
-    position = Point3(radius, 0, 0)
-    camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
     pose_0 = camera.pose()
 
-    # Create the set of ground-truth landmarks and poses
-    angular_velocity = math.radians(180)  # rad/sec
     delta_t = 1.0/18  # makes for 10 degrees per step
+    angular_velocity = math.radians(180)  # rad/sec
+    scenario = get_scenario(radius, pose_0, angular_velocity, delta_t)
 
-    angular_velocity_vector = vector3(0, -angular_velocity, 0)
+    PARAMS, BIAS_COVARIANCE, DELTA = preintegration_parameters()
-    linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
-    scenario = ConstantTwistScenario(
-        angular_velocity_vector, linear_velocity_vector, pose_0)
 
     # Create a factor graph
-    newgraph = NonlinearFactorGraph()
+    graph = NonlinearFactorGraph()
 
     # Create (incremental) ISAM2 solver
     isam = ISAM2()
@@ -102,21 +96,21 @@ def IMU_example():
     # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
     noise = gtsam.noiseModel.Diagonal.Sigmas(
         np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))
-    newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise))
+    graph.push_back(PriorFactorPose3(X(0), pose_0, noise))
 
     # Add imu priors
     biasKey = B(0)
     biasnoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
     biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias.ConstantBias(),
                                         biasnoise)
-    newgraph.push_back(biasprior)
+    graph.push_back(biasprior)
     initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
     velnoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
 
     # Calculate with correct initial velocity
     n_velocity = vector3(0, angular_velocity * radius, 0)
     velprior = PriorFactorVector(V(0), n_velocity, velnoise)
-    newgraph.push_back(velprior)
+    graph.push_back(velprior)
     initialEstimate.insert(V(0), n_velocity)
 
     accum = gtsam.PreintegratedImuMeasurements(PARAMS)
@@ -138,7 +132,7 @@ def IMU_example():
                 biasKey += 1
                 factor = BetweenFactorConstantBias(
                     biasKey - 1, biasKey, gtsam.imuBias.ConstantBias(), BIAS_COVARIANCE)
-                newgraph.add(factor)
+                graph.add(factor)
                 initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
 
             # Predict acceleration and gyro measurements in (actual) body frame
@@ -150,21 +144,24 @@ def IMU_example():
 
             # Add Imu Factor
             imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
-            newgraph.add(imufac)
+            graph.add(imufac)
 
             # insert new velocity, which is wrong
             initialEstimate.insert(V(i), n_velocity)
             accum.resetIntegration()
 
         # Incremental solution
-        isam.update(newgraph, initialEstimate)
+        isam.update(graph, initialEstimate)
         result = isam.calculateEstimate()
-        ISAM2_plot(result)
+        plot.plot_incremental_trajectory(0, result,
+                                         start=i, scale=3, time_interval=0.01)
 
         # reset
-        newgraph = NonlinearFactorGraph()
+        graph = NonlinearFactorGraph()
         initialEstimate.clear()
 
+    plt.show()
+
 
 if __name__ == '__main__':
     IMU_example()