From 1ba304a2e368283db5eef0520f11c52e814d2979 Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 27 Jan 2016 12:03:26 -0800 Subject: [PATCH] Moved preintegration into separate example, inherit from it --- python/gtsam_examples/ImuFactorExample.py | 98 +------------- .../gtsam_examples/PreintegrationExample.py | 120 ++++++++++++++++++ 2 files changed, 126 insertions(+), 92 deletions(-) create mode 100644 python/gtsam_examples/PreintegrationExample.py diff --git a/python/gtsam_examples/ImuFactorExample.py b/python/gtsam_examples/ImuFactorExample.py index c2432673e..6ab5c1211 100644 --- a/python/gtsam_examples/ImuFactorExample.py +++ b/python/gtsam_examples/ImuFactorExample.py @@ -1,5 +1,5 @@ """ -A script validating the ImuFactor prediction and inference. +A script validating the ImuFactor inference. """ import math @@ -10,106 +10,20 @@ from mpl_toolkits.mplot3d import Axes3D import gtsam from gtsam_utils import plotPose3 +from PreintegrationExample import PreintegrationExample -class ImuFactorExample(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.gyroscopeCovariance = kGyroSigma ** 2 * np.identity(3, np.float) - params.accelerometerCovariance = kAccelSigma ** 2 * np.identity(3, np.float) - params.integrationCovariance = 0.0000001 ** 2 * np.identity(3, np.float) - return params - - def __init__(self): - # setup interactive plotting - plt.ion() - - # Setup loop scenario - # Forward velocity 2m/s - # Pitch up with angular velocity 6 degree/sec (negative in FLU) - v = 2 - w = math.radians(30) - W = np.array([0, -w, 0]) - V = np.array([v, 0, 0]) - self.scenario = gtsam.ConstantTwistScenario(W, V) - self.dt = 1e-2 - - # Calculate time to do 1 loop - self.radius = v / w - self.timeForOneLoop = 2.0 * math.pi / w - self.labels = list('xyz') - self.colors = list('rgb') - - # Create runner - self.g = 10 # simple gravity constant - self.params = self.defaultParams(self.g) - ptr = gtsam.ScenarioPointer(self.scenario) - accBias = np.array([0, 0.1, 0]) - gyroBias = np.array([0, 0, 0]) - self.actualBias = gtsam.ConstantBias(accBias, gyroBias) - print(self.actualBias) - self.runner = gtsam.ScenarioRunner(ptr, self.params, self.dt, self.actualBias) - - def plot(self, t, measuredOmega, measuredAcc): - # plot angular velocity - omega_b = self.scenario.omega_b(t) - plt.figure(1) - for i, (label, color) in enumerate(zip(self.labels, self.colors)): - plt.subplot(3, 1, i + 1) - plt.scatter(t, omega_b[i], color='k', marker='.') - plt.scatter(t, measuredOmega[i], color=color, marker='.') - plt.xlabel(label) - - # plot acceleration in nav - plt.figure(2) - acceleration_n = self.scenario.acceleration_n(t) - for i, (label, color) in enumerate(zip(self.labels, self.colors)): - plt.subplot(3, 1, i + 1) - plt.scatter(t, acceleration_n[i], color=color, marker='.') - plt.xlabel(label) - - # plot acceleration in body - plt.figure(3) - acceleration_b = self.scenario.acceleration_b(t) - for i, (label, color) in enumerate(zip(self.labels, self.colors)): - plt.subplot(3, 1, i + 1) - plt.scatter(t, acceleration_b[i], color=color, marker='.') - plt.xlabel(label) - - # plot ground truth pose, as well as prediction from integrated IMU measurements - actualPose = self.scenario.pose(t) - plotPose3(4, actualPose, 0.3) - pim = self.runner.integrate(t, self.actualBias, True) - predictedNavState = self.runner.predict(pim, self.actualBias) - plotPose3(4, predictedNavState.pose(), 0.1) - ax = plt.gca() - ax.set_xlim3d(-self.radius, self.radius) - ax.set_ylim3d(-self.radius, self.radius) - ax.set_zlim3d(0, self.radius * 2) - - # plot actual specific force, as well as corrupted - plt.figure(5) - actual = self.runner.actualSpecificForce(t) - for i, (label, color) in enumerate(zip(self.labels, self.colors)): - plt.subplot(3, 1, i + 1) - plt.scatter(t, actual[i], color='k', marker='.') - plt.scatter(t, measuredAcc[i], color=color, marker='.') - plt.xlabel(label) - - plt.pause(0.01) +class ImuFactorExample(PreintegrationExample): def run(self): # simulate the loop up to the top T = self.timeForOneLoop + pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) 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.plot(t, measuredOmega, measuredAcc) + self.plotImu(t, measuredOmega, measuredAcc) + self.plotGroundTruthPose(t) plt.ioff() plt.show() diff --git a/python/gtsam_examples/PreintegrationExample.py b/python/gtsam_examples/PreintegrationExample.py new file mode 100644 index 000000000..db0ce4363 --- /dev/null +++ b/python/gtsam_examples/PreintegrationExample.py @@ -0,0 +1,120 @@ +""" +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 import plotPose3 + +IMU_FIG = 1 +GROUND_TRUTH_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.gyroscopeCovariance = kGyroSigma ** 2 * np.identity(3, np.float) + params.accelerometerCovariance = kAccelSigma ** 2 * np.identity(3, np.float) + params.integrationCovariance = 0.0000001 ** 2 * np.identity(3, np.float) + return params + + def __init__(self): + # setup interactive plotting + plt.ion() + + # Setup loop scenario + # Forward velocity 2m/s + # Pitch up with angular velocity 6 degree/sec (negative in FLU) + v = 2 + w = math.radians(30) + W = np.array([0, -w, 0]) + V = np.array([v, 0, 0]) + self.scenario = gtsam.ConstantTwistScenario(W, V) + self.dt = 1e-2 + + # Calculate time to do 1 loop + self.radius = v / w + self.timeForOneLoop = 2.0 * math.pi / w + self.labels = list('xyz') + self.colors = list('rgb') + + # Create runner + self.g = 10 # simple gravity constant + self.params = self.defaultParams(self.g) + ptr = gtsam.ScenarioPointer(self.scenario) + accBias = np.array([0, 0.1, 0]) + gyroBias = np.array([0, 0, 0]) + self.actualBias = gtsam.ConstantBias(accBias, gyroBias) + self.runner = gtsam.ScenarioRunner(ptr, 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) + plotPose3(GROUND_TRUTH_FIG, actualPose, 0.3) + ax = plt.gca() + ax.set_xlim3d(-self.radius, self.radius) + ax.set_ylim3d(-self.radius, self.radius) + ax.set_zlim3d(0, self.radius * 2) + + plt.pause(0.01) + + def run(self): + # simulate the loop up to the top + T = self.timeForOneLoop + 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) + plotPose3(GROUND_TRUTH_FIG, predictedNavState.pose(), 0.1) + + plt.ioff() + plt.show() + +if __name__ == '__main__': + PreintegrationExample().run()