def find_transform(filename, plot=False):
LOG.info(' loading trajectories from {}'.format(filename))
data = np.load(filename)
gps, loam = data['gps'], data['loam']
t, l1, l2 = jmft.synchronyze(loam, gps)
p = jmft.fit_horn(l1, l2)
LOG.info(' computed transformation\n{}'.format(np.array(p)))
l3 = jmft.transform(l1, p)
if plot:
margins, figsize = (0.03, 0.05, 0.98, 0.95, 0.06, 0.18), (0.75 * 20.48,
0.4 * 15.36)
figure = jmp.prepare_fig(None,
"Slam Run",
figsize=figsize,
margins=margins)
ax = plt.gcf().add_subplot(121, projection='3d')
ax.plot(loam[:, 1], loam[:, 2], loam[:, 3], '.-m', markersize=0.5)
jmp.decorate(ax, legend=['loam'])
jmp.set_axes_equal(ax)
ax = plt.gcf().add_subplot(122, projection='3d')
ax.plot(gps[:, 1], gps[:, 2], gps[:, 3], '.-m', markersize=0.5)
ax.plot(l3[:, 0], l3[:, 1], l3[:, 2], '.-g', markersize=0.5)
jmp.decorate(ax, legend=['gps', 'loam_transformed'])
jmp.set_axes_equal(ax)
plt.show()
return np.array(p)
def plot_velocities(ds, odom_lvel, odom_rvel):
plt.figure()
ax = plt.subplot(2, 1, 1)
plt.plot(ds.truth_stamp, ds.truth_lvel_b[:, 0])
plt.plot(ds.enc_vel_stamp, odom_lvel)
jmp.decorate(ax,
'linear velocity',
ylab='m/s',
legend=['thuth', 'odometry'])
ax = plt.subplot(2, 1, 2)
plt.plot(ds.truth_stamp, ds.truth_rvel_b[:, 2])
plt.plot(ds.enc_vel_stamp, odom_rvel)
jmp.decorate(ax,
'angular velocity',
ylab='r/s',
legend=['thuth', 'odometry'])
def plot_encoders_vel(enc_vel_stamp, enc_vel_lw, enc_vel_rw, enc_vel_avg,
enc_stamp, enc_st):
# plot encoders velocities
plt.figure()
ax = plt.subplot(2, 1, 1)
plt.plot(enc_vel_stamp, enc_vel_lw, '.')
plt.plot(enc_vel_stamp, enc_vel_rw, '.')
plt.plot(enc_vel_stamp, enc_vel_avg, '.')
jmp.decorate(
ax,
'rotary encoders velocity',
ylab='r',
legend=['left rear wheel', 'right rear wheel', 'average rear wheels'])
ax = plt.subplot(2, 1, 2)
plt.plot(enc_stamp, enc_st, '.')
jmp.decorate(ax, 'steering encoder', ylab='r')
def plot_test_steering_sensor(sensors, angles, predicted_angles, txt):
plt.suptitle(txt)
ax = plt.subplot(2, 1, 1)
plt.plot(np.rad2deg(angles), sensors)
plt.plot(np.rad2deg(predicted_angles), sensors)
jmp.decorate(ax,
xlab='angle (deg)',
ylab='sensor',
title='sensor',
legend=['truth', 'ann'])
ax = plt.subplot(2, 2, 3)
residuals_deg = np.rad2deg(predicted_angles - angles)
plt.plot(np.rad2deg(angles), residuals_deg)
jmp.decorate(ax,
xlab='angle (deg)',
ylab='residuals (deg)',
title='residual')
ax = plt.subplot(2, 2, 4)
plt.hist(residuals_deg, bins=100)
mu, sigma = np.mean(residuals_deg), np.std(residuals_deg)
plt.legend(['$\mu$ {:.2e}\n$\sigma$ {:.2e}'.format(mu, sigma)])
def plot_positions(ds, odom_heading, odom_xy):
plt.figure()
ax = plt.subplot(2, 2, 1)
plt.plot(ds.truth_stamp, ds.truth_heading_unwrapped)
plt.plot(ds.enc_vel_stamp, odom_heading)
jmp.decorate(ax, 'heading', legend=['truth', 'odom'])
ax = plt.subplot(2, 2, 2)
plt.plot(ds.truth_pos[:, 0], ds.truth_pos[:, 1])
plt.plot(odom_xy[:, 0], odom_xy[:, 1])
jmp.decorate(ax, '2D')
heading_err = np.rad2deg(odom_heading - ds.truth_heading_unwrapped1)
ax = plt.subplot(4, 1, 3)
plt.plot(ds.enc_vel_stamp, heading_err)
jmp.decorate(ax, 'heading err', ylab='deg')
pos_err = np.linalg.norm(odom_xy - ds.truth_xy1, axis=1)
ax = plt.subplot(4, 1, 4)
plt.plot(ds.enc_vel_stamp, pos_err)
jmp.decorate(ax, 'err dist', ylab='m')
def plot_body_truth(truth_stamp, truth_lvel_b, truth_rvel_b):
plt.figure()
ax = plt.subplot(3, 2, 1)
plt.plot(truth_stamp, truth_lvel_b[:, 0])
jmp.decorate(ax, 'lvel body x')
ax = plt.subplot(3, 2, 3)
plt.plot(truth_stamp, truth_lvel_b[:, 1])
jmp.decorate(ax, 'lvel body y')
ax = plt.subplot(3, 2, 5)
plt.plot(truth_stamp, truth_lvel_b[:, 2])
jmp.decorate(ax, 'lvel body z')
ax = plt.subplot(3, 2, 2)
plt.plot(truth_stamp, truth_rvel_b[:, 0])
jmp.decorate(ax, 'rvel body x')
ax = plt.subplot(3, 2, 4)
plt.plot(truth_stamp, truth_rvel_b[:, 1])
jmp.decorate(ax, 'rvel body y')
ax = plt.subplot(3, 2, 6)
plt.plot(truth_stamp, truth_rvel_b[:, 2])
jmp.decorate(ax, 'rvel body z')
def check_timestamps(enc_vel_stamp, enc_dt):
plt.figure()
#plt.plot(enc_vel_stamp, enc_dt)
plt.hist(enc_dt, bins=100)
jmp.decorate(plt.gca(), 'encoder delta timestamp')
def plot_steering_sensor(lim=math.pi / 6):
steering_angle = np.linspace(-lim, lim, 100)
steering_sensor = BrokenDataSet.steering_sensor(steering_angle)
plt.plot(np.rad2deg(steering_angle), steering_sensor)
jmp.decorate(plt.gca(), xlab='angle (deg)', ylab='sensor')