def plot_wheel_radius(self, filename=None):
fig = jpu.prepare_fig(window_title='Wheel radius regression')
inlier_mask = self.ransac_wr.inlier_mask_
outlier_mask = np.logical_not(inlier_mask)
#plt.scatter(self.ds.enc_vel_sum, self.truth_lvel_body_1[:,0], color='gold', marker='.', label='Outliers')
plt.scatter(self.ds.enc_vel_sum[outlier_mask],
self.truth_lvel_body_1[:, 0][outlier_mask],
color='gold',
marker='.',
label='Outliers')
plt.scatter(self.ds.enc_vel_sum[inlier_mask],
self.truth_lvel_body_1[:, 0][inlier_mask],
color='yellowgreen',
marker='.',
label='Inliers')
test_enc_vel_sum = np.linspace(np.min(self.ds.enc_vel_sum),
np.max(self.ds.enc_vel_sum),
100)[:, np.newaxis]
test_vel_simple = 0.5 * test_enc_vel_sum * self.wheel_radius_simple
test_vel_ransac = 0.5 * self.ransac_wr.predict(test_enc_vel_sum)
plt.plot(test_enc_vel_sum, test_vel_simple, label='simple')
plt.plot(test_enc_vel_sum, test_vel_ransac, label='ransac')
jpu.decorate(plt.gca(),
title='wheel radius fit',
xlab='enc_avg (rad/s)',
ylab="v (m/s)",
legend=True) #, xlim=[-1, 20], ylim=[-0.1, 1.])
jpu.savefig(filename)
def plot_wheel_sep(self, filename=None):
fig = jpu.prepare_fig(window_title='Wheel sep regression')
inlier_mask = self.ransac_ws.inlier_mask_
outlier_mask = np.logical_not(inlier_mask)
plt.scatter(self.ds.enc_vel_dif[outlier_mask],
self.truth_rvel_1[:, 2][outlier_mask],
color='gold',
marker='.',
label='Outliers')
plt.scatter(self.ds.enc_vel_dif[inlier_mask],
self.truth_rvel_1[:, 2][inlier_mask],
color='yellowgreen',
marker='.',
label='Inliers')
test_enc_vel_dif = np.linspace(np.min(self.ds.enc_vel_dif),
np.max(self.ds.enc_vel_dif),
100)[:, np.newaxis]
test_rvel_simple = test_enc_vel_dif * -self.wheel_radius / self.wheel_sep_simple
#test_rvel_ransac = self.ransac_wr.predict(test_enc_vel_dif)
test_rvel_ransac = test_enc_vel_dif * -self.wheel_radius / self.wheel_sep_ransac
plt.plot(test_enc_vel_dif, test_rvel_simple, label='simple')
plt.plot(test_enc_vel_dif, test_rvel_ransac, label='ransac')
jpu.decorate(plt.gca(),
title='wheel separation fit',
xlab='enc_diff (rad/s)',
ylab="rvel (rad/s)",
legend=True,
xlim=[-5., 5.],
ylim=[-2.5, 2.5])
jpu.savefig(filename)
def plot_truth_vel(_ds, filename=None):
fig = jpu.prepare_fig(window_title='Truth')
ax = plt.subplot(2, 1, 1)
#plt.plot(_ds.truth_stamp, _ds.truth_norm_vel, '.')
plt.plot(_ds.truth_vel_stamp, _ds.truth_lvel_body[:, 0], '.', label='x')
plt.plot(_ds.truth_vel_stamp, _ds.truth_lvel_body[:, 1], '.', label='y')
plt.plot(_ds.truth_vel_stamp, _ds.truth_lvel_body[:, 2], '.', label='z')
jpu.decorate(ax,
title='Truth velocity',
xlab='time in s',
ylab='m/s',
legend=True,
xlim=None,
ylim=None)
ax = plt.subplot(2, 1, 2)
plt.plot(_ds.truth_vel_stamp, _ds.truth_rvel[:, 0], '.', label='p')
plt.plot(_ds.truth_vel_stamp, _ds.truth_rvel[:, 1], '.', label='q')
plt.plot(_ds.truth_vel_stamp, _ds.truth_rvel[:, 2], '.', label='r')
jpu.decorate(ax,
title='Truth rotational velocity',
xlab='time in s',
ylab='rad/s',
legend=True,
xlim=None,
ylim=[-2, 2])
jpu.savefig(filename)
def plot2d(_ds, filename=None):
fig = jpu.prepare_fig(window_title='2D')
plt.plot(_ds.truth_pos[:, 0], _ds.truth_pos[:, 1], '.', label='truth')
jpu.decorate(plt.gca(),
title='truth 2D points',
xlab='m',
ylab='m',
legend=True,
xlim=None,
ylim=None)
plt.axis('equal')
jpu.savefig(filename)
return fig
def plot_encoders_stats(_ds, filename=None, title=''):
''' plot encoders histograms '''
fig = jpu.prepare_fig(window_title='Encoders Stats ({})'.format(title))
ax = plt.subplot(2, 2, 1)
plt.hist(_ds.enc_vel_lw)
jpu.decorate(ax, title='Left Wheel', xlab='rvel in rad/s', ylab='samples')
ax = plt.subplot(2, 2, 2)
plt.hist(_ds.enc_vel_lw)
jpu.decorate(ax, title='Right Wheel', xlab='rvel in rad/s', ylab='samples')
ax = plt.subplot(2, 2, 3)
plt.hist(_ds.truth_lvel_body[:, 0])
jpu.decorate(ax, title='Linear Velocity', xlab='m/s', ylab='samples')
ax = plt.subplot(2, 2, 4)
plt.hist(_ds.truth_rvel[:, 2])
jpu.decorate(ax, title='Angular Velocity', xlab='rad/s', ylab='samples')
jpu.savefig(filename)
def plot_kinematics_truth(_ds, filename=None):
#fig = jpu.prepare_fig(window_title='Encoders 3D')
fig = hciods.plot_encoders_3D(_ds)
ax = fig.add_subplot(121, projection='3d')
X = np.arange(-1, 1, 0.01)
Y = np.arange(-1, 1, 0.01)
X, Y = np.meshgrid(X, Y)
lvel, rvel = np.zeros_like(X), np.zeros_like(X)
for i in range(X.shape[0]):
for j in range(X.shape[1]):
lvel[i, j], rvel[i, j] = hcu.kin_vel_of_wheel_vel(
X[i, j], Y[i, j], _ds.w_r, _ds.w_s)
#pdb.set_trace()
ax = fig.add_subplot(121, projection='3d')
surf = ax.plot_surface(X,
Y,
lvel,
cmap=matplotlib.cm.coolwarm,
linewidth=0,
antialiased=True,
alpha=0.5)
ax.scatter(_ds.enc_vel_lw, _ds.enc_vel_rw, _ds.truth_lvel,
s=0.1) #, c=c, marker=m)
ax.set_xlabel('$\omega_l (rad/s)$')
ax.set_ylabel('$\omega_r (rad/s)$')
ax.set_zlabel('$V (m/s)$')
jpu.decorate(ax, title='Linear Velocity vs/enc vels')
ax = fig.add_subplot(122, projection='3d')
surf = ax.plot_surface(X,
Y,
rvel,
cmap=matplotlib.cm.coolwarm,
linewidth=0,
antialiased=True,
alpha=0.5)
ax.scatter(_ds.enc_vel_lw, _ds.enc_vel_rw, _ds.truth_rvel,
s=0.1) #, c=c, marker=m)
ax.set_xlabel('$\omega_l (rad/s)$')
ax.set_ylabel('$\omega_r (rad/s)$')
ax.set_zlabel('$\Omega (rad/s)$')
jpu.decorate(ax, title='Angular Velocity vs/enc vels')
jpu.savefig(filename)
def plot_encoders_3D(_ds, filename=None, title=''):
fig = jpu.prepare_fig(window_title='Encoders 3D ({})'.format(title))
ax = fig.add_subplot(121, projection='3d')
truth_lvel_body_1 = interpolate(_ds.truth_lvel_body, _ds.truth_vel_stamp,
_ds.enc_vel_stamp)
ax.scatter(_ds.enc_vel_lw, _ds.enc_vel_rw, truth_lvel_body_1[:, 0], s=0.1)
ax.set_xlabel('$\omega_l (rad/s)$')
ax.set_ylabel('$\omega_r (rad/s)$')
ax.set_zlabel('$V (m/s)$')
jpu.decorate(ax, title='Linear Velocity vs/enc vels')
ax = fig.add_subplot(122, projection='3d')
truth_rvel_1 = interpolate(_ds.truth_rvel, _ds.truth_vel_stamp,
_ds.enc_vel_stamp)
ax.scatter(_ds.enc_vel_lw, _ds.enc_vel_rw, truth_rvel_1[:, 2],
s=0.1) #, c=c, marker=m)
ax.set_xlabel('$\omega_l (rad/s)$')
ax.set_ylabel('$\omega_r (rad/s)$')
ax.set_zlabel('$\Omega (rad/s)$')
jpu.decorate(ax, title='Angular Velocity vs/enc vels')
jpu.savefig(filename)
return fig
def plot_encoders(_ds, filename=None):
fig = jpu.prepare_fig(window_title='Encoders')
ax = plt.subplot(4, 1, 1)
plt.plot(_ds.enc_stamp, _ds.enc_lw, '.', label="lw")
plt.plot(_ds.enc_stamp, _ds.enc_rw, '.', label="rw")
jpu.decorate(ax,
title='Encoders position',
xlab='time in s',
ylab='rad',
legend=True)
ax = plt.subplot(4, 1, 2)
plt.plot(_ds.enc_vel_stamp, _ds.enc_vel_lw, '.')
plt.plot(_ds.enc_vel_stamp, _ds.enc_vel_rw, '.')
jpu.decorate(ax,
title='Encoders velocity',
xlab='time in s',
ylab='rad/s',
legend=None,
xlim=None,
ylim=None)
ax = plt.subplot(4, 1, 3)
plt.plot(_ds.enc_vel_stamp, _ds.enc_vel_sum, '.')
jpu.decorate(ax,
title='Encoders velocity sum',
xlab='time in s',
ylab='rad/s',
legend=None,
xlim=None,
ylim=None)
ax = plt.subplot(4, 1, 4)
plt.plot(_ds.enc_vel_stamp, _ds.enc_vel_dif, '.')
jpu.decorate(ax,
title='Encoders velocity dif',
xlab='time in s',
ylab='rad/s',
legend=None,
xlim=None,
ylim=None)
jpu.savefig(filename)
def plot_residuals(self, info, filename=None):
figsize = (20.48, 10.24)
margins = 0.05, 0.07, 0.97, 0.95, 0.14, 0.39
fig = jpu.prepare_fig(
window_title='Odometry parameters regression ({})'.format(info),
figsize=figsize,
margins=margins)
inlier_mask = self.ransac_wr.inlier_mask_
outlier_mask = np.logical_not(inlier_mask)
ax = plt.subplot(4, 1, 1)
#plt.scatter(self.ds.enc_vel_stamp, self.res_radius_simple, marker='.')
plt.scatter(self.ds.enc_vel_stamp[outlier_mask],
self.res_radius_ransac[outlier_mask],
color='gold',
marker='.',
label='Outliers')
plt.scatter(self.ds.enc_vel_stamp[inlier_mask],
self.res_radius_ransac[inlier_mask],
color='yellowgreen',
marker='.',
label='Inliers')
jpu.decorate(ax,
title='wheel radius residuals',
xlab='time in s',
ylab='m',
legend=True,
xlim=None,
ylim=None)
_fmt_legend = '$\mu: {:.2e} \quad \sigma: {:.2e}$'
ax = plt.subplot(4, 2, 3)
plt.hist(self.res_radius_simple, normed=True, bins=30)
jpu.decorate(ax,
title='simple',
xlab='m',
legend=[
_fmt_legend.format(self.res_radius_simple_mu,
self.res_radius_simple_sigma)
])
ax = plt.subplot(4, 2, 4)
plt.hist(self.res_radius_ransac[self.ransac_wr.inlier_mask_],
normed=True,
bins=30)
jpu.decorate(ax,
title='ransac',
xlab='m',
legend=[
_fmt_legend.format(self.res_radius_ransac_mu,
self.res_radius_ransac_sigma)
])
inlier_mask = self.ransac_ws.inlier_mask_
outlier_mask = np.logical_not(inlier_mask)
ax = plt.subplot(4, 1, 3)
#plt.scatter(self.ds.enc_vel_stamp, self.res_wheel_sep_simple, marker='.')
plt.scatter(self.ds.enc_vel_stamp[outlier_mask],
self.res_wheel_sep_ransac[outlier_mask],
color='gold',
marker='.',
label='Outliers')
plt.scatter(self.ds.enc_vel_stamp[inlier_mask],
self.res_wheel_sep_ransac[inlier_mask],
color='yellowgreen',
marker='.',
label='Inliers')
jpu.decorate(ax,
title='wheel separation residuals',
xlab='time in s',
ylab='',
legend=True,
xlim=None,
ylim=None)
ax = plt.subplot(4, 2, 7)
plt.hist(self.res_wheel_sep_simple, normed=True, bins=30)
jpu.decorate(ax,
title='simple',
legend=[
_fmt_legend.format(self.res_wheel_sep_simple_mu,
self.res_wheel_sep_simple_sigma)
])
ax = plt.subplot(4, 2, 8)
plt.hist(self.res_wheel_sep_ransac[self.ransac_ws.inlier_mask_],
normed=True,
bins=30)
jpu.decorate(ax,
title='ransac',
legend=[
_fmt_legend.format(self.res_wheel_sep_ransac_mu,
self.res_wheel_sep_ransac_sigma)
])
jpu.savefig(filename)