视觉SLAM十四讲--第三章旋转公式_绕一轴旋转旋转矩阵

第三讲 三维空间刚体运动

本讲的主要问题是：一个刚体在三维空间中的运动是如何描述的。（一次旋转加一次平移）

3.1 旋转矩阵

1…点和向量，坐标系

点–
向量—指具有大小（magnitude）和方向的量。可以想象成从原点指向某处的一个箭头。是空间中的一样东西。在没有确定坐标系的情况下，不能讨论向量的坐标。如果确定了坐标系，即一个线性空间的基（$e_1,e_2,e_3$）

日了狗，没保存，丢了
增加个博客内容吧：https://zhuanlan.zhihu.com/p/78987582 （介绍了点积，叉积，旋转等内容，较详细）

对等式左右同时左乘

中间矩阵为R，为两组基之间的内积，称为旋转矩阵。旋转矩阵是一个行列式为1的正交矩阵，反之，行列式为1的正交矩阵也是一个旋转矩阵。

旋转矩阵为正交阵，他的逆（即转置）描述了一个相反的旋转。

加上平移：

3. 变换矩阵与齐次坐标

欧式空间描述旋转与平移，存在一个小问题，即多次变换，导致非线性

于是引入齐次坐标和变换矩阵重写：使得整个关系变成了线性关系，$T$称为变换矩阵。

3.3 旋转向量和欧拉角

1.旋转向量

旋转矩阵的缺点：
（1）九个量，只需三个自由度，造成冗余。而变换矩阵用十六个量表达六自由度的变换。
（2）自身带有约束：正交矩阵，且行列式为1。变换矩阵亦是如此，这些约束使得求解困难。

旋转向量：用一个旋转轴和一个旋转角来表示旋转。 刚体变换可以用六个自由度表示。采用方向与旋转轴一致，长度等于旋转角的三维旋转向量描述旋转，因此，变换矩阵就可以使用一个旋转向量和一个平移向量表达，维数为六维（就是李代数，下一章详细介绍，这里只是知道如何表达）。

旋转向量和旋转矩阵之间的转换：

假设旋转轴为k,角度为$\theta$,对应的旋转向量$\theta k$。

补充：罗德里格斯公式：

向量$\mathbf{v}$在旋转轴$\mathbf{k}$所在坐标系$\mathbf{k}=[v_{\parallel }，v_{\perp }，w]$下可分解为两个方向的分量相加，即，$\mathbf{v}={\mathbf{v}}_{\parallel }+{\mathbf{v}}_{\perp }$，绕旋转轴旋转$\theta$角度后的向量为${\mathbf{v}}_{\mathbf{r}\mathbf{o}\mathbf{t}}$.

其中${\mathbf{v}}_{\perp }$与$\mathbf{k}$正交，${\mathbf{v}}_{\parallel }$为$\mathbf{v}$在$\mathbf{k}$上的正交投影。由正交投影公式：

$v_{\parallel }=（\mathbf{k}.\mathbf{v}）\mathbf{k}$

根据上边的公式可以写成:

${\mathbf{v}}_{\perp }=\mathbf{v}-{\mathbf{v}}_{\parallel }=\mathbf{v}-（\mathbf{k}.\mathbf{v}）\mathbf{k}$，而${\mathbf{v}}_{\parallel }$与$\mathbf{v}$重合，旋转时不变。$\mathbf{w}=\mathbf{k}\times {\mathbf{v}}_{\perp }=\mathbf{k}\times \mathbf{v}$

根据图中向量和角度之间的关系，可以得出结论：

${\mathbf{v}}_{\mathbf{r}\mathbf{o}\mathbf{t}}={\mathbf{v}}_{\parallel }-{\mathbf{v}}_{\perp }\mathbf{c}\mathbf{o}\mathbf{s}(\mathbf{\pi }-\mathbf{\theta })+\mathbf{w}\mathbf{s}\mathbf{i}\mathbf{n}(\mathbf{\pi }-\mathbf{\theta })={\mathbf{v}}_{\parallel }+{\mathbf{v}}_{\perp }\mathbf{c}\mathbf{o}\mathbf{s}\mathbf{\theta }+\mathbf{w}\mathbf{s}\mathbf{i}\mathbf{n}\mathbf{\theta }$

带入相关公式（向量交换律,叉乘，反对称等性质）：

