def resample_quaternions_from_samples(times, quaternions, samples):
  """
  Resample the quaternions at the times specified in 'samples'.
  Uses SLERP for quaternion interpolation.
  """
  interp_quaternions = []
  for sample in samples:
    assert sample <= times[-1], (sample, times)
    assert sample >= times[0], (sample, times[0])

    right_idx = bisect.bisect_left(times, sample)
    if (np.isclose(sample, times[right_idx], atol=1e-16)):
      interp_quaternions.append(quaternions[right_idx])
    else:
      left_idx = right_idx - 1
      assert right_idx < times.shape[0], end_idx
      assert left_idx >= 0, left_idx
      sample_times = []
      sample_times.append(sample)
      quaternion_interp = quaternions_interpolate(
          quaternions[left_idx], times[left_idx], quaternions[right_idx],
          times[right_idx], sample_times)
      interp_quaternions += quaternion_interp
  assert len(interp_quaternions) == samples.shape[0], str(
      len(interp_quaternions)) + " vs " + str(samples.shape[0])
  return interp_quaternions

  Resample the quaternions at the times specified in 'samples'.
  Uses SLERP for quaternion interpolation.
  
hand_eye_calibration/python/hand_eye_calibration/time_alignment.py