【视觉SLAM十四讲-个人笔记】三维空间刚体运动
前言
由于三维空间刚体运动比较重要，后面有可能还会专门做一次总结，结合IMU位姿估计一起吧。本章内容还是好理解的所以不做过多的讨论。
旋转矩阵
旋转矩阵是一个行列式为1的正交矩阵，反之亦然，所以对于所有旋转矩阵，我们把他们放在一个集合里：
\[SO(n)= \{R\in\mathbb{R}^{n\times n}|RR^T=I,det(R)=1 \}
\]
我们称这是一个特殊正交群，表示所有旋转矩阵组成的群，n=3表示3维。这里提前了解了一下群论，再后期会仔细的进行讲解。
- 性质:
由于是正交阵，所以对于旋转矩阵R有：\(R^{-1}=R^T\),可以减少大量的计算量
变换矩阵
变换矩阵也可以组成一个群：
\[ SE(3)=\{T=\begin{pmatrix}R&t\\0^T&1 \end{pmatrix}\in\mathbb{R^{4\times4}}| R\in SO(3),t\in \mathbb{R^3} \}
\]
变换矩阵有着与旋转矩阵相似的性质：
\[T^{-1}=\begin{bmatrix}
R^T&-R^Tt\\0^T&1
\end{bmatrix}
\]
旋转向量与Rodrigues 公式
任意旋转都可以由一个旋转轴与一个旋转角度来描述：\(\theta\vec{n}\)
旋转向量方便描述，但是不方便计算，旋转矩阵就便于计算，任何一个向量左乘上旋转矩阵，都表示旋转后的向量。然后旋转向量和旋转矩阵可以通过Rodrigues 公式来转换：
旋转向量\(\to\)旋转矩阵
\[ R=cos\theta I+(1-cos\theta)\vec{n}\vec{n}^T+sin\theta [\vec{n}]_\times
\]
其中\([\vec{n}]_\times\)在视觉SLAM十四讲书中对应\(\vec{n}^\land\),不过\([ ]_\times\)符号用的比较多。
旋转矩阵\(\to\)旋转向量
\[\theta=arccos(\frac{tr(R)-1}{2})\\
R\vec{n}=\vec{n}
\]
\(tr(R)\)是矩阵的迹，对角线之和，在得到\(\theta\)后再解方程\(R\vec{n}=\vec{n}\)就好了。
欧拉角
直接转步，这个还是要看图看视频好理解。
https://www.jianshu.com/p/21ab3e1d3422
注意欧拉角的死锁问题
四元数
这个就是为了解决死锁问题出来的，而且要比旋转向量方便计算。比旋转矩阵规模要小。
假设某个旋转矩阵是绕单位向量\(\vec{n}=(n_x,n_y,n_z)^T\)进行了角度为\(\theta\)的旋转，则四元数为：
\[q=(cos\frac{\theta}{2},n_xsin\frac{\theta}{2},n_ysin\frac{\theta}{2},n_zsin\frac{\theta}{2})
\]
用四元数表示旋转,若\(p=(0,x,y,z)\),\(p^\prime\)是经过旋转q得到的，那么
\[p^\prime=qpq^{-1}
\]
