1.1.3 齐次变换矩阵与位姿数据pose的转换

在有些场合（如线扫激光），设备给出的特征点的空间位姿数据是4×4的齐次变换矩阵形式，如式（1-3）所示。由于ABB工业机器人并没有提供齐次变换矩阵到pose类型的数据转化函数，因此我们可以自行编写函数，将齐次变换矩阵中的旋转矩阵先转化为欧拉角，然后再将欧拉角转化为四元数，或者直接将旋转矩阵转化为四元数。

假设3个轴x、y、z的欧拉角分别为θ_x、θ_y、θ_z，其正弦值和余弦值分别为s_x、c_x、s_y、c_y、s_z、c_z，那么对应的旋转矩阵为

整理可得：

可得：

将以上公式用RAPID代码编写，具体实现过程如下：