def test_slerp_single_rot(): with pytest.raises(ValueError, match="at least 2 rotations"): r = Rotation.from_quat([1, 2, 3, 4]) Slerp([1], r)
def test_slerp_equal_times(): with pytest.raises(ValueError, match="strictly increasing order"): np.random.seed(0) r = Rotation.from_quat(np.random.uniform(size=(5, 4))) t = [0, 1, 2, 2, 4] Slerp(t, r)
total_time = time_array[-1] t_new = np.arange(time_array[0], total_time, 1.0/FPS_VIDEO) fx = interpolate.interp1d(time_array, data[:,1], fill_value="extrapolate") fy = interpolate.interp1d(time_array, data[:,2], fill_value="extrapolate") fz = interpolate.interp1d(time_array, data[:,3], fill_value="extrapolate") pos_new = np.zeros((t_new.shape[0], 3)) pos_new[:,0] = fx(t_new) pos_new[:,1] = fy(t_new) pos_new[:,2] = fz(t_new) att_array = np.empty((0,4)) for i in range(data.shape[0]): att_array = np.append(att_array, quat_wx2xw(Euler2quat(data[i,7:10]))[np.newaxis,:], axis=0) key_rots = R.from_quat(att_array) slerp = Slerp(time_array, key_rots) interp_rots = slerp(t_new) att_new_array = R.as_quat(interp_rots) att_new = np.zeros_like(att_new_array) for i in range(t_new.shape[0]): att_new[i,:] = quat_xw2wx(att_new_array[i,:]) # Progress Bar data_len = t_new.shape[0] print("data length: {}".format(data_len)) bar_iter = 0 bar_max = data_len bar = Bar('Processing Video', max=bar_max, suffix='%(percent)d%%') bar_step = np.around(data_len/bar_max) # Request Image and Save