def scroll(event):
if event.step != 0:
if self.display[
'scale'] - speed_scroll * event.step > scroll_limit:
self.display['scale'] = self.display[
'scale'] - speed_scroll * event.step
else:
self.display['scale'] = scroll_limit
center = -self.precision * np.array(
[0.5 * shape[1], 0.5 * shape[0], 0])
img, _ = self.extract_from_map(
self.display['gps_pos'] +
self.attitude.apply(self.display['pos'] + center, True),
self.attitude, shape, self.display['scale'])
self.display['img'].set_data(
increase_saturation(np.nan_to_num(img)))
if not self.display['overlay'] is None:
img_overlay = np.nan_to_num(
self.display['overlay'].predict_image(
self.display['gps_pos'] + self.attitude.apply(
self.display['pos'] + center, True), self.attitude,
(int(np.ceil(shape[0] / self.display['scale'])),
int(np.ceil(shape[1] / self.display['scale'])))))
overlay_red = np.zeros(
(np.shape(img_overlay)[0], np.shape(img_overlay)[1], 4))
overlay_red[:, :, 0] = img_overlay
overlay_red[:, :, 3] = (img_overlay != 0) * overlay_alpha * 255
self.display['overlay_fig'].set_data(
increase_saturation(overlay_red.astype(np.uint8)))
plt.draw()
def process_images(t):
center = -self.kalman.mapdata.precision * np.array(
[0.5 * shape[1], 0.5 * shape[0], 0])
gps_pos = projection(
self.kalman.mapdata.gps_pos, self.kalman.mapdata.attitude,
rbd_translate(self.get_positions(t), self.get_attitudes(t),
self.reader.tracklog_translation))
img, _ = self.kalman.mapdata.extract_from_map(
gps_pos + self.kalman.mapdata.attitude.apply(center, True),
self.kalman.mapdata.attitude, shape)
data = deepcopy(self.reader.get_radardata(t))
data.gps_pos, data.attitude = self.get_positions(
t), self.get_attitudes(t)
img_border = 255 * np.ones(np.shape(data.img))
img_border[border:-border,
border:-border] = data.img[border:-border,
border:-border]
data.img = img_border
img_overlay = np.nan_to_num(
data.predict_image(
gps_pos + self.kalman.mapdata.attitude.apply(center, True),
self.kalman.mapdata.attitude, shape))
error = np.min(
[np.linalg.norm(self.get_kalman_error(t)[0][0:2]), bar_scale])
return increase_saturation(
np.nan_to_num(img)), increase_saturation(img_overlay), error
def press(event):
if event.key == 'left':
self.display['pos'] = self.display['pos'] - np.array([
self.display['scale'] * speed_trans * self.precision, 0, 0
])
elif event.key == 'right':
self.display['pos'] = self.display['pos'] + np.array([
self.display['scale'] * speed_trans * self.precision, 0, 0
])
elif event.key == 'up':
self.display['pos'] = self.display['pos'] - np.array([
0, self.display['scale'] * speed_trans * self.precision, 0
])
elif event.key == 'down':
self.display['pos'] = self.display['pos'] + np.array([
0, self.display['scale'] * speed_trans * self.precision, 0
])
center = -self.precision * np.array(
[0.5 * shape[1], 0.5 * shape[0], 0])
img, _ = self.extract_from_map(
self.display['gps_pos'] +
self.attitude.apply(self.display['pos'] + center, True),
self.attitude, shape, self.display['scale'])
self.display['img'].set_data(
increase_saturation(np.nan_to_num(img)))
if not self.display['overlay'] is None:
img_overlay = np.nan_to_num(
self.display['overlay'].predict_image(
self.display['gps_pos'] + self.attitude.apply(
self.display['pos'] + center, True), self.attitude,
(int(np.ceil(shape[0] / self.display['scale'])),
int(np.ceil(shape[1] / self.display['scale'])))))
overlay_red = np.zeros(
(np.shape(img_overlay)[0], np.shape(img_overlay)[1], 4))
overlay_red[:, :, 0] = img_overlay
overlay_red[:, :, 3] = (img_overlay != 0) * overlay_alpha * 255
self.display['overlay_fig'].set_data(
increase_saturation(overlay_red.astype(np.uint8)))
self.display['text'].set_text(
str(round(self.display['pos'][0], 2)) + " ; " +
str(round(self.display['pos'][1], 2)))
plt.draw()
def update(t):
center = -self.kalman.mapdata.precision * np.array(
[0.5 * shape[1], 0.5 * shape[0], 0])
gps_pos = projection(
self.kalman.mapdata.gps_pos, self.kalman.mapdata.attitude,
rbd_translate(self.get_position(t), self.get_attitude(t),
self.reader.tracklog_translation))
img, _ = self.kalman.mapdata.extract_from_map(
gps_pos + self.kalman.mapdata.attitude.apply(center, True),
self.kalman.mapdata.attitude, shape)
data = deepcopy(self.reader.get_radardata(t))
data.gps_pos, data.gps_att = self.get_position(
t), self.get_attitude(t)
img_border = 255 * np.ones(np.shape(data.img))
img_border[border:-border,
border:-border] = data.img[border:-border,
border:-border]
data.img = img_border
img_overlay = np.nan_to_num(
data.predict_image(
gps_pos + self.kalman.mapdata.attitude.apply(center, True),
self.kalman.mapdata.attitude, shape))
overlay_red = np.zeros(
(np.shape(img_overlay)[0], np.shape(img_overlay)[1], 4))
overlay_red[:, :, 0] = img_overlay
overlay_red[:, :, 3] = (img_overlay != 0) * overlay_alpha * 255
return [
plt.imshow(increase_saturation(np.nan_to_num(img)),
cmap='gray',
vmin=0,
vmax=255,
zorder=1),
plt.imshow(increase_saturation(overlay_red.astype(np.uint8)),
alpha=0.5,
zorder=2,
interpolation=None),
plt.text(0.6, 0.5, str(round(t, 2)))
]
def show(self, gps_pos=None, overlay=None):
""" Show a matplotlib representation of the map
gps_pos: GPS pos where to show the map, origin of the map is used if not specified
"""
# Parameter for the map display
speed_trans = 100
speed_scroll = 0.1
shape = (1000, 2000)
scroll_limit = 0.4
overlay_alpha = 0.5
border = 2
def press(event):
if event.key == 'left':
self.display['pos'] = self.display['pos'] - np.array([
self.display['scale'] * speed_trans * self.precision, 0, 0
])
elif event.key == 'right':
self.display['pos'] = self.display['pos'] + np.array([
self.display['scale'] * speed_trans * self.precision, 0, 0
])
elif event.key == 'up':
self.display['pos'] = self.display['pos'] - np.array([
0, self.display['scale'] * speed_trans * self.precision, 0
])
elif event.key == 'down':
self.display['pos'] = self.display['pos'] + np.array([
0, self.display['scale'] * speed_trans * self.precision, 0
])
center = -self.precision * np.array(
[0.5 * shape[1], 0.5 * shape[0], 0])
img, _ = self.extract_from_map(
self.display['gps_pos'] +
self.attitude.apply(self.display['pos'] + center, True),
self.attitude, shape, self.display['scale'])
self.display['img'].set_data(
increase_saturation(np.nan_to_num(img)))
if not self.display['overlay'] is None:
img_overlay = np.nan_to_num(
self.display['overlay'].predict_image(
self.display['gps_pos'] + self.attitude.apply(
self.display['pos'] + center, True), self.attitude,
(int(np.ceil(shape[0] / self.display['scale'])),
int(np.ceil(shape[1] / self.display['scale'])))))
overlay_red = np.zeros(
(np.shape(img_overlay)[0], np.shape(img_overlay)[1], 4))
overlay_red[:, :, 0] = img_overlay
overlay_red[:, :, 3] = (img_overlay != 0) * overlay_alpha * 255
self.display['overlay_fig'].set_data(
increase_saturation(overlay_red.astype(np.uint8)))
self.display['text'].set_text(
str(round(self.display['pos'][0], 2)) + " ; " +
str(round(self.display['pos'][1], 2)))
plt.draw()
def scroll(event):
if event.step != 0:
if self.display[
'scale'] - speed_scroll * event.step > scroll_limit:
self.display['scale'] = self.display[
'scale'] - speed_scroll * event.step
else:
self.display['scale'] = scroll_limit
center = -self.precision * np.array(
[0.5 * shape[1], 0.5 * shape[0], 0])
img, _ = self.extract_from_map(
self.display['gps_pos'] +
self.attitude.apply(self.display['pos'] + center, True),
self.attitude, shape, self.display['scale'])
self.display['img'].set_data(
increase_saturation(np.nan_to_num(img)))
if not self.display['overlay'] is None:
img_overlay = np.nan_to_num(
self.display['overlay'].predict_image(
self.display['gps_pos'] + self.attitude.apply(
self.display['pos'] + center, True), self.attitude,
(int(np.ceil(shape[0] / self.display['scale'])),
int(np.ceil(shape[1] / self.display['scale'])))))
overlay_red = np.zeros(
(np.shape(img_overlay)[0], np.shape(img_overlay)[1], 4))
overlay_red[:, :, 0] = img_overlay
overlay_red[:, :, 3] = (img_overlay != 0) * overlay_alpha * 255
self.display['overlay_fig'].set_data(
increase_saturation(overlay_red.astype(np.uint8)))
plt.draw()
def close(event):
self.display['fig'] = None
missing = (self.display['fig'] is None)
if gps_pos is None:
self.display['gps_pos'] = deepcopy(self.gps_pos)
else:
if len(gps_pos) == 2:
self.display['gps_pos'] = deepcopy(self.gps_pos)
self.display['pos'] = np.append(gps_pos, 0)
else:
self.display['gps_pos'] = projection(self.gps_pos,
self.attitude, gps_pos)
if missing:
self.display['pos'] = np.array([0, 0, 0])
center = -self.precision * np.array(
[0.5 * shape[1], 0.5 * shape[0], 0])
if not overlay is None:
self.display['overlay'] = data_projection(self.gps_pos,
self.attitude, overlay)
img_border = 255 * np.ones(np.shape(overlay.img))
img_border[border:-border,
border:-border] = overlay.img[border:-border,
border:-border]
self.display['overlay'].img = img_border
else:
self.display['overlay'] = None
if missing:
plt.close(self.map_name)
if self.display['fig'] is None:
self.display['text'] = "0 ; 0"
self.display['scale'] = 1
self.display['fig'] = plt.figure(num=self.map_name,
facecolor=(1, 1, 1))
self.display['fig'].canvas.mpl_connect('key_press_event',
press)
self.display['fig'].canvas.mpl_connect('scroll_event', scroll)
self.display['fig'].canvas.mpl_connect('close_event', close)
self.display['axes'] = plt.axes()
self.display['axes'].set_facecolor("black")
self.display['axes'].get_xaxis().set_visible(False)
self.display['axes'].get_yaxis().set_visible(False)
if self.gps_pos is None:
img = np.nan * np.ones(shape)
else:
img, _ = self.extract_from_map(
self.display['gps_pos'] +
self.attitude.apply(self.display['pos'] + center, True),
self.attitude, shape, self.display['scale'])
self.display['img'] = self.display['axes'].imshow(
increase_saturation(np.nan_to_num(img)),
cmap='gray',
vmin=0,
vmax=255,
zorder=1)
if not self.display['overlay'] is None:
img_overlay = np.nan_to_num(
self.display['overlay'].predict_image(
self.display['gps_pos'] + self.attitude.apply(
self.display['pos'] + center, True), self.attitude,
(int(np.ceil(shape[0] / self.display['scale'])),
int(np.ceil(shape[1] / self.display['scale'])))))
overlay_red = np.zeros(
(np.shape(img_overlay)[0], np.shape(img_overlay)[1], 4))
overlay_red[:, :, 0] = img_overlay
overlay_red[:, :, 3] = (img_overlay != 0) * overlay_alpha * 255
self.display['overlay_fig'] = self.display['axes'].imshow(
increase_saturation(overlay_red.astype(np.uint8)),
alpha=0.5,
zorder=2,
interpolation=None)
self.display['text'] = self.display['axes'].text(
0,
0,
str(round(self.display['pos'][0], 2)) + " ; " +
str(round(self.display['pos'][1], 2)),
color='black',
horizontalalignment='left',
verticalalignment='top',
transform=self.display['axes'].transAxes)
plt.show()
else:
if self.gps_pos is None:
img = np.nan * np.ones(shape)
else:
img, _ = self.extract_from_map(
self.display['gps_pos'] +
self.attitude.apply(self.display['pos'] + center, True),
self.attitude, shape, self.display['scale'])
self.display['img'].set_data(
increase_saturation(np.nan_to_num(img)))
if not self.display['overlay'] is None:
img_overlay = np.nan_to_num(
self.display['overlay'].predict_image(
self.display['gps_pos'] + self.attitude.apply(
self.display['pos'] + center, True), self.attitude,
(int(np.ceil(shape[0] / self.display['scale'])),
int(np.ceil(shape[1] / self.display['scale'])))))
overlay_red = np.zeros(
(np.shape(img_overlay)[0], np.shape(img_overlay)[1], 4))
overlay_red[:, :, 0] = img_overlay
overlay_red[:, :, 3] = (img_overlay != 0) * overlay_alpha * 255
if self.display['overlay_fig'] is None:
self.display['overlay_fig'] = self.display['axes'].imshow(
increase_saturation(overlay_red.astype(np.uint8)),
alpha=0.5,
zorder=2)
else:
self.display['overlay_fig'].set_data(
increase_saturation(overlay_red.astype(np.uint8)))
else:
if not self.display['overlay_fig'] is None:
self.display['overlay_fig'].set_data(
np.zeros(
(int(np.ceil(shape[0] / self.display['scale'])),
int(np.ceil(shape[1] / self.display['scale'])),
4)))
self.display['overlay_fig'] = None
self.display['text'].set_text(
str(round(self.display['pos'][0], 2)) + " ; " +
str(round(self.display['pos'][1], 2)))
plt.draw()
plt.pause(0.001)