PreintegrationExample.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 
6 See LICENSE for the license information
7 
8 A script validating the Preintegration of IMU measurements.
9 
10 Authors: Frank Dellaert, Varun Agrawal.
11 """
12 
13 # pylint: disable=invalid-name,unused-import,wrong-import-order
14 
15 from typing import Optional, Sequence
16 
17 import gtsam
18 import matplotlib.pyplot as plt
19 import numpy as np
20 from gtsam.utils.plot import plot_pose3
21 from mpl_toolkits.mplot3d import Axes3D
22 
23 IMU_FIG = 1
24 POSES_FIG = 2
25 GRAVITY = 10
26 
27 
29  """Base class for all preintegration examples."""
30  @staticmethod
31  def defaultParams(g: float):
32  """Create default parameters with Z *up* and realistic noise parameters"""
34  kGyroSigma = np.radians(0.5) / 60 # 0.5 degree ARW
35  kAccelSigma = 0.1 / 60 # 10 cm VRW
36  params.setGyroscopeCovariance(kGyroSigma**2 * np.identity(3, float))
37  params.setAccelerometerCovariance(kAccelSigma**2 *
38  np.identity(3, float))
39  params.setIntegrationCovariance(0.0000001**2 * np.identity(3, float))
40  return params
41 
42  def __init__(self,
43  twist: Optional[np.ndarray] = None,
44  bias: Optional[gtsam.imuBias.ConstantBias] = None,
45  params: Optional[gtsam.PreintegrationParams] = None,
46  dt: float = 1e-2):
47  """Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
48 
49  # setup interactive plotting
50  plt.ion()
51 
52  # Setup loop as default scenario
53  if twist is not None:
54  (W, V) = twist
55  else:
56  # default = loop with forward velocity 2m/s, while pitching up
57  # with angular velocity 30 degree/sec (negative in FLU)
58  W = np.array([0, -np.radians(30), 0])
59  V = np.array([2, 0, 0])
60 
62  self.dt = dt
63 
64  self.maxDim = 5
65  self.labels = list('xyz')
66  self.colors = list('rgb')
67 
68  if params:
69  self.params = params
70  else:
71  # Default params with simple gravity constant
72  self.params = self.defaultParams(g=GRAVITY)
73 
74  if bias is not None:
75  self.actualBias = bias
76  else:
77  accBias = np.array([0, 0.1, 0])
78  gyroBias = np.array([0, 0, 0])
79  self.actualBias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
80 
81  # Create runner
82  self.runner = gtsam.ScenarioRunner(self.scenario, self.params, self.dt,
83  self.actualBias)
84 
85  fig, self.axes = plt.subplots(4, 3)
86  fig.set_tight_layout(True)
87 
88  def plotImu(self, t: float, measuredOmega: Sequence,
89  measuredAcc: Sequence):
90  """
91  Plot IMU measurements.
92  Args:
93  t: The time at which the measurement was recoreded.
94  measuredOmega: Measured angular velocity.
95  measuredAcc: Measured linear acceleration.
96  """
97  plt.figure(IMU_FIG)
98 
99  # plot angular velocity
100  omega_b = self.scenario.omega_b(t)
101  for i, (label, color) in enumerate(zip(self.labels, self.colors)):
102  ax = self.axes[0][i]
103  ax.scatter(t, omega_b[i], color='k', marker='.')
104  ax.scatter(t, measuredOmega[i], color=color, marker='.')
105  ax.set_xlabel('angular velocity ' + label)
106 
107  # plot acceleration in nav
108  acceleration_n = self.scenario.acceleration_n(t)
109  for i, (label, color) in enumerate(zip(self.labels, self.colors)):
110  ax = self.axes[1][i]
111  ax.scatter(t, acceleration_n[i], color=color, marker='.')
112  ax.set_xlabel('acceleration in nav ' + label)
113 
114  # plot acceleration in body
115  acceleration_b = self.scenario.acceleration_b(t)
116  for i, (label, color) in enumerate(zip(self.labels, self.colors)):
117  ax = self.axes[2][i]
118  ax.scatter(t, acceleration_b[i], color=color, marker='.')
119  ax.set_xlabel('acceleration in body ' + label)
120 
121  # plot actual specific force, as well as corrupted
122  actual = self.runner.actualSpecificForce(t)
123  for i, (label, color) in enumerate(zip(self.labels, self.colors)):
124  ax = self.axes[3][i]
125  ax.scatter(t, actual[i], color='k', marker='.')
126  ax.scatter(t, measuredAcc[i], color=color, marker='.')
127  ax.set_xlabel('specific force ' + label)
128 
130  t: float,
131  scale: float = 0.3,
132  time_interval: float = 0.01):
133  """
134  Plot ground truth pose, as well as prediction from integrated IMU measurements.
135  Args:
136  t: Time at which the pose was obtained.
137  scale: The scaling factor for the pose axes.
138  time_interval: The time to wait before showing the plot.
139  """
140  actualPose = self.scenario.pose(t)
141  plot_pose3(POSES_FIG, actualPose, scale)
142  translation = actualPose.translation()
143  self.maxDim = max([max(np.abs(translation)), self.maxDim])
144  ax = plt.gca()
145  ax.set_xlim3d(-self.maxDim, self.maxDim)
146  ax.set_ylim3d(-self.maxDim, self.maxDim)
147  ax.set_zlim3d(-self.maxDim, self.maxDim)
148 
149  plt.pause(time_interval)
150 
151  def run(self, T: int = 12):
152  """Simulate the loop."""
153  for i, t in enumerate(np.arange(0, T, self.dt)):
154  measuredOmega = self.runner.measuredAngularVelocity(t)
155  measuredAcc = self.runner.measuredSpecificForce(t)
156  if i % 25 == 0:
157  self.plotImu(t, measuredOmega, measuredAcc)
158  self.plotGroundTruthPose(t)
159  pim = self.runner.integrate(t, self.actualBias, True)
160  predictedNavState = self.runner.predict(pim, self.actualBias)
161  plot_pose3(POSES_FIG, predictedNavState.pose(), 0.1)
162 
163  plt.ioff()
164  plt.show()
165 
166 
167 if __name__ == '__main__':
gtsam::utils.plot.plot_pose3
plt.Figure plot_pose3(int fignum, Pose3 pose, float axis_length=0.1, np.ndarray P=None, Iterable[str] axis_labels=("X axis", "Y axis", "Z axis"))
Definition: plot.py:436
gtsam.examples.PreintegrationExample.PreintegrationExample.__init__
def __init__(self, Optional[np.ndarray] twist=None, Optional[gtsam.imuBias.ConstantBias] bias=None, Optional[gtsam.PreintegrationParams] params=None, float dt=1e-2)
Definition: PreintegrationExample.py:42
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
list
Definition: pytypes.h:2166
gtsam::PreintegrationParams
Definition: PreintegrationParams.h:25
gtsam::ScenarioRunner
Definition: ScenarioRunner.h:40
gtsam::ConstantTwistScenario
Definition: Scenario.h:60
gtsam.examples.PreintegrationExample.PreintegrationExample.defaultParams
def defaultParams(float g)
Definition: PreintegrationExample.py:31
gtsam.examples.PreintegrationExample.PreintegrationExample.runner
runner
Definition: PreintegrationExample.py:78
gtsam.examples.PreintegrationExample.PreintegrationExample.colors
colors
Definition: PreintegrationExample.py:62
gtsam.examples.PreintegrationExample.PreintegrationExample.plotImu
def plotImu(self, float t, Sequence measuredOmega, Sequence measuredAcc)
Definition: PreintegrationExample.py:88
gtsam.examples.PreintegrationExample.PreintegrationExample.dt
dt
Definition: PreintegrationExample.py:58
gtsam.examples.PreintegrationExample.PreintegrationExample.run
def run(self, int T=12)
Definition: PreintegrationExample.py:151
gtsam.examples.PreintegrationExample.PreintegrationExample.actualBias
actualBias
Definition: PreintegrationExample.py:71
gtsam.examples.PreintegrationExample.PreintegrationExample.labels
labels
Definition: PreintegrationExample.py:61
gtsam.examples.PreintegrationExample.PreintegrationExample.params
params
Definition: PreintegrationExample.py:65
gtsam.examples.PreintegrationExample.PreintegrationExample.scenario
scenario
Definition: PreintegrationExample.py:57
gtsam.examples.PreintegrationExample.PreintegrationExample.axes
axes
Definition: PreintegrationExample.py:81
gtsam::PreintegrationParams::MakeSharedU
static std::shared_ptr< PreintegrationParams > MakeSharedU(double g=9.81)
Definition: PreintegrationParams.h:56
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
gtsam.examples.PreintegrationExample.PreintegrationExample
Definition: PreintegrationExample.py:28
gtsam.examples.PreintegrationExample.PreintegrationExample.plotGroundTruthPose
def plotGroundTruthPose(self, float t, float scale=0.3, float time_interval=0.01)
Definition: PreintegrationExample.py:129
gtsam.examples.PreintegrationExample.PreintegrationExample.maxDim
maxDim
Definition: PreintegrationExample.py:60
max
#define max(a, b)
Definition: datatypes.h:20
gtsam::utils.plot
Definition: plot.py:1


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:25