def visualize_poses(self, pose_list, draw=True, style='-rx'):
if len(pose_list) == 0:
print('pose list is empty, skipping')
return
se3_init = pose_list[0]
points_to_be_graphed = np.matmul(se3_init, self.point_pair)[0:3, :]
for i in range(1, len(pose_list)):
se3 = pose_list[i]
points_transformed = np.matmul(se3, self.point_pair)[0:3, :]
# identity gets transformed twice
points_to_be_graphed = np.append(points_to_be_graphed,
points_transformed,
axis=1)
if self.plot_trajectory:
Plot3D.plot_array_lines(points_to_be_graphed,
self.se3_graph,
clear=False,
draw=draw)
Plot3D.plot_translation_component(0,
pose_list,
self.x_graph,
style=style,
clear=False,
draw=draw)
Plot3D.plot_translation_component(1,
pose_list,
self.y_graph,
style=style,
clear=False,
draw=draw)
Plot3D.plot_translation_component(2,
pose_list,
self.z_graph,
style=style,
clear=False,
draw=draw)
if self.plot_rmse:
Plot3D.plot_rmse(self.ground_truth_list,
pose_list,
self.rmse_graph,
clear=False,
draw=draw,
offset=1)
def visualize_ground_truth(self, clear=True, draw=False):
if len(self.ground_truth_list) > 0:
gt_init = self.ground_truth_list[0]
points_gt_to_be_graphed = np.matmul(gt_init,
self.point_pair)[0:3, :]
for i in range(1, len(self.ground_truth_list)):
se3 = self.ground_truth_list[i]
points_transformed = np.matmul(se3, self.point_pair)[0:3, :]
# identity gets transformed twice
points_gt_to_be_graphed = np.append(points_gt_to_be_graphed,
points_transformed,
axis=1)
if self.plot_trajectory:
Plot3D.plot_array_lines(points_gt_to_be_graphed,
self.se3_graph,
'-go',
clear=clear,
draw=False)
Plot3D.plot_translation_component(0,
self.ground_truth_list,
self.x_graph,
style='-gx',
clear=False,
draw=draw)
Plot3D.plot_translation_component(1,
self.ground_truth_list,
self.y_graph,
style='-gx',
clear=False,
draw=draw)
Plot3D.plot_translation_component(2,
self.ground_truth_list,
self.z_graph,
style='-gx',
clear=False,
draw=draw)