${\mathbf{v}}_{\mathbf{r}\mathbf{o}\mathbf{t}}={\mathbf{v}}_{\parallel }+{\mathbf{v}}_{\perp }\mathbf{c}\mathbf{o}\mathbf{s}\mathbf{\theta }+\mathbf{w}\mathbf{s}\mathbf{i}\mathbf{n}\mathbf{\theta }$

=$（\mathbf{k}.\mathbf{v}）\mathbf{k}+(\mathbf{v}-（\mathbf{k}.\mathbf{v}）\mathbf{k})\mathbf{c}\mathbf{o}\mathbf{s}\mathbf{\theta }+(\mathbf{k}\times \mathbf{v})\mathbf{s}\mathbf{i}\mathbf{n}\mathbf{\theta }$

$=\mathbf{c}\mathbf{o}\mathbf{s}\mathbf{\theta }\mathbf{v}+(1-\mathbf{c}\mathbf{o}\mathbf{s}\mathbf{\theta })(\mathbf{k}.\mathbf{v}）\mathbf{k}+(\mathbf{k}\times \mathbf{v})\mathbf{s}\mathbf{i}\mathbf{n}\mathbf{\theta }$

$=(\mathbf{c}\mathbf{o}\mathbf{s}\mathbf{\theta }\mathbf{I}+(1-\mathbf{c}\mathbf{o}\mathbf{s}\mathbf{\theta })\mathbf{k}{\mathbf{k}}^{\mathbf{T}}+\mathbf{s}\mathbf{i}\mathbf{n}\mathbf{\theta }\mathbf{k}\wedge )\mathbf{v}$

$\mathbf{R}=\mathbf{c}\mathbf{o}\mathbf{s}\mathbf{\theta }\mathbf{I}+(1-\mathbf{c}\mathbf{o}\mathbf{s}\mathbf{\theta })\mathbf{k}{\mathbf{k}}^{\mathbf{T}}+\mathbf{s}\mathbf{i}\mathbf{n}\mathbf{\theta }\mathbf{k}\wedge$

对矩阵$\mathbf{R}$左右求秩,根据公式（3.3）得到：

$\mathbf{t}\mathbf{r}(\mathbf{R})=\mathbf{c}\mathbf{o}\mathbf{s}\mathbf{\theta }\mathbf{t}\mathbf{r}(\mathbf{I})+(1-\mathbf{c}\mathbf{o}\mathbf{s}\mathbf{\theta })\mathbf{t}\mathbf{r}(\mathbf{k}{\mathbf{k}}^{\mathbf{T}})+\mathbf{s}\mathbf{i}\mathbf{n}\mathbf{\theta }\mathbf{t}\mathbf{r}(\mathbf{k}\wedge )=3\mathbf{c}\mathbf{o}\mathbf{s}\mathbf{\theta }+(1-\mathbf{c}\mathbf{o}\mathbf{s}\mathbf{\theta })=1+2\mathbf{c}\mathbf{o}\mathbf{s}\mathbf{\theta }$

可以得出：$\theta =arccos((tr(\mathbf{R})-1)/2)$

2.欧拉角

欧拉角动态旋转

万向锁问题

3.4 四元数

补充知识：

复数表示旋转的描述 ：https://editor.csdn.net/md/?articleId=107150734
3D旋转和罗德里格斯公式：https://editor.csdn.net/md/?articleId=107173576

四元数定义

旋转矩阵用九个量描述三自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的，但
具有奇异性。

四元数
是 Hamilton 找到的一种扩展的复数. 它既是紧凑的，也没有奇异性。如果说缺点的话，四
元数不够直观，其运算稍为复杂一些。

3.2 四元数与3D旋转

将一个向量$\mathbf{v}$沿着一个用单位向量所定义的旋转轴$\mathbf{u}$旋转$\theta$度，因此，可以$\mathbf{v}$拆分成正交于旋转轴向量${\mathbf{v}}_{\perp }$和平行于旋转轴的向量${\mathbf{v}}_{\parallel }$，即$\mathbf{v}={\mathbf{v}}_{\perp }+{\mathbf{v}}_{\parallel }$

结论：如果绕着同一个轴$\mathbf{u}$连续旋转$\theta$度两次，等同意直接绕轴$\mathbf{u}$旋转2$\theta$度.

利用引理2和引理3可以得出：