Merged in fix/458_scenario_examples (pull request #433)

close issue #458
ImuFactor python examples ported

cython/gtsam/examples/ImuFactorExample.py

@@ -0,0 +1,143 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+A script validating the ImuFactor inference.
+"""
+
+from __future__ import print_function
+
+import math
+
+import matplotlib.pyplot as plt
+import numpy as np
+from mpl_toolkits.mplot3d import Axes3D
+
+import gtsam
+from gtsam.utils.plot import plot_pose3
+from PreintegrationExample import POSES_FIG, PreintegrationExample
+
+BIAS_KEY = int(gtsam.symbol(ord('b'), 0))
+
+
+def X(key):
+    """Create symbol for pose key."""
+    return gtsam.symbol(ord('x'), key)
+
+
+def V(key):
+    """Create symbol for velocity key."""
+    return gtsam.symbol(ord('v'), key)
+
+
+np.set_printoptions(precision=3, suppress=True)
+
+
+class ImuFactorExample(PreintegrationExample):
+
+    def __init__(self):
+        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)
+
+        # Choose one of these twists to change scenario:
+        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]), self.velocity)
+
+        accBias = np.array([-0.3, 0.1, 0.2])
+        gyroBias = np.array([0.1, 0.3, -0.1])
+        bias = gtsam.imuBias_ConstantBias(accBias, gyroBias)
+
+        dt = 1e-2
+        super(ImuFactorExample, self).__init__(sick_twist, bias, dt)
+
+    def addPrior(self, i, graph):
+        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 run(self):
+        graph = gtsam.NonlinearFactorGraph()
+
+        # initialize data structure for pre-integrated IMU measurements
+        pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
+
+        T = 12
+        num_poses = T + 1  # assumes 1 factor per second
+        initial = gtsam.Values()
+        initial.insert(BIAS_KEY, self.actualBias)
+        for i in range(num_poses):
+            state_i = self.scenario.navState(float(i))
+            initial.insert(X(i), state_i.pose())
+            initial.insert(V(i), state_i.velocity())
+
+        # simulate the loop
+        i = 0  # state index
+        actual_state_i = self.scenario.navState(0)
+        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)
+            pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
+
+            # Plot IMU many times
+            if k % 10 == 0:
+                self.plotImu(t, measuredOmega, measuredAcc)
+
+            # Plot every second
+            if k % int(1 / self.dt) == 0:
+                self.plotGroundTruthPose(t)
+
+            # create IMU factor every second
+            if (k + 1) % int(1 / self.dt) == 0:
+                factor = gtsam.ImuFactor(X(i), V(i), X(
+                    i + 1), V(i + 1), BIAS_KEY, pim)
+                graph.push_back(factor)
+                if True:
+                    print(factor)
+                    print(pim.predict(actual_state_i, self.actualBias))
+                pim.resetIntegration()
+                actual_state_i = self.scenario.navState(t + self.dt)
+                i += 1
+
+        # add priors on beginning and end
+        self.addPrior(0, graph)
+        self.addPrior(num_poses - 1, graph)
+
+        # optimize using Levenberg-Marquardt optimization
+        params = gtsam.LevenbergMarquardtParams()
+        params.setVerbosityLM("SUMMARY")
+        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
+        result = optimizer.optimize()
+
+        # Calculate and print marginal covariances
+        marginals = gtsam.Marginals(graph, result)
+        print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY))
+        for i in range(num_poses):
+            print("Covariance on pose {}:\n{}\n".format(
+                i, marginals.marginalCovariance(X(i))))
+            print("Covariance on vel {}:\n{}\n".format(
+                i, marginals.marginalCovariance(V(i))))
+
+        # Plot resulting poses
+        i = 0
+        while result.exists(X(i)):
+            pose_i = result.atPose3(X(i))
+            plot_pose3(POSES_FIG, pose_i, 0.1)
+            i += 1
+        print(result.atimuBias_ConstantBias(BIAS_KEY))
+
+        plt.ioff()
+        plt.show()
+
+
+if __name__ == '__main__':
+    ImuFactorExample().run()

cython/gtsam/examples/PreintegrationExample.py

@@ -0,0 +1,140 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+A script validating the Preintegration of IMU measurements
+"""
+
+import math
+
+import matplotlib.pyplot as plt
+import numpy as np
+from mpl_toolkits.mplot3d import Axes3D
+
+import gtsam
+from gtsam.utils.plot import plot_pose3
+
+IMU_FIG = 1
+POSES_FIG = 2
+
+
+class PreintegrationExample(object):
+
+    @staticmethod
+    def defaultParams(g):
+        """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
+        kAccelSigma = 0.1 / 60  # 10 cm VRW
+        params.setGyroscopeCovariance(
+            kGyroSigma ** 2 * np.identity(3, np.float))
+        params.setAccelerometerCovariance(
+            kAccelSigma ** 2 * np.identity(3, np.float))
+        params.setIntegrationCovariance(
+            0.0000001 ** 2 * np.identity(3, np.float))
+        return params
+
+    def __init__(self, twist=None, bias=None, dt=1e-2):
+        """Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
+
+        # setup interactive plotting
+        plt.ion()
+
+        # Setup loop as default scenario
+        if twist is not None:
+            (W, V) = twist
+        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])
+            V = np.array([2, 0, 0])
+
+        self.scenario = gtsam.ConstantTwistScenario(W, V)
+        self.dt = dt
+
+        self.maxDim = 5
+        self.labels = list('xyz')
+        self.colors = list('rgb')
+
+        # Create runner
+        self.g = 10  # simple gravity constant
+        self.params = self.defaultParams(self.g)
+
+        if bias is not None:
+            self.actualBias = bias
+        else:
+            accBias = np.array([0, 0.1, 0])
+            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)
+
+    def plotImu(self, t, measuredOmega, measuredAcc):
+        plt.figure(IMU_FIG)
+
+        # plot angular velocity
+        omega_b = self.scenario.omega_b(t)
+        for i, (label, color) in enumerate(zip(self.labels, self.colors)):
+            plt.subplot(4, 3, i + 1)
+            plt.scatter(t, omega_b[i], color='k', marker='.')
+            plt.scatter(t, measuredOmega[i], color=color, marker='.')
+            plt.xlabel('angular velocity ' + label)
+
+        # plot acceleration in nav
+        acceleration_n = self.scenario.acceleration_n(t)
+        for i, (label, color) in enumerate(zip(self.labels, self.colors)):
+            plt.subplot(4, 3, i + 4)
+            plt.scatter(t, acceleration_n[i], color=color, marker='.')
+            plt.xlabel('acceleration in nav ' + label)
+
+        # plot acceleration in body
+        acceleration_b = self.scenario.acceleration_b(t)
+        for i, (label, color) in enumerate(zip(self.labels, self.colors)):
+            plt.subplot(4, 3, i + 7)
+            plt.scatter(t, acceleration_b[i], color=color, marker='.')
+            plt.xlabel('acceleration in body ' + label)
+
+        # plot actual specific force, as well as corrupted
+        actual = self.runner.actualSpecificForce(t)
+        for i, (label, color) in enumerate(zip(self.labels, self.colors)):
+            plt.subplot(4, 3, i + 10)
+            plt.scatter(t, actual[i], color='k', marker='.')
+            plt.scatter(t, measuredAcc[i], color=color, marker='.')
+            plt.xlabel('specific force ' + label)
+
+    def plotGroundTruthPose(self, t):
+        # plot ground truth pose, as well as prediction from integrated IMU measurements
+        actualPose = self.scenario.pose(t)
+        plot_pose3(POSES_FIG, actualPose, 0.3)
+        t = actualPose.translation()
+        self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), 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(0.01)
+
+    def run(self):
+        # simulate the loop
+        T = 12
+        for i, t in enumerate(np.arange(0, T, self.dt)):
+            measuredOmega = self.runner.measuredAngularVelocity(t)
+            measuredAcc = self.runner.measuredSpecificForce(t)
+            if i % 25 == 0:
+                self.plotImu(t, measuredOmega, measuredAcc)
+                self.plotGroundTruthPose(t)
+                pim = self.runner.integrate(t, self.actualBias, True)
+                predictedNavState = self.runner.predict(pim, self.actualBias)
+                plot_pose3(POSES_FIG, predictedNavState.pose(), 0.1)
+
+        plt.ioff()
+        plt.show()
+
+
+if __name__ == '__main__':
+    PreintegrationExample().run()

