齐次变换矩阵（HTM）

旋转矩阵 R、平移向量 p，SE(3) 群入门