《视觉SLAM十四讲》第三章阅读笔记（一）

《视觉SLAM十四讲》第三章阅读笔记（一）

第三章主要描述三维空间的刚体运动，主要的描述的方式为旋转矩阵、四元数和欧拉角。本文中将介绍三种之间的转换方式以及Eigen库和ROS系统中的关于旋转矩阵、四元数和欧拉角的具体表达和之间转换。，且本文都是基于右手系进行推导。

1.旋转矩阵

定义一个三维旋转矩阵R，用来表示两个坐标之间的坐标变换关系，同时也是一个行列式为1的正交矩阵，因此可以定义三维空间的李群SO（3）。
S O ( 3 ) = { R ∈ R n × n ｜ R R T = I , d e t ( R ) = 1 } SO(3)=\{\bm{R}\in\mathbb{R^{n\times{n}}}｜\bm{RR}^{T}=\bm{I},det(\bm{R})=1\} SO(3)={R∈Rn×n｜RRT=I,det(R)=1}
在SLAM系统里面，车体的旋转运动会体现到相机的旋转，当然前提是将车体和相机之间的连接看成刚性连接，意味着当相机测出旋转运动时，可以反推出车体的运动，从而求出车体的位姿。然而通常情况下，旋转矩阵不足以表达出车体的运动，在车辆行驶过程中，除了旋转还有平移运动，因此还需要引入平移向量t。则有：
$$\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^{\mathbf{T}}& 1\end{array}\right]$$
这里引入的是齐次坐标系，然而为何引入齐次坐标呢？首先需要解答的第一个问题是什么是齐次坐标，齐次坐标就是在原有坐标基础上再加一个维度，如：
$$(x,y)\to (x,y,1)$$
$$(x,y,z)\to (x,y,z,1)$$
如果直线l可以用直线方程ax+by+c=0来表示的话，该直线用向量表示的话，记做：
$$l=(x,y,1{)}^T$$
则直线的坐标可以设置成$$P=(x,y,1)$$直线方程则为两点之间的点乘也就是内积。
$$ax+by+c\times 1=(a,b,c{)}^T(x,y,1)=l^T\ast P^{\prime }=0$$
同样地，在三维空间内假设平面方程A为
$$ax+by+cz+d=0，P=（x，y，z，1）$$
$$ax+by+cz+d=(a,b,c,d{)}^T(x,y,z,1)=A^T\ast P^{\prime }=0$$
从普通坐标转换成齐次坐标时
如果(x,y,z)是个点，则变为(x,y,z,1);
如果(x,y,z)是个向量，则变为(x,y,z,0)
从齐次坐标转换成普通坐标时
如果是(x,y,z,1)，则知道它是个点，变成(x,y,z);
如果是(x,y,z,0)，则知道它是个向量，仍然变成(x,y,z)
具体可见：齐次坐标的理解
使用齐次坐标可以将欧式变换中的加法转换为乘法，如二维平面点$x=[u，v]$，平移量为$t=[tu,tv]$，变换后的点应为：
$$x^{\prime }\to \left[\begin{array}{c}{u}^{\prime }\\ v^{\prime }\end{array}\right]=\left[\begin{array}{c}u+tu\\ v+t_v\end{array}\right]=x+t$$
将齐次坐标作为替代，重写写为：
$$x^{\prime }\to \left[\begin{array}{c}{u}^{\prime }\\ v^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ccc}1& 0& u\\ 0& 1& v\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\mathbf{T}\mathbf{x}$$

在三维空间内，SLAM存在存在着旋转和平移，考虑世界坐标系中的向量a，经过一次旋转（用 R 描述）和一次平移 t 后，得到了 a′，那么把旋转和平移合到一起，有：
$${\mathbf{a}}^{\prime }=\mathbf{R}\mathbf{a}+\mathbf{t}$$
其中，t 称为平移向量。相比于旋转，平移部分只需把这个平移量加到旋转之后的坐标上，
显得非常简洁。通过上式，我们用一个旋转矩阵 R 和一个平移向量 t 完整地描述了一个欧氏空间的坐标变换关系。
引入齐次坐标和变换矩阵重写：
$$\left[\begin{array}{c}{\mathbf{a}}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^{\mathbf{T}}& 1\end{array}\right]\left[\begin{array}{c}\mathbf{a}\\ 1\end{array}\right]\triangleq \mathbf{T}\left[\begin{array}{c}\mathbf{a}\\ 1\end{array}\right]$$
同样地，特殊欧氏群为：
S E ( 3 ) = { T = [ R t   0 T 1 ] ∈ R 4 × 4 ｜ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3)=\{\bm{T=\begin{bmatrix} \bm{R}&\bm{t} \\\ 0^{\bm{T}} & 1 \end{bmatrix}}\in\mathbb{R^{4\times4}}｜\bm{R}\in SO(3),t\in\mathbb{R}^{3}\} SE(3)={T=[R 0T​t1​]∈R4×4｜R∈SO(3),t∈R3}

