82 lines
2.5 KiB
Python
82 lines
2.5 KiB
Python
"""
|
|
A script validating the ImuFactor prediction and inference.
|
|
"""
|
|
|
|
import math
|
|
import matplotlib.pyplot as plt
|
|
import numpy as np
|
|
|
|
from mpl_toolkits.mplot3d import Axes3D
|
|
|
|
import gtsam
|
|
from gtsam_utils import plotPose3
|
|
|
|
class ImuFactorExample(object):
|
|
|
|
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(6)
|
|
W = np.array([0, -w, 0])
|
|
V = np.array([v, 0, 0])
|
|
self.scenario = gtsam.ConstantTwistScenario(W, V)
|
|
self.dt = 0.5
|
|
self.realTimeFactor = 10.0
|
|
|
|
# Calculate time to do 1 loop
|
|
self.radius = v / w
|
|
self.timeForOneLoop = 2 * math.pi / w
|
|
self.labels = list('xyz')
|
|
self.colors = list('rgb')
|
|
|
|
def plot(self, t, pose, omega_b, acceleration_n, acceleration_b):
|
|
# plot angular velocity
|
|
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=color, marker='.')
|
|
plt.xlabel(label)
|
|
|
|
# plot acceleration in nav
|
|
plt.figure(2)
|
|
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)
|
|
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
|
|
plotPose3(4, pose, 1.0)
|
|
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(self.dt / self.realTimeFactor)
|
|
|
|
def run(self):
|
|
# simulate the loop up to the top
|
|
for t in np.arange(0, self.timeForOneLoop, self.dt):
|
|
self.plot(t,
|
|
self.scenario.pose(t),
|
|
self.scenario.omega_b(t),
|
|
self.scenario.acceleration_n(t),
|
|
self.scenario.acceleration_b(t))
|
|
|
|
plt.ioff()
|
|
plt.show()
|
|
|
|
if __name__ == '__main__':
|
|
ImuFactorExample().run()
|