视觉SLAM十四讲2nd—学习笔记（一）

3.1旋转矩阵

点、向量、坐标系

向量$a$：$a=e{a}_e={\Sigma }_1^3{a}_i{e}_i$，$e=[e_1,e_2,e_3]$为三维空间的一组基（是一个矩阵，每个元素$e_i$是一个列向量），$a_e$为$a$在这组基下的坐标（是一个列向量）。
内积：$a\cdot b=a_e^T{b}_e=\Sigma a_i{b}_i=|a||b|cos<a,b>$，向量的坐标与基有关，所以右小角有一个${\cdot }_e$，但向量的长度、夹角都与基无关，所以不加${\cdot }_e$，$a_i,b_i$分别是$a_e,b_e$的元素。
外积：$a\times b=det(\left[\begin{array}{c}e\\ a_e^T\\ b_e^T\end{array}\right])=\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]b_e$，
$\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$是一个由$a_e$改写成的反对称矩阵。
注：这么写有点麻烦，后面无特殊说明，$a,b$就指用同一组基表示的向量。
注：反对称矩阵满足$A^T=-A$

坐标系间的欧式变换

世界坐标系或惯性坐标系是一个静坐标系，相机坐标系是一个动坐标系。
刚体运动：如果相机运动是一个刚体运动，则两个坐标系间的运动由一个旋转加一个平移组成。或者说两个坐标系间相差一个欧式变换（Euclidean Transform）。

向量$a$在两个坐标系下的坐标为$a_e,a_{e^{\prime }}$，两个坐标系是经过一次旋转的关系，则有：
$a_e=R{a}_{e^{\prime }}$，其中$R=e^T{e}^{\prime }$称为旋转矩阵或方向余弦矩阵（direction cosine）。
旋转矩阵是行列式为1的正交矩阵；行列式为1的正交矩阵是旋转矩阵。
注：正交矩阵满足$A^T=A^{-1}$，则有$a_{e^{\prime }}=R^{-1}{a}_e$，矩阵求逆描述了反向旋转
注：行列式为-1的正交矩阵是瑕旋转矩阵（一次旋转加一次反射）。

$n$维旋转矩阵的集合： S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = I } SO(n) = \{R \in \mathbb R^{n\times n} | RR^T = I, det(R)=I \} SO(n)={R∈Rn×n∣RRT=I,det(R)=I}，$SO(n)$是特殊正交群的意思。

欧式变换：两个坐标系是一次旋转加一次平移的关系，$a_{e^{\prime }}=R{a}_e+t$，$t$为平移向量
注：将$a_{e^{\prime }}=R{a}_e+t$简写为$a^{\prime }=Ra+t$

变换矩阵与齐次坐标

如果向量$a$至$c$经过两次欧式变换，则有：$c=R_2(R_{1}a+t_1)+t_2$
为了简便表示，引入齐次坐标，将$a^{\prime }=Ra+t$表示为：
$\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]=T\left[\begin{array}{c}a\\ 1\end{array}\right]$
用变换矩阵$T$表示欧式变换，$a$用齐次坐标$\left[\begin{array}{c}a\\ 1\end{array}\right]$代替（为简便表示，$a$就是齐次坐标），则两次欧式变换可写作：$c=T_2{T}_{1}a$。

特殊欧式群： S E ( 3 ) = { T = [ R t 0 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3) = \{T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} \in \mathbb{R}^{4\times 4} | R \in SO(3), t\in \mathbb{R}^3 \} SE(3)={T=[R0​t1​]∈R4×4∣R∈SO(3),t∈R3}。
欧式变换的逆：$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0& 1\end{array}\right]$

3.3 旋转向量与欧拉角

旋转向量

旋转矩阵有9个分量，但一次旋转只有3个自由度，是否有更紧凑的表达？
答：罗德里格斯公式(Rodrigues)，用一个向量$n$和一个标量$\theta$表示