cython/gtsam/tests/testScenarioRunner.py

@@ -0,0 +1,47 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+ScenarioRunner unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import math
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestScenarioRunner(GtsamTestCase):
+    def setUp(self):
+        self.g = 10  # simple gravity constant
+
+    def test_loop_runner(self):
+        # Forward velocity 2m/s
+        # Pitch up with angular velocity 6 degree/sec (negative in FLU)
+        v = 2
+        w = math.radians(6)
+        W = np.array([0, -w, 0])
+        V = np.array([v, 0, 0])
+        scenario = gtsam.ConstantTwistScenario(W, V)
+
+        dt = 0.1
+        params = gtsam.PreintegrationParams.MakeSharedU(self.g)
+        bias = gtsam.imuBias_ConstantBias()
+        runner = gtsam.ScenarioRunner(
+            scenario, params, dt, bias)
+
+        # Test specific force at time 0: a is pointing up
+        t = 0.0
+        a = w * v
+        np.testing.assert_almost_equal(
+            np.array([0, 0, a + self.g]), runner.actualSpecificForce(t))
+
+
+if __name__ == '__main__':
+    unittest.main()

gtsam.h

@@ -2856,6 +2856,30 @@ virtual class AcceleratingScenario : gtsam::Scenario {
                        Vector omega_b);
 };
 
