一、算法原理
1、轴角表示法
假设刚体坐标系为B（Oxyz）绕单位向量 ω ⃗ \vec{ω} ω所表示的轴旋转 θ θ θ角，可以推导出其对应的旋转矩阵。首先假设刚体坐标系B的 z z z轴与 ω ⃗ \vecω ω所表示的任意轴重合，然后B坐标系绕参考坐标系A（OXYZ）的Z轴旋转 − α -α −α角使 z z z 轴在XOY 平面的投影与X 轴重合，然后再绕 − β -β −β角，使z 轴和Z 轴重合，接着绕Z 轴旋转 θ θ θ角，最后为了使z 重新回到与 ω ⃗ \vec{ω} ω轴重合的位置，可以绕Y 轴旋转 β β β角和绕Z 轴旋转 β β β角，如下图所示。
2、罗德里格斯公式
二、代码验证
使用PCL进行实现还是非常简单的，两行代码即可：
