Merge pull request #869 from borglab/fix/imu-factor-example

gtsam/linear/linear.i

@@ -16,9 +16,9 @@ virtual class Base {
 };
 
 virtual class Gaussian : gtsam::noiseModel::Base {
-  static gtsam::noiseModel::Gaussian* Information(Matrix R);
+  static gtsam::noiseModel::Gaussian* Information(Matrix R, bool smart = true);
-  static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
+  static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R, bool smart = true);
-  static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
+  static gtsam::noiseModel::Gaussian* Covariance(Matrix R, bool smart = true);
 
   bool equals(gtsam::noiseModel::Base& expected, double tol);
 
@@ -37,9 +37,9 @@ virtual class Gaussian : gtsam::noiseModel::Base {
 };
 
 virtual class Diagonal : gtsam::noiseModel::Gaussian {
-  static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas);
+  static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas, bool smart = true);
-  static gtsam::noiseModel::Diagonal* Variances(Vector variances);
+  static gtsam::noiseModel::Diagonal* Variances(Vector variances, bool smart = true);
-  static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
+  static gtsam::noiseModel::Diagonal* Precisions(Vector precisions, bool smart = true);
   Matrix R() const;
 
   // access to noise model
@@ -69,9 +69,9 @@ virtual class Constrained : gtsam::noiseModel::Diagonal {
 };
 
 virtual class Isotropic : gtsam::noiseModel::Diagonal {
-  static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma);
+  static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma, bool smart = true);
-  static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
+  static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace, bool smart = true);
-  static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
+  static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision, bool smart = true);
 
   // access to noise model
   double sigma() const;

gtsam/navigation/tests/testImuFactor.cpp

