编程实现了用四元数表示的位姿矩阵，将其变换到四行四列的位姿矩阵。