2.旋转向量

使用旋转矩阵来表示三维空间的变换存在以下缺点：
1.SO(3) 的旋转矩阵有九个量，但一次旋转只有三个自由度。因此这种表达方式是冗
余的。同理，变换矩阵用十六个量表达了六自由度的变换。
2.旋转矩阵自身带有约束：正交矩阵，且行列式为 1。变换矩阵也是如此。当
估计或优化一个旋转矩阵/变换矩阵时，这些约束会使得求解变得更困难。
引入一个新的概念:旋转向量，即用一个旋转轴和一个旋转角来刻画。关键关键问题在于旋转矩阵和旋转向量之间的转换。
接下来关于罗德里格斯公式（Rodrigues’s Formula）的推导。
首先给出推导的结果，即罗德里格斯公式（Rodrigues’s Formula ）。
$$R=cos\theta \mathbf{I}+(1-cos\theta )\mathbf{n}{\mathbf{n}}^{\mathbf{T}}+sin\theta {\mathbf{n}}^{\wedge }$$
使用旋转矩阵表示的R为
$$R=\left[\begin{array}{ccc}{r}_{xx}& r_{xy}& r_{xz}\\ r_{yx}& r_{yy}& r_{yz}\\ r_{zx}& r_{zy}& r_{zz}\end{array}\right]$$
上述已经描述过，旋转矩阵为标准的正交矩阵且矩阵的行列式为1，下式表达：
$${\mathbf{R}}^T\mathbf{R}={\mathbf{R}}^{-1}\mathbf{R}=\mathbf{E}$$
$$|\mathbf{R}|=1$$
假设原坐标系基向量矩阵为 B，旋转后的坐标系基向量矩阵为 C。
$$\mathbf{B}=[b_x,b_y,bz]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$
$$\mathbf{C}=\mathbf{R}\mathbf{B}$$
即
$$\mathbf{C}=\left[\begin{array}{ccc}{r}_{xx}& r_{xy}& r_{xz}\\ r_{yx}& r_{yy}& r_{yz}\\ r_{zx}& r_{zy}& r_{zz}\end{array}\right][b_x,b_y,bz]$$
假设一个单位向量
$$n=[n_x,n_y,n_z]$$

考虑向量旋转，其中$v$作为原向量，$v_{rot}$为旋转后的向量。先通过点积得到$v$在$n$方向的平行分量$v_{||}$:
$$v_{||}=(v\cdot n)n$$
再通过叉乘得到与$k$正交的两个向量$v_{\perp }$与$w$。
$$v_{\perp }=v-v_{\parallel }=v-(v\cdot k)k=-k\times (k\times v)\cdot \cdot \cdot \cdot \cdot \cdot$$
$$w=k\times v$$
根据图中可以得出：
$$v_{rot}=v_{\parallel }+cos(\theta )v_{\perp }+sin(\theta )w$$
反对称矩阵可见反对成矩阵
则$n$的反对称矩阵为：
$$n^{\wedge }=\left[\begin{array}{ccc}0& -n_z& n_y\\ n_z& 0& -n_x\\ -n_y& n_x& 0\end{array}\right]$$其中
$$n\times v=n^{\wedge }v$$
因此，可以得到以下公式
$$v_{\parallel }=v+n\times (n\times v)$$
所以可以得到
$$v_{rot}=v+n\times (n\times v)-cos(\theta )k\times (k\times v)+sin(\theta )k\times v$$
根据叉积矩阵性质：
$$v_{rot}=v+(1-cos(\theta )(n^{\wedge }{)}^{2}v+sin(\theta )(n^{\wedge }{)}^{2}v$$
$$v_{rot}=(I+(1-cos(\theta ))(n^{\wedge }{)}^{2}v+sin(\theta )(n^{\wedge }{)}^{2}v$$
将$B$,$C$代入可知：
$$B=(I+(1-cos(\theta ))(n^{\wedge }{)}^2+sin(\theta )n^{\wedge })C\iff \mathbf{R}=I+(1-cos(\theta ))(n^{\wedge }{)}^2+sin(\theta )n^{\wedge }$$
同时，反过来计算，从旋转矩阵到旋转向量的转换。对于转角$\theta$，对每一项取迹有
$$tr(\mathbf{R})=cos\theta tr(\mathbf{I})+(1-cos\theta )tr({\mathbf{n}\mathbf{n}}^T)+sin\theta tr({\mathbf{n}}^{\wedge })=3cos\theta +(1-cos\theta )=1+2cos\theta$$
符号${}^{\wedge }$为反对称矩阵转换符。
因此：
$$\theta =arcos(\frac{tr(\mathbf{R})-1}{2})$$
关于转轴$n$，由于旋转轴上的向量在旋转后不发生改变，说明：
$$\mathbf{R}\mathbf{n}=\mathbf{n}$$

参考：
罗德里格斯公式 理解、推导