path_yz = []
path_3d = []
if ground_truth:
_, gt_positions = mh.decomposeTransformations(ground_truth.poses)
path_xy.append((gt_positions[:, 0], gt_positions[:, 1]))
path_xz.append((gt_positions[:, 0], gt_positions[:, 2]))
path_yz.append((gt_positions[:, 1], gt_positions[:, 2]))
path_3d.append(
(gt_positions[:, 0], gt_positions[:, 1], gt_positions[:, 2]))
for label, data in logs.iteritems():
path_xy.append((data.positions[:, 0], data.positions[:, 1]))
path_xz.append((data.positions[:, 0], data.positions[:, 2]))
path_yz.append((data.positions[:, 1], data.positions[:, 2]))
path_3d.append(
(data.positions[:, 0], data.positions[:, 1], data.positions[:, 2]))
ph.plotPaths2D(path_xy, labels, colors, xlabel="x (m)", ylabel="y (m)")
ph.plotPaths2D(path_xz, labels, colors, xlabel="x (m)", ylabel="z (m)")
ph.plotPaths2D(path_yz, labels, colors, xlabel="y (m)", ylabel="z (m)")
ph.plotPaths3D(path_3d, labels, colors)
#~ plt.savefig("fig.png", bbox_inches='tight', transparent=True, dpi=200)
####################################################################
# Show all plots
####################################################################
plt.show()
# add adjusted data, with 0.5 alpha
# x_stam = experiment.adjusted_pos[:,0]
# y_stam = experiment.adjusted_pos[:,1]
# z_stam = experiment.adjusted_pos[:,2]
# xz_path.append( (x_stam, z_stam, experiment.name+"-aligned", color+(0.5,)) )
# zy_path.append( (z_stam, y_stam, experiment.name+"-aligned", color+(0.5,)) )
# lines3D.append( (x_stam, y_stam, z_stam, experiment.name+"-aligned", color+(0.5,)) )
ph.plotPaths2D(xz_path)
plt.savefig('path.png', bbox_inches='tight')
ph.plotPaths2D(zy_path)
ph.plotPaths3D(lines3D)
#~ plt.savefig('path3D.png', bbox_inches='tight')
####################################################################
# Compute Errors
####################################################################
# Compute the relative movement between positions
"""
diff_pos_grnd = pos_grnd[1:,:] - pos_grnd[:-1,:]
total_dist = 0
for diff in diff_pos_grnd:
dist = np.linalg.norm( diff )
total_dist = total_dist + dist
pos_kf_sptam = np.array(pos_kf_sptam)
print("First sptam pose", pos_sptam[0])
print('Final sptam pose:', pos_sptam[-1, :])
x_sptam = pos_sptam[:, 0]
y_sptam = pos_sptam[:, 1]
z_sptam = pos_sptam[:, 2]
x_kf_sptam = pos_kf_sptam[:, 0]
y_kf_sptam = pos_kf_sptam[:, 1]
z_kf_sptam = pos_kf_sptam[:, 2]
label_sptam = "S-PTAM"
pointCloudFile = "/home/taihu/Desktop/pasillo_u_reverso.dat"
ph.plotPaths3D([(x_sptam, y_sptam, z_sptam, label_sptam),
(x_kf_sptam, y_kf_sptam, z_kf_sptam, "S-PTAM adjusted")],
"System trajectory")
ph.plotPaths2D([(x_sptam, z_sptam, label_sptam)], pointCloudFile)
####################################################################
# Draw 2D Trayectory
####################################################################
fig = plt.figure()
ax = fig.add_subplot(111)
ax.set_axis_off()
#bg_img = plt.imread('pab1-2do.png')
#plt.imshow( bg_img )