Linear Pose Interpolation

slerp.py

def trajectory_slerp(evaluate_times, traj_raw_times, traj_raw, order=3):
    """
    linear interpolation trajectory
    :param evaluate_times: A list of float, the timestamps list at which to evaluate the interpolated poses.
    :param traj_raw_times: A list of float, the timestamps list of the input sparse poses
    :param traj_raw: A list of np[4x4] matrix, the SE3 matrices of the input sparse poses
    :param order: The degree of the spline fit
    :return:
    """
    assert len(traj_raw_times) == len(traj_raw)
    # rotation part
    traj_rotations = R.from_matrix(np.stack([M[:3, :3] for M in traj_raw]))
    rotations_slerp = Slerp(traj_raw_times, traj_rotations)
    interp_rotations = rotations_slerp(evaluate_times).as_matrix()

    # translation part
    xyz = np.array([M[:3, 3] for M in traj_raw])
    trans_x_slerp = splrep(traj_raw_times, xyz[:, 0], k=order)
    trans_y_slerp = splrep(traj_raw_times, xyz[:, 1], k=order)
    trans_z_slerp = splrep(traj_raw_times, xyz[:, 2], k=order)
    interp_trans = np.row_stack([
        splev(evaluate_times, trans_x_slerp),
        splev(evaluate_times, trans_y_slerp),
        splev(evaluate_times, trans_z_slerp)
    ]).T

    interp_traj = np.concatenate([interp_rotations, interp_trans.reshape(-1, 3, 1)], axis=2)
    # complete the last row of SE3
    interp_traj = np.concatenate([interp_traj, np.full([interp_traj.shape[0], 1, 4], [0, 0, 0, 1])], axis=1)

    return evaluate_times, list(interp_traj)