+#include <gtsam/navigation/ScenarioRunner.h>
+class ScenarioRunner {
+  ScenarioRunner(const gtsam::Scenario& scenario,
+                 const gtsam::PreintegrationParams* p,
+                 double imuSampleTime,
+                 const gtsam::imuBias::ConstantBias& bias);
+  Vector gravity_n() const;
+  Vector actualAngularVelocity(double t) const;
+  Vector actualSpecificForce(double t) const;
+  Vector measuredAngularVelocity(double t) const;
+  Vector measuredSpecificForce(double t) const;
+  double imuSampleTime() const;
+  gtsam::PreintegratedImuMeasurements integrate(
+      double T, const gtsam::imuBias::ConstantBias& estimatedBias,
+      bool corrupted) const;
+  gtsam::NavState predict(
+      const gtsam::PreintegratedImuMeasurements& pim,
+      const gtsam::imuBias::ConstantBias& estimatedBias) const;
+  Matrix estimateCovariance(
+      double T, size_t N,
+      const gtsam::imuBias::ConstantBias& estimatedBias) const;
+  Matrix estimateNoiseCovariance(size_t N) const;
+};
+
 //*************************************************************************
 // Utilities
 //*************************************************************************

gtsam/navigation/ImuFactor.cpp

@@ -235,8 +235,8 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
     const bool use2ndOrderCoriolis) :
 Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
     pose_j, vel_j, bias), _PIM_(pim) {
-  boost::shared_ptr<PreintegratedImuMeasurements::Params> p = boost::make_shared<
+  boost::shared_ptr<PreintegrationParams> p = boost::make_shared<
-  PreintegratedImuMeasurements::Params>(pim.p());
+  PreintegrationParams>(pim.p());
   p->n_gravity = n_gravity;
   p->omegaCoriolis = omegaCoriolis;
   p->body_P_sensor = body_P_sensor;

gtsam/navigation/ScenarioRunner.h

@@ -39,7 +39,7 @@ static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) {
 class ScenarioRunner {
  public:
   typedef imuBias::ConstantBias Bias;
-  typedef boost::shared_ptr<PreintegratedImuMeasurements::Params> SharedParams;
+  typedef boost::shared_ptr<PreintegrationParams> SharedParams;
 
  private:
   const Scenario& scenario_;

gtsam/navigation/tests/testImuFactor.cpp

@@ -803,11 +803,11 @@ TEST(ImuFactor, bodyPSensorWithBias) {
 static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6;
 
 struct ImuFactorMergeTest {
-  boost::shared_ptr<PreintegratedImuMeasurements::Params> p_;
+  boost::shared_ptr<PreintegrationParams> p_;
   const ConstantTwistScenario forward_, loop_;
 
   ImuFactorMergeTest()
-      : p_(PreintegratedImuMeasurements::Params::MakeSharedU(kGravity)),
+      : p_(PreintegrationParams::MakeSharedU(kGravity)),
         forward_(kZero, Vector3(kVelocity, 0, 0)),
         loop_(Vector3(0, -kAngularVelocity, 0), Vector3(kVelocity, 0, 0)) {
     // arbitrary noise values

gtsam/navigation/tests/testScenarios.cpp

@@ -34,7 +34,7 @@ static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3);
 static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias);
 
 // Create default parameters with Z-up and above noise parameters
-static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
+static boost::shared_ptr<PreintegrationParams> defaultParams() {
   auto p = PreintegrationParams::MakeSharedU(10);
   p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
   p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;