任意旋转都可以用一个旋转轴和一个旋转角表示，转轴是单位向量$n$，转角为$\theta$，则有：
$R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta n^{ssm}$
其中，${\cdot }^{ssm}$表示向量的反对称矩阵
缺点：具有奇异性，$\theta =0$时，$R=I$，$n$可以是任意向量，即转轴不唯一。

由$R$计算$n,\theta$：
$\theta =arccos(\frac{tr(R)-1}{2})$
因为$Rn=n$，所以$n$是$R$的特征值为1对应的特征向量。

欧拉角

欧拉角：将一个旋转分解为三次绕不同轴的旋转，最常见的是采用偏航-俯仰-滚转描述旋转。
偏航-俯仰-滚转：绕Z轴旋转$\alpha$，绕旋转后的Y轴旋转$\beta$，绕旋转后的X轴旋转$\gamma$。
缺点：用三个标量表示旋转会出现奇异性问题（万向锁问题）。

万向锁：如果绕Y轴旋转90度，那么系统丢失一个自由度（绕X轴的旋转）。绕Z轴旋转$\alpha$再绕旋转后的Y轴旋转$90$再绕旋转后的X轴旋转$\beta$ 等于 绕Z轴旋转$\alpha \pm \beta$再绕旋转后的Y轴旋转$90$。
欧拉角演示网站 https://danceswithcode.net/engineeringnotes/rotations_in_3d/demo3D/rotations_in_3d_tool.html

3.4 四元数

四元数即紧凑也没有奇异性
平面旋转可由单位复数描述$e^{i\theta }=cos\theta +isin\theta$，三维旋转可由单位四元数描述。

四元数$q=[s,v{]}^T,s=q_0\in \mathbb{R},v=[q_1,q_2,q_3{]}^T\in {\mathbb{R}}^3$，四元数有三个虚部一个实部。
四元数的加减、乘、逆、数乘和复数类似。

用四元数描述旋转：$p^{\prime }=qp{q}^{-1}$，等效为$p^{\prime }=Rp$
其中，三维空间点$p=[0,x,y,z{]}^T$（用虚四元数表示），单位四元数$q$。
注：用四元数表示旋转，则$p$为虚四元数；用旋转矩阵表示旋转，则$p$为三维向量。需要读者带入当前语境中自行理解。

旋转即可用单位四元数$q=[s,v{]}^T$表示，也可用旋转矩阵表示，则前者转换为后者的公式为：
$R=v{v}^T+s^{2}I+2s{v}^{ssm}+(v^{ssm}{)}^2$
反过来的转换公式：
$\theta =2arccos(s),n=[q_1,q_2,q_3]/sin(\theta /2)$

3.5 相似、仿射、射影变换

相似变换比欧式变换多了一个自由度：缩放，变换矩阵为：
$T_S=\left[\begin{array}{cc}sR& t\\ 0& 1\end{array}\right]$
用$Sim(3)$表示三维相似变换的集合（相似变换群）

仿射变换（正交投影）仅要求$A$是一个可逆矩阵，不是必须为一个可逆矩阵，变换矩阵为：
$T_A=\left[\begin{array}{cc}A& t\\ 0& 1\end{array}\right]$

射影变换是最一般的变换，变换矩阵为：
$T_A=\left[\begin{array}{cc}A& t\\ a^T& v\end{array}\right]$
其中，$A$是一个可逆矩阵，左下角为缩放，右上角为平移

真实世界到相机照片的变换，如果焦距有限是射影变换，焦距无限是仿射变换。

欧式变换：保持长度、夹角、体积，6自由度；
相似变换：保持体积比，7自由度；
仿射变换：保持平行性、体积比，12自由度；
射影变换：保持接触平面的相交和相切，15自由度。
注：欧式变换的旋转矩阵有正交性，将其自由度缩减为3，再加上平移向量，共6个自由度。
注：射影变换右下角的$v=0$或$1$，共15个自由度

有空继续补充一下，编程及其运行结果。