-
Notifications
You must be signed in to change notification settings - Fork 1
/
visualization.py
32 lines (26 loc) · 1.15 KB
/
visualization.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.cm import get_cmap
def setup_plot(ax):
if ax == None:
fig, ax = plt.subplots()
else:
fig = ax.get_figure()
return fig, ax
def plot_measurements(measurements_all, ax=None, cmap=get_cmap('Blues')):
fig, ax = setup_plot(ax)
time = np.array([measurements[0].timestamp for measurements in measurements_all])
interval = (time-time[0])/(1.*time[-1]-time[0])
for index, timestamp in enumerate(time):
color = cmap(interval[index])
[ax.plot(z.value[1], z.value[0], 'o',markeredgewidth=0.5, markeredgecolor="black", color=color) for z in measurements_all[index]]
return fig, ax
def plot_track_pos(track_file, ax=None, color='k', add_index=False):
fig, ax = setup_plot(ax)
for track_id, state_list in track_file.items():
states = np.array([est.est_posterior for est in state_list])
ax.plot(states[:,2], states[:,0], color=color, label="Track_ID: " + str(track_id))
ax.plot(states[0,2], states[0,0], 'o', color=color)
if add_index:
ax.text(states[-1,2], states[-1,0], str(track_id))
return fig, ax