def test_rotate_vector(self): q = Quaternion(0.5, 0.5, 0.5, 0.5) rotated_vector = q.rotate_vector(np.array([1., 2., 3.])) expected_rotated_vector = np.array([3., 1., 2.]) npt.assert_allclose(rotated_vector, expected_rotated_vector)
def plot_poses(poses_list, plot_arrows=True, title="", blocking=True): title_position = 1.05 fig = plt.figure() plt.clf() ax = Axes3D(fig) if title: fig.suptitle(title, fontsize='24') colors = ['r', 'g', 'b', 'c', 'm', 'k'] num_colors = len(colors) assert len(poses_list) < num_colors, ( "Need to define more colors to plot more trajectories!") (bbox_max, bbox_min) = compute_bbox_3D(poses_list) arrow_size = np.linalg.norm(bbox_max - bbox_min) * 0.05 arrow_width = 2 axis_min = np.amin(bbox_min) axis_max = np.amax(bbox_max) ax.set_xlim3d(axis_min, axis_max) ax.set_ylim3d(axis_min, axis_max) ax.set_zlim3d(axis_min, axis_max) for i in range(0, len(poses_list)): poses = poses_list[i].copy() # Plot line. positions = ax.plot(xs=poses[:, 0], ys=poses[:, 1], zs=poses[:, 2], color=colors[i]) for pose in poses: # Position point ax.plot([pose[0]], [pose[1]], [pose[2]], 'o', markersize=5, color=colors[i], alpha=0.5) if not plot_arrows: continue rotation_quaternion = Quaternion(q=pose[3:7]) x_rotated = rotation_quaternion.rotate_vector([1, 0, 0, 0]) x_rotated *= arrow_size a = Arrow3D([pose[0], pose[0] + x_rotated[0]], [pose[1], pose[1] + x_rotated[1]], [pose[2], pose[2] + x_rotated[2]], mutation_scale=20, lw=arrow_width, arrowstyle="-|>", color="r") ax.add_artist(a) y_rotated = rotation_quaternion.rotate_vector([0, 1, 0, 0]) y_rotated *= arrow_size a = Arrow3D([pose[0], pose[0] + y_rotated[0]], [pose[1], pose[1] + y_rotated[1]], [pose[2], pose[2] + y_rotated[2]], mutation_scale=20, lw=arrow_width, arrowstyle="-|>", color="g") ax.add_artist(a) z_rotated = rotation_quaternion.rotate_vector([0, 0, 1, 0]) z_rotated *= arrow_size a = Arrow3D([pose[0], pose[0] + z_rotated[0]], [pose[1], pose[1] + z_rotated[1]], [pose[2], pose[2] + z_rotated[2]], mutation_scale=20, lw=arrow_width, arrowstyle="-|>", color="b") ax.add_artist(a) ax.auto_scale_xyz([axis_min, axis_max], [axis_min, axis_max], [axis_min, axis_max]) plt.show(block=blocking)