def interpolate_poses_from_samples(time_stamped_poses, samples):
  """
  Interpolate time stamped poses at the time stamps provided in samples.
  The poses are expected in the following format:
    [timestamp [s], x [m], y [m], z [m], qx, qy, qz, qw]

  We apply linear interpolation to the position and use SLERP for
  the quaternion interpolation.
  """

  # Extract the quaternions from the poses.
  quaternions = []
  for pose in time_stamped_poses[:, 1:]:
    quaternions.append(Quaternion(q=pose[3:]))

  # interpolate the quaternions.
  quaternions_interp = resample_quaternions_from_samples(
      time_stamped_poses[:, 0], quaternions, samples)

  # Interpolate the position and assemble the full aligned pose vector.
  num_poses = samples.shape[0]
  aligned_poses = np.zeros((num_poses, time_stamped_poses.shape[1]))
  aligned_poses[:, 0] = samples[:]

  for i in [1, 2, 3]:
    aligned_poses[:, i] = np.interp(
        samples,
        np.asarray(time_stamped_poses[:, 0]).ravel(),
        np.asarray(time_stamped_poses[:, i]).ravel())

  for i in range(0, num_poses):
    aligned_poses[i, 4:8] = quaternions_interp[i].q

  return aligned_poses.copy()

  Interpolate time stamped poses at the time stamps provided in samples.
  The poses are expected in the following format:
    [timestamp [s], x [m], y [m], z [m], qx, qy, qz, qw]

  We apply linear interpolation to the position and use SLERP for
  the quaternion interpolation.
  
hand_eye_calibration/python/hand_eye_calibration/time_alignment.py