import numpy as np
from scipy.spatial.transform import Rotation as R


def to_se3_matrix(pose_vec):
    pose_mat = np.eye(4)
    pose_mat[:3, :3] = R.from_quat(pose_vec[4:8]).as_matrix()
    pose_mat[:3, 3] = pose_vec[1:4]
    return pose_mat


def to_se3_vec(pose_mat):
    quat = R.from_matrix(pose_mat[:3, :3]).as_quat()
    return np.hstack((pose_mat[:3, 3], quat))
