time_alignment_plotting_tools.py 文件源码

def plot_time_stamped_poses(title,
                            time_stamped_poses_A,
                            time_stamped_poses_B,
                            block=True):
  fig = plt.figure()

  title_position = 1.05

  fig.suptitle(title + " [A = top, B = bottom]", fontsize='24')

  a1 = plt.subplot(2, 2, 1)
  a1.set_title(
      "Orientation \nx [red], y [green], z [blue], w [cyan]",
      y=title_position)
  plt.plot(time_stamped_poses_A[:, 4], c='r')
  plt.plot(time_stamped_poses_A[:, 5], c='g')
  plt.plot(time_stamped_poses_A[:, 6], c='b')
  plt.plot(time_stamped_poses_A[:, 7], c='c')

  a2 = plt.subplot(2, 2, 2)
  a2.set_title(
      "Position (eye coordinate frame) \nx [red], y [green], z [blue]", y=title_position)
  plt.plot(time_stamped_poses_A[:, 1], c='r')
  plt.plot(time_stamped_poses_A[:, 2], c='g')
  plt.plot(time_stamped_poses_A[:, 3], c='b')

  a3 = plt.subplot(2, 2, 3)
  plt.plot(time_stamped_poses_B[:, 4], c='r')
  plt.plot(time_stamped_poses_B[:, 5], c='g')
  plt.plot(time_stamped_poses_B[:, 6], c='b')
  plt.plot(time_stamped_poses_B[:, 7], c='c')

  a4 = plt.subplot(2, 2, 4)
  plt.plot(time_stamped_poses_B[:, 1], c='r')
  plt.plot(time_stamped_poses_B[:, 2], c='g')
  plt.plot(time_stamped_poses_B[:, 3], c='b')

  plt.subplots_adjust(left=0.025, right=0.975, top=0.8, bottom=0.05)

  if plt.get_backend() == 'TkAgg':
    mng = plt.get_current_fig_manager()
    max_size = mng.window.maxsize()
    max_size = (max_size[0], max_size[1] * 0.45)
    mng.resize(*max_size)
  plt.show(block=block)