def show_error_plot(x_true, x_star, label, saveto):
xdifflist = []
ydifflist = []
thdifflist = []
for i in xrange(len(x_true)):
diff = np.array(x_true[i]) - np.array(x_star[i])
diff[2] = wraptopi(diff[2])
xdifflist.append(np.sqrt(diff[0]**2 + diff[1]**2))
thdifflist.append(np.fabs(diff[2]))
if np.fabs(diff[2]) > 1.5:
#import pdb; pdb.set_trace()
print i, diff[2]
print 'show_error_plot ' + label
plt.subplot(1, 2, 1)
plt.plot(range(len(x_true)),
xdifflist,
label=label,
alpha=0.7,
linewidth=1.0)
plt.title('position error')
plt.xlabel('timestep')
plt.ylabel('error (m)')
plt.subplot(1, 2, 2)
plt.plot(range(len(x_true)),
thdifflist,
label=label,
alpha=0.7,
linewidth=1.0)
plt.title('rotation error ' + label)
plt.xlabel('timestep')
plt.ylabel('error (rad)')
def show_rmse(x_true, x_star, label, saveto):
summ = np.array([0., 0., 0.])
for i in xrange(len(x_true)):
diff = np.array(x_true[i]) - np.array(x_star[i])
diff[2] = wraptopi(diff[2])
summ += np.square(diff)
summ /= float(len(x_true))
sqsum = np.sqrt(summ)
with open(saveto, 'a+') as f1:
f1.write("%s RMSE: %g %g %g\n" % (label, sqsum[0], sqsum[1], sqsum[2]))
return summ
def show_rmse(x_true, x_star, label, saveto):
summ = np.array([0., 0., 0.])
diff_array = []
for i in xrange(len(x_true)):
diff = np.array(x_true[i]) - np.array(x_star[i])
diff[2] = wraptopi(diff[2])
summ += np.square(diff)
diff_array.append([(diff[0]**2 + diff[1]**2)**0.5 * 1000,
diff[2] / 3.1415 * 180.0])
summ /= float(len(x_true))
sqsum = np.sqrt(summ)
with open(saveto, 'a+') as f1:
f1.write("%s RMSE: %g %g (%g mm) %g (%g deg)\n" %
(label, sqsum[0], sqsum[1],
(sqsum[0]**2 + sqsum[1]**2)**0.5 * 1000, sqsum[2],
sqsum[2] / 3.1415 * 180.0))
f1.write("stds (%g mm) (%g deg)\n" % tuple(np.std(diff_array, axis=0)))
return summ
def show_error_plot(true_x, x_star, label, saveto):
xdifflist = []
ydifflist = []
thdifflist = []
for i in xrange(len(true_x)):
diff = np.array(true_x[i]) - np.array(x_star[i])
diff[2] = wraptopi(diff[2])
xdifflist.append(np.sqrt(diff[0]**2 + diff[1]**2))
thdifflist.append(np.fabs(diff[2]))
print 'show_error_plot ' + label
plt.subplot(1, 2, 1)
plt.plot(range(len(true_x)), xdifflist, label=label)
plt.title('position error')
plt.xlabel('timestep')
plt.ylabel('error (m)')
plt.subplot(1, 2, 2)
plt.plot(range(len(true_x)), thdifflist, label=label)
plt.title('rotation error' + label)
plt.xlabel('timestep')
plt.ylabel('error (rad)')