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)
dt = 1.0 pose = Pose.Pose() steering_command_straight = SteeringCommand.SteeringCommands(1.0, 0.0) ackermann_motion = Ackermann.Ackermann() new_motion_delta = ackermann_motion.ackermann_dead_reckoning_delta(steering_command_straight) pose.apply_motion(new_motion_delta,dt) #TODO investigate which theta to use # this might actually be better since we are interested in the uncertainty only in this timestep theta = new_motion_delta.delta_theta # traditional uses accumulated theta #theta = pose.theta motion_cov_small = ackermann_motion.covariance_dead_reckoning(steering_command_straight,theta,dt) se3 = SE3.generate_se3_from_motion_delta(new_motion_delta) origin_transformed = np.matmul(se3, origin) points = np.append(origin, origin_transformed, axis=1) points_xyz = points[0:3,:] fig = plt.figure() ax = fig.add_subplot(111, projection='3d') Plot3D.plot_array_lines(points_xyz, ax)
motion_delta = ackermann_motion.pose_delta_list[0]
se3 = SE3.generate_se3_from_motion_delta(motion_delta)
origin_transformed = np.matmul(se3, origin)
origin_transformed_2 = np.matmul(se3, origin_transformed)
points = np.append(np.append(origin, origin_transformed, axis=1),
origin_transformed_2,
axis=1)
points_xyz = points[0:3, :]
# testing inverse
#cov_inv = np.linalg.inv(motion_cov_small_1)
#t = np.matmul(cov_inv,motion_cov_small_1)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
#Plot3D.plot_wireframe_ellipsoid(1,1,1,ax,label_axes=True, clear=True,draw=False)
Plot3D.plot_array_lines(points_xyz, ax, clear=True, draw=False)
Plot3D.plot_wireframe_ellipsoid(0.1,
ellipse_factor_list,
se3_list,
ax,
label_axes=True,
clear=False,
draw=False)
ax.set_xlim(-10, 10)
ax.set_ylim(-1, 1)
ax.set_zlim(-10, 10)
Plot3D.show()