第三讲——旋转矩阵

本章说

  题头诗：

  重走SLAM路，之前学的全忘光
  今日重拾起，痛定思痛写总结
  长征路漫漫，君当切实行

  第三讲主要聊了聊旋转矩阵R、变换矩阵T、四元数、欧拉角之间的关系，并做了相关的数学推导。在实际使用中，3D数学库，例如eigen库已经内在处理了它们的转换，所以对于如何从其中一个变换到另一个的数学推导过程可以不求甚解，重点是理解四个描述方式的不同使用场景与各自的特点。

讲内容

  首先解析一下旋转矩阵，我认为其中有两个重要内容：

1. 旋转矩阵$\mathbf{R}$描述了坐标系1仅做旋转变换成为坐标系2后，向量$\mathbf{a}$在坐标系1中的坐标${\mathbf{a}}_1$该如何对应于坐标系2中的${\mathbf{a}}_2$，即${\mathbf{a}}_2={\mathbf{R}}_{21}{\mathbf{a}}_1$（下标遵循右读原则）
2. 旋转矩阵$\mathbf{R}$是一个行列式为1的正交矩阵，它的逆${\mathbf{R}}^{-1}$等于它的转置${\mathbf{R}}^T$，逆描述了一个相反的旋转，即${\mathbf{R}}_{21}={\mathbf{R}}_{12}^{-1}$

  由于旋转矩阵加上平移向量才可以完整描述一个3维空间刚体运动，再加上旋转矩阵和平移向量的下标描述有一定的区别，所以对于平移向量，我认为也有两个重要内容：

1. 平移向量$\mathbf{t}$描述了坐标系1仅做平移变换成为坐标系2后，坐标系1原点$O_1$指向坐标系2原点$O_2$的向量在坐标系1中的坐标，该描述下平移向量记为${\mathbf{t}}_{12}$（下标遵循左读原则）
2. 平移向量取反不代表从1->2的描述变成了2->1的描述，1->2的描述最终以坐标系1生成平移向量，2->1的描述最终由坐标系2生成平移向量，由于旋转的存在，1->2的平移向量取反不等于2->1的平移向量

  再次，解析一下变换矩阵与齐次坐标，其中有两个重要内容：

1. 变换矩阵$\mathbf{T}$是$4\times 4$矩阵，其形式为$\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]$，一次性描述了旋转和平移过程，齐次坐标直接左乘$\mathbf{T}$与先旋转再平移是一致的 （下标遵循右读原则）
2. 变换矩阵求逆同样描述了一个相反的变换过程（旋转$\mathbf{R}$，平移$\mathbf{t}$----->平移$-{\mathbf{R}}^T\mathbf{t}$，旋转${\mathbf{R}}^T$）,即变换矩阵$\mathbf{T}$的逆矩阵${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^T& -{\mathbf{R}}^T\mathbf{t}\\ 0^T& 1\end{array}\right]$

  接着，解析旋转向量，旋转向量有三个重要内容：

1. 旋转向量是旋转轴$\mathbf{n}$与旋转角$\theta$的乘积
2. 旋转向量与旋转矩阵的转换公式由罗德里格斯公式给出，经变换后有：$\theta =acrcos(\frac{\mathbf{t}\mathbf{r}(\mathbf{R})-1}{2})$，$\mathbf{R}\mathbf{n}=\mathbf{n}$，前一个直接确定了旋转角的值，后一个公式对旋转矩阵$\mathbf{R}$做特征分解后，旋转轴即特征值1对应的特征向量 （证明可以从特征分解后，特征向量均正交，左式要等于右式，旋转轴必须等于特征值1对应的特征向量着手分析）
3. 旋转向量存在奇异性，不可直接用于优化

  然后，继续分析欧拉角，这个概念最通俗简单，有两个重要内容：

1. 欧拉角有很多种类，在slam中使用的欧拉角是ZYX顺序，每次均是在旋转之后的刚体上重新确定轴 （X轴为刚体正前方，Y轴为右侧，Z轴为上方，右手坐标系）
2. 欧拉角描述旋转也存在奇异性，不可直接用于优化

  最后，介绍本讲的最后一个概念，四元数，四元数与旋转矩阵，旋转向量之间的转化推导过程比较复杂且有创意，但可以浓缩为两个内容：

1. 四元数可以定义为矩阵形式和一般形式，一般形式为：$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k$，矩阵形式为$\mathbf{q}=[s,\mathbf{v}{]}^T$，需要注意的是，在描述旋转时我们一般使用的都是单位四元数，其具备逆等于共轭的性质
2. （注意，以下的转换公式只在单位四元数$\mathbf{q}$的i,j,k与旋转前坐标系的x,y,z重合时成立） 由单位四元数转换为旋转矩阵的公式为：$\mathbf{R}=\mathbf{v}{\mathbf{v}}^T+s^2\mathbf{I}+2s\mathbf{v}\hat{}+(\mathbf{v}\hat{}{)}^2$。根据该公式则可以推导出对应的旋转角$\theta$，同样是两边取迹，$\theta =2arccos(s)$；以及对应的旋转轴为单位四元数$\mathbf{q}$的虚部除以模，$[n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/sin\frac{\theta }{2}$ （模等于$sin\frac{\theta }{2}$是因为$s=cos\frac{\theta }{2}$，而$\mathbf{q}$又是单位四元数）

写总结

  总的来说，本章的思路还是非常清晰的，围绕着旋转矩阵，变换矩阵，旋转向量，欧拉角，四元数之间的相互联系，可以很好的对整体内容进行理解。这里的话，我们利用eigen库中的各描述子的转换图，来作为本章的结束：

1.四元数---->旋转向量：     v_rotate = AngleAxisd (q)
2.旋转向量----->四元数：    q = Quaterniond (v_rotate)
3.旋转向量----->旋转矩阵： R = v_rotate.matrix() 或 R = v_rotate.toRotationMatrix()
4.旋转矩阵----->旋转向量： v_rotate = AngleAxisd (R)
5.四元数------>旋转矩阵：    R = Matrix3d (q)
6.旋转矩阵----->四元数：     q = Quaterniond (R)
7.四元数----->变换矩阵：     T.rotate(q) , T.pretranslate(t) , T.matrix()
8.旋转矩阵----->欧拉角：      euler_angle = R.eulerAngles(2,1,0) //zyx
9.欧拉角------>旋转矩阵：     R = AngleAxisd(euler_angle[0],Vector3d::UnitZ())*AngleAxisd(euler_angle[1],Vector3d::UnitY())*AngleAxisd(euler_angle[2],Vector3d::UnitX())

为什么欧拉角到旋转矩阵是上述转换式？
首先需要明白，假定有欧拉角zyx，分别绕z轴，y轴，x轴旋转γ，β，α度，可以通过连续左乘（看作三次旋转）完成这一欧拉角旋转，而三次连续左乘组合起来，就是该欧拉角对应的旋转矩阵
${\mathbf{R}}_{AB}={\mathbf{R}}_Z(\gamma ){\mathbf{R}}_Y(\beta ){\mathbf{R}}_X(\alpha )$