@@ -819,7 +819,6 @@ struct ImuFactorMergeTest {
         loop_(Vector3(0, -kAngularVelocity, 0), Vector3(kVelocity, 0, 0)) {
     // arbitrary noise values
     p_->gyroscopeCovariance = I_3x3 * 0.01;
-    p_->accelerometerCovariance = I_3x3 * 0.02;
     p_->accelerometerCovariance = I_3x3 * 0.03;
   }
 

python/gtsam/examples/ImuFactorExample.py

@@ -10,28 +10,30 @@ A script validating and demonstrating the ImuFactor inference.
 Author: Frank Dellaert, Varun Agrawal
 """
 
+# pylint: disable=no-name-in-module,unused-import,arguments-differ
+
 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
 
 BIAS_KEY = B(0)
 
-
 np.set_printoptions(precision=3, suppress=True)
 
 
 class ImuFactorExample(PreintegrationExample):
-
+    """Class to run example of the Imu Factor."""
     def __init__(self, twist_scenario="sick_twist"):
         self.velocity = np.array([2, 0, 0])
         self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
@@ -42,9 +44,8 @@ class ImuFactorExample(PreintegrationExample):
             zero_twist=(np.zeros(3), np.zeros(3)),
             forward_twist=(np.zeros(3), self.velocity),
             loop_twist=(np.array([0, -math.radians(30), 0]), self.velocity),
-            sick_twist=(np.array([math.radians(30), -math.radians(30), 0]),
+            sick_twist=(np.array([math.radians(30), -math.radians(30),
-                        self.velocity)
+                                  0]), self.velocity))
-        )
 
         accBias = np.array([-0.3, 0.1, 0.2])
         gyroBias = np.array([0.1, 0.3, -0.1])
@@ -55,19 +56,44 @@ class ImuFactorExample(PreintegrationExample):
                                                bias, dt)
 
     def addPrior(self, i, graph):
+        """Add priors at time step `i`."""
         state = self.scenario.navState(i)
-        graph.push_back(gtsam.PriorFactorPose3(
+        graph.push_back(
-            X(i), state.pose(), self.priorNoise))
+            gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise))
-        graph.push_back(gtsam.PriorFactorVector(
+        graph.push_back(
-            V(i), state.velocity(), self.velNoise))
+            gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise))
+
+    def optimize(self, graph, initial):
+        """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."""
+        i = 0
+        while result.exists(X(i)):
+            pose_i = result.atPose3(X(i))
+            plot_pose3(POSES_FIG + 1, pose_i, 1)
+            i += 1
+        plt.title("Estimated Trajectory")
+
+        gtsam.utils.plot.set_axes_equal(POSES_FIG + 1)
+
+        print("Bias Values", result.atConstantBias(BIAS_KEY))
+
+        plt.ioff()
+        plt.show()
 
     def run(self, T=12, compute_covariances=False, verbose=True):
+        """Main runner."""
         graph = gtsam.NonlinearFactorGraph()
 
         # initialize data structure for pre-integrated IMU measurements
         pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
 
-        T = 12
         num_poses = T  # assumes 1 factor per second
         initial = gtsam.Values()
         initial.insert(BIAS_KEY, self.actualBias)
@@ -91,14 +117,13 @@ class ImuFactorExample(PreintegrationExample):
             if k % 10 == 0:
                 self.plotImu(t, measuredOmega, measuredAcc)
 
-            if (k+1) % int(1 / self.dt) == 0:
+            if (k + 1) % int(1 / self.dt) == 0:
                 # Plot every second
                 self.plotGroundTruthPose(t, scale=1)
                 plt.title("Ground Truth Trajectory")
 
                 # create IMU factor every second
-                factor = gtsam.ImuFactor(X(i), V(i),
+                factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1),
-                                         X(i + 1), V(i + 1),
                                          BIAS_KEY, pim)
                 graph.push_back(factor)
 
@@ -108,34 +133,34 @@ class ImuFactorExample(PreintegrationExample):
 
                 pim.resetIntegration()
 
-                rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1)
+                rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3) * 0.1)
-                translationNoise = gtsam.Point3(*np.random.randn(3)*1)
+                translationNoise = gtsam.Point3(*np.random.randn(3) * 1)
                 poseNoise = gtsam.Pose3(rotationNoise, translationNoise)
 
                 actual_state_i = self.scenario.navState(t + self.dt)
                 print("Actual state at {0}:\n{1}".format(
-                    t+self.dt, actual_state_i))
+                    t + self.dt, actual_state_i))
 
                 noisy_state_i = gtsam.NavState(
                     actual_state_i.pose().compose(poseNoise),
-                    actual_state_i.velocity() + np.random.randn(3)*0.1)
+                    actual_state_i.velocity() + np.random.randn(3) * 0.1)
 
-                initial.insert(X(i+1), noisy_state_i.pose())
+                initial.insert(X(i + 1), noisy_state_i.pose())
-                initial.insert(V(i+1), noisy_state_i.velocity())
+                initial.insert(V(i + 1), noisy_state_i.velocity())
                 i += 1
 
         # add priors on end
         self.addPrior(num_poses - 1, graph)
 
-        initial.print_("Initial values:")
+        initial.print("Initial values:")
 
-        # optimize using Levenberg-Marquardt optimization
+        result = self.optimize(graph, initial)
-        params = gtsam.LevenbergMarquardtParams()
-        params.setVerbosityLM("SUMMARY")
-        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
-        result = optimizer.optimize()
 
-        result.print_("Optimized values:")
+        result.print("Optimized values:")
+        print("------------------")
+        print(graph.error(initial))
+        print(graph.error(result))
+        print("------------------")
 
         if compute_covariances:
             # Calculate and print marginal covariances
@@ -148,33 +173,26 @@ class ImuFactorExample(PreintegrationExample):
                 print("Covariance on vel {}:\n{}\n".format(
                     i, marginals.marginalCovariance(V(i))))
 
-        # Plot resulting poses
+        self.plot(result)
-        i = 0
-        while result.exists(X(i)):
-            pose_i = result.atPose3(X(i))
-            plot_pose3(POSES_FIG+1, pose_i, 1)
-            i += 1
-        plt.title("Estimated Trajectory")
-
-        gtsam.utils.plot.set_axes_equal(POSES_FIG+1)
-
-        print("Bias Values", result.atConstantBias(BIAS_KEY))
-
-        plt.ioff()
-        plt.show()
 
 
 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"))
+                        choices=("zero_twist", "forward_twist", "loop_twist",
-    parser.add_argument("--time", "-T", default=12,
+                                 "sick_twist"))
-                        type=int, help="Total time in seconds")
+    parser.add_argument("--time",
+                        "-T",
+                        default=12,
+                        type=int,
+                        help="Total time in seconds")
     parser.add_argument("--compute_covariances",
-                        default=False, action='store_true')
+                        default=False,
+                        action='store_true')
     parser.add_argument("--verbose", default=False, action='store_true')
     args = parser.parse_args()
 
-    ImuFactorExample(args.twist_scenario).run(
+    ImuFactorExample(args.twist_scenario).run(args.time,
-        args.time, args.compute_covariances, args.verbose)
+                                              args.compute_covariances,
+                                              args.verbose)

python/gtsam/utils/plot.py

@@ -23,7 +23,10 @@ def set_axes_equal(fignum: int) -> None:
       fignum: An integer representing the figure number for Matplotlib.
     """
     fig = plt.figure(fignum)
-    ax = fig.gca(projection='3d')
+    if not fig.axes:
+        ax = fig.add_subplot(projection='3d')
+    else:
+        ax = fig.axes[0]
 
     limits = np.array([
         ax.get_xlim3d(),
@@ -339,7 +342,11 @@ def plot_pose3(
     """
     # get figure object
     fig = plt.figure(fignum)
-    axes = fig.gca(projection='3d')
+    if not fig.axes:
+        axes = fig.add_subplot(projection='3d')
+    else:
+        axes = fig.axes[0]
+
     plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length)
 
     axes.set_xlabel(axis_labels[0])