xz_path.append((x_stam, z_stam, experiment.name, color))
zy_path.append((z_stam, y_stam, experiment.name, color))
lines3D.append((x_stam, y_stam, z_stam, experiment.name, color))
# 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,:]
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()
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, )))
# There is a small fix as the KFs are in the coordinate system of the camera
yx_path.append(((-1 * y_stam) + 0.155, x_stam,
experiment.name + "-aligned", color + (0.5, )))
lines3D.append((x_stam, y_stam, z_stam, experiment.name + "-aligned",
color + (0.5, )))
ph.plotPaths2D(yx_path)
#~ plt.savefig('path.png', bbox_inches='tight')
ph.plotPaths3D(lines3D)
#~ plt.savefig('path3D.png', bbox_inches='tight')
####################################################################
# Plot detected loops
####################################################################
pos_grnd_overtime = []
# convert data to numpy array
np_timestamps_data = np.array(timestamps_data) / 60.
pos_grnd_overtime.append((x_grnd, y_grnd, np_timestamps_data, (0, 0, 0)))
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 )
ax.plot(x_sptam, y_sptam)
ax.plot(x_kf_sptam, y_kf_sptam)
