diff --git a/cython/gtsam/examples/ImuFactorExample.py b/cython/gtsam/examples/ImuFactorExample.py new file mode 100644 index 000000000..6cc8979f1 --- /dev/null +++ b/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() diff --git a/cython/gtsam/examples/PreintegrationExample.py b/cython/gtsam/examples/PreintegrationExample.py new file mode 100644 index 000000000..76520b355 --- /dev/null +++ b/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() diff --git a/cython/gtsam/tests/testScenarioRunner.py b/cython/gtsam/tests/testScenarioRunner.py new file mode 100644 index 000000000..97a97b0ec --- /dev/null +++ b/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()