def plot_particle_proposer(self, sess, batch, proposed_particles, task):
# define the inputs and train/run the model
input_dict = {
**{self.placeholders[key]: batch[key]
for key in 'osa'},
**{
self.placeholders['num_particles']: 100
},
}
s_samples = sess.run(proposed_particles, input_dict)
plt.figure('Particle Proposer')
plt.gca().clear()
plot_maze(task)
for i in range(min(len(s_samples), 10)):
color = np.random.uniform(0.0, 1.0, 3)
plt.quiver(s_samples[i, :, 0],
s_samples[i, :, 1],
np.cos(s_samples[i, :, 2]),
np.sin(s_samples[i, :, 2]),
color=color,
width=0.001,
scale=100)
plt.quiver(batch['s'][i, 0, 0],
batch['s'][i, 0, 1],
np.cos(batch['s'][i, 0, 2]),
np.sin(batch['s'][i, 0, 2]),
color=color,
scale=50,
width=0.003)
plt.pause(0.01)
def plot_proposer(session, method, statistics, batch, task, num_examples,
variant):
num_particles = 1000
proposer_out = method.propose_particles(method.encodings[0, :],
num_particles,
statistics['state_mins'],
statistics['state_maxs'])
# define the inputs and train/run the model
input_dict = {
**{method.placeholders[key]: batch[key]
for key in 'osa'},
}
particles = session.run(proposer_out, input_dict)
for i in range(num_examples):
fig = plt.figure(figsize=(2.4, 1.29 / 0.9),
num="%s %s proposer" % (variant, i))
# plt.gca().clear()
plot_maze(task, margin=5, linewidth=0.5)
quiv = plt.quiver(particles[i, :, 0],
particles[i, :, 1],
np.cos(particles[i, :, 2]),
np.sin(particles[i, :, 2]),
np.ones([num_particles]),
cmap='viridis_r',
clim=[0, 2],
alpha=1.0,
**quiv_kwargs)
plt.quiver([batch['s'][0, i, 0]], [batch['s'][0, i, 1]],
np.cos([batch['s'][0, i, 2]]),
np.sin([batch['s'][0, i, 2]]),
color='red',
**quiv_kwargs) # width=0.01, scale=100
plt.plot([batch['s'][0, i, 0]], [batch['s'][0, i, 1]], 'or',
**marker_kwargs)
plt.gca().axis('off')
plt.subplots_adjust(left=0.0,
bottom=0.05,
right=1.0,
top=0.95,
wspace=0.0,
hspace=0.00)
# plt.subplots_adjust(left=0.0, bottom=0.0, right=1.0, top=1.0, wspace=0.001, hspace=0.1)
plt.savefig('../plots/models/prop{}.pdf'.format(i),
transparent=True,
dpi=600,
frameon=False,
facecolor='w',
pad_inches=0.01)
def plot_motion_model(self, sess, batch, motion_samples, task):
# define the inputs and train/run the model
input_dict = {
**{self.placeholders[key]: batch[key]
for key in 'osa'},
**{
self.placeholders['num_particles']: 100
},
}
s_motion_samples = sess.run(motion_samples, input_dict)
plt.figure('Motion Model')
plt.gca().clear()
plot_maze(task)
for i in range(min(len(s_motion_samples), 10)):
plt.quiver(s_motion_samples[i, :, 0],
s_motion_samples[i, :, 1],
np.cos(s_motion_samples[i, :, 2]),
np.sin(s_motion_samples[i, :, 2]),
color='blue',
width=0.001,
scale=100)
plt.quiver(batch['s'][i, 0, 0],
batch['s'][i, 0, 1],
np.cos(batch['s'][i, 0, 2]),
np.sin(batch['s'][i, 0, 2]),
color='black',
scale=50,
width=0.003)
plt.quiver(batch['s'][i, 1, 0],
batch['s'][i, 1, 1],
np.cos(batch['s'][i, 1, 2]),
np.sin(batch['s'][i, 1, 2]),
color='red',
scale=50,
width=0.003)
plt.gca().set_aspect('equal')
plt.pause(0.01)
# np.random.seed(11)
# nav02: i=108
i = 108
# for i in range(100, 120):
np.random.seed(i)
dat = shuffle_data(data['train'])
dat = reduce_data(dat, 1)
dat = noisyfy_data(dat)
plot_trajectory(dat,
figure_name=None,
emphasize=0,
mincolor=0.0,
linewidth=0.5)
plot_maze(task)
# plot_trajectories(data['val'], figure_name='2', emphasize=None, mincolor=0.0, linewidth=0.5)
# plot_maze(task)
plt.tick_params(top='off',
bottom='off',
left='off',
right='off',
labelleft='off',
labelbottom='off')
plt.tight_layout()
# plt.savefig("../plots/"+task +".png",
# bbox_inches='tight',
# transparent=False,
# pad_inches=0,
# dpi=200)
def plot_particle_filter(self, sess, batch, particle_list,
particle_probs_list, num_particles,
state_step_sizes, task):
num_particles = 1000
head_scale = 1.5
quiv_kwargs = {
'scale_units': 'xy',
'scale': 1. / 40.,
'width': 0.003,
'headlength': 5 * head_scale,
'headwidth': 3 * head_scale,
'headaxislength': 4.5 * head_scale
}
marker_kwargs = {
'markersize': 4.5,
'markerfacecolor': 'None',
'markeredgewidth': 0.5
}
color_list = plt.cm.tab10(np.linspace(0, 1, 10))
colors = {
'lstm': color_list[0],
'pf_e2e': color_list[1],
'pf_ind_e2e': color_list[2],
'pf_ind': color_list[3],
'ff': color_list[4],
'odom': color_list[4]
}
pred, s_particle_list, s_particle_probs_list = self.predict(
sess, batch, num_particles, return_particles=True)
num_steps = 20 # s_particle_list.shape[1]
for s in range(1):
plt.figure("example {}".format(s), figsize=[12, 5.15])
plt.gca().clear()
for i in range(num_steps):
ax = plt.subplot(4, 5, i + 1, frameon=False)
plt.gca().clear()
plot_maze(task, margin=5, linewidth=0.5)
if i < num_steps - 1:
ax.quiver(s_particle_list[s, i, :, 0],
s_particle_list[s, i, :, 1],
np.cos(s_particle_list[s, i, :, 2]),
np.sin(s_particle_list[s, i, :, 2]),
s_particle_probs_list[s, i, :],
cmap='viridis_r',
clim=[.0, 2.0 / num_particles],
alpha=1.0,
**quiv_kwargs)
current_state = batch['s'][s, i, :]
plt.quiver(current_state[0],
current_state[1],
np.cos(current_state[2]),
np.sin(current_state[2]),
color="red",
**quiv_kwargs)
plt.plot(current_state[0], current_state[1], 'or',
**marker_kwargs)
else:
ax.plot(batch['s'][s, :num_steps, 0],
batch['s'][s, :num_steps, 1],
'-',
linewidth=0.6,
color='red')
ax.plot(pred[s, :num_steps, 0],
pred[s, :num_steps, 1],
'-',
linewidth=0.6,
color=colors['pf_ind_e2e'])
ax.plot(batch['s'][s, :1, 0],
batch['s'][s, :1, 1],
'.',
linewidth=0.6,
color='red',
markersize=3)
ax.plot(pred[s, :1, 0],
pred[s, :1, 1],
'.',
linewidth=0.6,
markersize=3,
color=colors['pf_ind_e2e'])
plt.subplots_adjust(left=0.0,
bottom=0.0,
right=1.0,
top=1.0,
wspace=0.001,
hspace=0.1)
plt.gca().set_aspect('equal')
plt.xticks([])
plt.yticks([])
show_pause(pause=0.01)
def plot_prediction(pred1, pred2, statistics, batch, task, num_examples,
variant):
color_list = plt.cm.tab10(np.linspace(0, 1, 10))
colors = {
'lstm': color_list[0],
'pf_e2e': color_list[1],
'pf_ind_e2e': color_list[2],
'pf_ind': color_list[3],
'ff': color_list[4],
'odom': color_list[4]
}
num_steps = 50
init_steps = 20
for s in range(num_examples):
fig = plt.figure(figsize=(2.4, 1.29),
num="%s prediction %s" % (variant, s))
# plt.figure("example {}, vartiant: {}".format(s, variant), figsize=[12, 5.15])
plt.gca().clear()
plot_maze(task, margin=5, linewidth=0.5)
plt.plot(batch['s'][s, :num_steps, 0],
batch['s'][s, :num_steps, 1],
'-',
linewidth=0.3,
color='gray')
plt.plot(pred1[s, :init_steps, 0],
pred1[s, :init_steps, 1],
'--',
linewidth=0.3,
color=colors['pf_ind_e2e'])
plt.plot(pred1[s, init_steps - 1:num_steps, 0],
pred1[s, init_steps - 1:num_steps, 1],
'-',
linewidth=0.3,
color=colors['pf_ind_e2e'])
plt.plot(pred2[s, :init_steps, 0],
pred2[s, :init_steps, 1],
'--',
linewidth=0.3,
color=colors['lstm'])
plt.plot(pred2[s, init_steps - 1:num_steps, 0],
pred2[s, init_steps - 1:num_steps, 1],
'-',
color=colors['lstm'],
linewidth=0.3)
# for i in range(init_steps, num_steps):
#
# p = pred1[s, i, :]
# plt.quiver(p[0], p[1], np.cos(p[2]),
# np.sin(p[2]), color=colors['pf_ind_e2e'], **quiv_kwargs)
# p = pred2[s, i, :]
# plt.quiver(p[0], p[1], np.cos(p[2]),
# np.sin(p[2]), color=colors['lstm'], **quiv_kwargs)
# # plt.plot(p[0], p[1], 'og', **marker_kwargs)
#
# current_state = batch['s'][s, i, :]
# plt.quiver(current_state[0], current_state[1], np.cos(current_state[2]),
# np.sin(current_state[2]), color="black", **quiv_kwargs)
# # plt.plot(current_state[0], current_state[1], 'or', **marker_kwargs)
plt.gca().set_aspect('equal')
plt.xticks([])
plt.yticks([])
plt.subplots_adjust(left=0.0,
bottom=0.0,
right=1.0,
top=1.0,
wspace=0.001,
hspace=0.1)
plt.savefig('../plots/models/pred{}.pdf'.format(s),
transparent=True,
dpi=600,
frameon=False,
facecolor='w',
pad_inches=0.01)
def plot_measurement_model(session, method, statistics, batch, task,
num_examples, variant):
batch_size = len(batch['o'])
x = np.linspace(100.0 / 4, 1000.0 - 100.0 / 4, 20)
y = np.linspace(100.0 / 4, 500.0 - 100.0 / 4, 10)
theta = np.linspace(-np.pi, np.pi, 12 + 1)[1:]
g = np.meshgrid(x, y, theta)
poses = np.vstack([np.ravel(x) for x in g]).transpose([1, 0])
test_poses = tf.tile(
tf.constant(poses, dtype='float32')[None, :, :], [batch_size, 1, 1])
measurement_model_out = method.measurement_update(method.encodings[0, :],
test_poses,
statistics['means'],
statistics['stds'])
# define the inputs and train/run the model
input_dict = {
**{method.placeholders[key]: batch[key]
for key in 'osa'},
}
obs_likelihood = session.run(measurement_model_out, input_dict)
print(obs_likelihood.shape)
for i in range(num_examples):
# plt.figure("%s likelihood" % i)
fig, (ax,
cax) = plt.subplots(1,
2,
figsize=(2.4 / 0.83 / 0.95 / 0.97,
1.29 / 0.9),
gridspec_kw={"width_ratios": [0.97, 0.03]},
num="%s %s likelihood" % (variant, i))
# plt.gca().clear()
plot_maze(task, margin=5, linewidth=0.5, ax=ax)
idx = obs_likelihood[i, :] > 1 * np.mean(obs_likelihood[i, :])
# idx = obs_likelihood[i, :] > 0 * np.mean(obs_likelihood[i, :])
max = np.max(obs_likelihood[i, :])
# ax.scatter([poses[:, 0]], [poses[:, 1]], s=[0.001], c=[(0.8, 0.8, 0.8)], marker='.')
quiv = ax.quiver(poses[idx, 0] + 0 * np.cos(poses[idx, 2]),
poses[idx, 1] + 0 * np.sin(poses[idx, 2]),
np.cos(poses[idx, 2]),
np.sin(poses[idx, 2]),
obs_likelihood[i, idx],
cmap='viridis_r',
clim=[0.0, max],
**quiv_kwargs)
ax.plot([batch['s'][0, i, 0]], [batch['s'][0, i, 1]], 'or',
**marker_kwargs)
ax.quiver([batch['s'][0, i, 0]], [batch['s'][0, i, 1]],
np.cos([batch['s'][0, i, 2]]),
np.sin([batch['s'][0, i, 2]]),
color='red',
**quiv_kwargs)
ax.axis('off')
fig.colorbar(quiv,
cax=cax,
orientation="vertical",
label='Obs. likelihood',
ticks=[0.0, 0.2, 0.4, 0.6, 0.8, 1.0])
plt.subplots_adjust(left=0.0,
bottom=0.05,
right=0.83,
top=0.95,
wspace=0.05,
hspace=0.00)
plt.savefig('../plots/models/measurement_model{}.pdf'.format(i),
transparent=True,
dpi=600,
frameon=False,
facecolor='w',
pad_inches=0.01)
def plot_particle_filter(session, method, statistics, batch, task,
num_examples, num_particles, variant):
color_list = plt.cm.tab10(np.linspace(0, 1, 10))
colors = {
'lstm': color_list[0],
'pf_e2e': color_list[1],
'pf_ind_e2e': color_list[2],
'pf_ind': color_list[3],
'ff': color_list[4],
'odom': color_list[4]
}
pred, s_particle_list, s_particle_probs_list = method.predict(
session, batch, num_particles, return_particles=True)
num_steps = 20 # s_particle_list.shape[1]
for s in range(num_examples):
plt.figure("example {}, vartiant: {}".format(s, variant),
figsize=[12, 5.15])
plt.gca().clear()
for i in range(num_steps):
ax = plt.subplot(4, 5, i + 1, frameon=False)
plt.gca().clear()
plot_maze(task, margin=5, linewidth=0.5)
if i < num_steps - 1:
ax.quiver(s_particle_list[s, i, :, 0],
s_particle_list[s, i, :, 1],
np.cos(s_particle_list[s, i, :, 2]),
np.sin(s_particle_list[s, i, :, 2]),
s_particle_probs_list[s, i, :],
cmap='viridis_r',
clim=[.0, 2.0 / num_particles],
alpha=1.0,
**quiv_kwargs)
current_state = batch['s'][s, i, :]
plt.quiver(current_state[0],
current_state[1],
np.cos(current_state[2]),
np.sin(current_state[2]),
color="red",
**quiv_kwargs)
plt.plot(current_state[0], current_state[1], 'or',
**marker_kwargs)
else:
ax.plot(batch['s'][s, :num_steps, 0],
batch['s'][s, :num_steps, 1],
'-',
linewidth=0.6,
color='red')
ax.plot(pred[s, :num_steps, 0],
pred[s, :num_steps, 1],
'-',
linewidth=0.6,
color=colors['pf_ind_e2e'])
ax.plot(batch['s'][s, :1, 0],
batch['s'][s, :1, 1],
'.',
linewidth=0.6,
color='red',
markersize=3)
ax.plot(pred[s, :1, 0],
pred[s, :1, 1],
'.',
linewidth=0.6,
markersize=3,
color=colors['pf_ind_e2e'])
plt.subplots_adjust(left=0.0,
bottom=0.0,
right=1.0,
top=1.0,
wspace=0.001,
hspace=0.1)
plt.gca().set_aspect('equal')
plt.xticks([])
plt.yticks([])
plt.savefig('../plots/models/pf{}.pdf'.format(s),
transparent=True,
dpi=600,
frameon=False,
facecolor='w',
pad_inches=0.01)
plt.figure('colorbar', (12, 0.6))
a = np.array([[0, 2.0 / num_particles]])
img = plt.imshow(a, cmap="viridis_r")
plt.gca().set_visible(False)
cax = plt.axes([0.25, 0.75, 0.50, 0.2])
plt.colorbar(orientation="horizontal",
cax=cax,
label='Particle weight',
ticks=[0, 0.001, 0.002])
plt.savefig('../plots/models/colorbar.pdf'.format(s),
transparent=True,
dpi=600,
frameon=False,
facecolor='w',
pad_inches=0.01)
def plot_motion_model(session, method, statistics, batch, task, num_examples,
num_particles, variant):
motion_samples = method.motion_update(
method.placeholders['a'][:, 1],
tf.tile(method.placeholders['s'][:, :1],
[1, num_particles, 1]), statistics['means'],
statistics['stds'], statistics['state_step_sizes'])
# define the inputs and train/run the model
input_dict = {
**{method.placeholders[key]: batch[key]
for key in 'osa'},
}
particles = session.run(motion_samples, input_dict)
fig = plt.figure(figsize=(2.4, 1.29), num="%s motion model" % (variant))
# plt.gca().clear()
plot_maze(task, margin=5, linewidth=0.5)
for i in range(num_examples):
plt.quiver(particles[i, :, 0],
particles[i, :, 1],
np.cos(particles[i, :, 2]),
np.sin(particles[i, :, 2]),
np.ones([num_particles]),
cmap='viridis_r',
**quiv_kwargs,
alpha=1.0,
clim=[0, 2]) # width=0.01, scale=100
plt.quiver(
[batch['s'][i, 0, 0]],
[batch['s'][i, 0, 1]],
np.cos([batch['s'][i, 0, 2]]),
np.sin([batch['s'][i, 0, 2]]),
color='black',
**quiv_kwargs,
) # width=0.01, scale=100
plt.plot(batch['s'][i, :2, 0],
batch['s'][i, :2, 1],
'--',
color='black',
linewidth=0.3)
plt.plot(batch['s'][i, :1, 0],
batch['s'][i, :1, 1],
'o',
color='black',
linewidth=0.3,
**marker_kwargs)
plt.plot(batch['s'][i, 1:2, 0],
batch['s'][i, 1:2, 1],
'o',
color='red',
linewidth=0.3,
**marker_kwargs)
plt.quiver([batch['s'][i, 1, 0]], [batch['s'][i, 1, 1]],
np.cos([batch['s'][i, 1, 2]]),
np.sin([batch['s'][i, 1, 2]]),
color='red',
**quiv_kwargs) # width=0.01, scale=100
plt.gca().axis('off')
plt.subplots_adjust(left=0.0,
bottom=0.0,
right=1.0,
top=1.0,
wspace=0.001,
hspace=0.1)
plt.savefig('../plots/models/motion_model{}.pdf'.format(i),
transparent=True,
dpi=600,
frameon=False,
facecolor='w',
pad_inches=0.01)