def compute_aligned_poses(time_stamped_poses_A,
                          time_stamped_poses_B,
                          time_offset,
                          plot=False):
  """
  time_stamped_poses should have the following format:
    [timestamp [s], x [m], y [m], z [m], qx, qy, qz, qw]
  """

  time_stamped_poses_A_shifted = time_stamped_poses_A.copy()

  # Apply time offset.
  time_stamped_poses_A_shifted[:, 0] += time_offset

  # Compute common time interval.
  start_time = max(
      time_stamped_poses_A_shifted[0, 0], time_stamped_poses_B[0, 0])
  end_time = min(
      time_stamped_poses_A_shifted[-1, 0], time_stamped_poses_B[-1, 0])
  interval = end_time - start_time

  # Resample at the lower frequency to prevent introducing more noise.
  # The median is supposed to be robust against cases where few, large gaps
  # exist in either of the signals.
  dt_A = np.median(np.diff(time_stamped_poses_A_shifted[:, 0]))
  dt_B = np.median(np.diff(time_stamped_poses_B[:, 0]))
  if dt_A >= dt_B:
    dt = dt_A
    timestamps_low = time_stamped_poses_A_shifted[:, 0].T
    timestamps_high = time_stamped_poses_B[:, 0].T
  else:
    dt = dt_B
    timestamps_low = time_stamped_poses_B[:, 0].T
    timestamps_high = time_stamped_poses_A_shifted[:, 0].T

  # Create samples at time stamps from lower frequency signal, check if there
  # are timely close samples available from the other signal.
  # Interpolate the signals to match the time stamps of the low frequency
  # signal.
  samples = []
  max_time_stamp_difference = 0.1
  for timestamp in timestamps_low:
    if (timestamp < start_time):
      continue
    idx = bisect.bisect_left(timestamps_high, timestamp)
    if idx >= timestamps_high.shape[0] - 1:
      print("Omitting timestamps at the end of the high frequency poses.")
      break
    # Check if times are identical.
    if timestamp == timestamps_high[idx]:
      samples.append(timestamp)
      continue
    left_idx = idx - 1

    if timestamp < timestamps_high[left_idx]:
      continue

    right_idx = idx
    assert right_idx < timestamps_high.shape[
        0], (right_idx, timestamps_high.shape[0])
    if ((timestamp - timestamps_high[left_idx]) < max_time_stamp_difference and
            (timestamps_high[right_idx] - timestamp) <= max_time_stamp_difference):
      samples.append(timestamp)
      continue

  samples = np.array(samples)

  # Uncomment if you want to have equally spaced samples in time.
  # samples = np.linspace(start_time, end_time, interval / dt + 1)

  aligned_poses_A = interpolate_poses_from_samples(time_stamped_poses_A_shifted,
                                                   samples)
  aligned_poses_B = interpolate_poses_from_samples(time_stamped_poses_B,
                                                   samples)
  assert aligned_poses_A.shape == aligned_poses_B.shape
  assert np.allclose(aligned_poses_A[:, 0], aligned_poses_B[:, 0], atol=1e-8)
  print("Found {} matching poses.".format(aligned_poses_A.shape[0]))

  if plot:
    plot_time_stamped_poses("Time Aligned Aligned Poses", aligned_poses_A,
                            aligned_poses_B)

  return (aligned_poses_A, aligned_poses_B)

  time_stamped_poses should have the following format:
    [timestamp [s], x [m], y [m], z [m], qx, qy, qz, qw]
  
hand_eye_calibration/python/hand_eye_calibration/time_alignment.py