上QQ阅读APP看本书，新人免费读10天
设备和账号都新为新人
1.1.3 齐次变换矩阵与位姿数据pose的转换
在有些场合（如线扫激光），设备给出的特征点的空间位姿数据是4×4的齐次变换矩阵形式，如式（1-3）所示。由于ABB工业机器人并没有提供齐次变换矩阵到pose类型的数据转化函数，因此我们可以自行编写函数，将齐次变换矩阵中的旋转矩阵先转化为欧拉角，然后再将欧拉角转化为四元数，或者直接将旋转矩阵转化为四元数。
假设3个轴x、y、z的欧拉角分别为θx、θy、θz，其正弦值和余弦值分别为sx、cx、sy、cy、sz、cz，那么对应的旋转矩阵为
整理可得：
可得：
将以上公式用RAPID代码编写，具体实现过程如下：
