视觉SLAM——二维三维几何、三维空间刚体变换

前言

本博客为主要学习《视觉SLAM十四讲》第3讲、《机器人学的状态估计》第6章三位几何学基础、《计算机视觉-算法和应用》第2章2.1几何基元变换等SLAM内容的总结与整理。
主要包括：
1、点、向量、坐标系、直线、平面等几何学基础
2、旋转矩阵、角轴、欧拉角、四元数等表示坐标系旋转的方法，罗德里格斯公式的证明，四元数左乘右乘、导数的推导
3、2D、3D空间的欧式、相似、仿射、射影变换性质

---

1、几何学基础

1.1 点

2D点，如图像中的像素坐标，可以用一对数值表示：$x=(x,y)\in {\mathbf{R}}^2$。
也可以用齐次坐标表示：
$$\tilde{x}=(\tilde{x},\tilde{y},\tilde{w})\in {\mathbf{P}}^2$$

其中仅在尺度上不同的矢量被视为等同的。${\mathbf{P}}^2={\mathbf{R}}^3-(0,0,0)$称作2D投影空间。齐次矢量$\tilde{x}$可以通过除以最后一个元素$\tilde{w}$转换为非齐次矢量$x$，即$\tilde{x}=(\tilde{x},\tilde{y},\tilde{w})=\tilde{w}(x,y,1)=\tilde{w}\bar{x}$，$\bar{x}$成为增广矢量。

3D点，也可以写成非齐次坐标$x=(x,y,z)\in {\mathbf{R}}^3$或齐次坐标$\tilde{x}=(\tilde{x},\tilde{y},\tilde{z},\tilde{w})=\tilde{w}(x,y,z,1)\in {\mathbf{P}}^3$。

1.2 向量与坐标系

向量是线性空间中的一个元素，可以想象成从原点指向某处的一个箭头。只有当指定三维空间的坐标系时，才存在向量在此坐标系下的坐标。例如确定了三维空间中坐标系$(e_1,e_2,e_3)$，向量在这组基下的坐标为：

$$a=[e_1,e_2,e_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=a_1{e}_1+a_2{e}_2+a_3{e}_3$$

坐标系通常由3个正交的坐标系组成。右手坐标系即给定x轴和y轴时，z轴的方向可以通过右手法则由$x\times y$定义。

向量内积：
$$a\cdot b=a^{T}b=\sum _{i=1}^3{a}_i{b}_i=|a||b|cos<a,b>$$

向量外积：
$$a\times b=\Vert \begin{array}{ccc}i& j& k\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\Vert =\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\triangleq a^{\wedge }b$$

引入^符号，将a写成一个反对称矩阵，将向量的外积转变为向量与矩阵的乘法。外积只对向量存在定义。

在三维几何中，向量a和向量b的外积结果是垂直于a和b向量构成平面的法向量，大小为向量a和向量b构成的平行四边形的面积。

1.3 2D直线

2D直线也可以用齐次坐标表达$\tilde{l}=(a,b,c)$。对应的直线方程为：
$$\bar{x}\tilde{l}=ax+by+c=0$$

也可以规范化直线方程，使得
$$l=({\hat{n}}_x,{\hat{n}}_y,d)=(\hat{n},d)，||\hat{n}||=1$$

此时$\hat{n}$是垂直于直线的法向量，d是其到原点的距离。但是这样有个例外，就是无穷远处的直线$\tilde{l}=(0,0,1)$，它包含了所有无穷远处的点。

使用齐次坐标系，可以计算两条直线的角点：
$$\tilde{x}={\tilde{l}}_1\times {\tilde{l}}_2$$

计算两点的直线：
$$\tilde{l}={\tilde{x}}_1\times {\tilde{x}}_2$$

点到直线的距离
1）对于点$(x_0,y_0)$，到直线$ax+by+c=0$的距离d可以如下计算：
$$d=|\frac{a{x}_0+b{y}_0+c}{\sqrt{a^2+b^2}}|$$

2）对于点A、B、C，求点A到直线BC的距离还可以这样算：
$$d=\frac{|AB\times BC|}{|BC|}$$

1.4 3D平面

3D平面也可以表达为齐次坐标$\hat{m}=(a,b,c,d)$，对应的平面方程为：
$$\bar{x}\cdot \hat{m}=ax+by+cz+d=0$$

也可以规范化平面方程，使得
$$l=({\hat{n}}_x,{\hat{n}}_y,{\hat{n}}_z,d)=(\hat{n},d)，||\hat{n}||=1$$

此时$\hat{n}$是垂直于平面的法向量，d是其到原点的距离。同样有个例外，就是无穷远处的平面$\tilde{l}=(0,0,0,1)$，它包含了所有无穷远处的点。

点到平面的距离
1）对于点$(x_0,y_0)$，到平面$ax+by+cz+d=0$的距离d可以如下计算：
$$d=|\frac{a{x}_0+b{y}_0+c{z}_0+d}{\sqrt{a^2+b^2+c^2}}|$$

2）对于点A、B、C、D，求点A到平面BCD的距离还可以这样算：
$$d=\frac{|(BC\times BD)\cdot AB|}{|BC\times BD|}$$

1.5 3D直线

3D的直线表达可以用直线上的两个点$(p,q)$，直线上的其他点可以表示为这两个点的线性组合：
$$r=(1-\lambda )p+\lambda q，\tilde{r}=(1-\lambda )\tilde{p}+\lambda \tilde{q}$$

一种特殊情况是当第二个点位于无穷远时，即$\tilde{q}=({\hat{d}}_x,{\hat{d}}_y,{\hat{d}}_z,0)=(\hat{d},0)$。这里$\hat{d}$即为直线的方向。可以讲非齐次坐标的3D直线方程重写为：
$$r=p+\lambda \hat{q}$$

---

2、旋转表示方法

2.1 旋转矩阵与欧式变换

假设某个单位正交基$(e_1,e_2,e_3)$经过一次旋转变成了$(e_1^{\prime },e_2^{\prime },e_3^{\prime })$，对于同一个向量a，它在两个坐标系下的坐标为$[a_1,a_2,a_3{]}^T$和$[a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^T$，有：
$$[e_1,e_2,e_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=[e_1^{\prime },e_2^{\prime },e_3^{\prime }]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]$$

$$\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{e}_1^T{e}_1^{\prime }& e_1^T{e}_2^{\prime }& e_1^T{e}_3^{\prime }\\ e_2^T{e}_1^{\prime }& e_2^T{e}_2^{\prime }& e_2^T{e}_3^{\prime }\\ e_3^T{e}_1^{\prime }& e_3^T{e}_2^{\prime }& e_3^T{e}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\triangleq R{a}^{\prime }$$

旋转矩阵R为行列式为1的正交矩阵，其集合定义如下：
S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } SO(n)=\{R∈R^{n\times n}|RR^T=I,det(R)=1\} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}

SO(n)为特殊正交群（Special Orthogonal Group）。

欧拉旋转定理：刚体在三维空间里的一般运动可以分解为刚体上方某一点的平移，和绕经过此点的旋转轴的转动。

欧式变换可以使用旋转矩阵R和平移向量t完整地描述：
$$a^{\prime }=Ra+t$$

引入齐次坐标，重写变换矩阵为：
$$\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]\triangleq T\left[\begin{array}{c}a\\ 1\end{array}\right]$$

变换矩阵T又被称为特殊欧式群（Special Euclidean Group）：
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3)=\{T=\left[ \begin{matrix} R&amp;t\\0^T&amp;1 \end{matrix} \right]\in R^{4\times 4}|R\in SO(3),t \in R^3\} SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}

变换矩阵的逆为：$\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]$

习惯上我们采用旋转矩阵$R_{12}$或者$R_2^1$表示从坐标系2到坐标系1的旋转矩阵：
$$a_1=R_{12}{a}_2$$

---

对SLAM来说，我们会用到相机坐标系和世界坐标系。

$T_{wc}$表示相机坐标系到世界坐标系的变换，常用来表示相机的位姿，因为更加直观：其平移部分是相机原点在世界坐标系下的坐标：
$$p_w=T_{wc}{p}_c=T_{wc}0=t_{wc}$$

$T_{cw}$表示世界坐标系到相机坐标系的变换，在SLAM输出的结果中更加常用。

假设世界坐标系到相机坐标系的变换Rt，相机中心在世界坐标系中的位置：

$$R{p}_w+t=p_c=0\Rightarrow p_w=-R^{T}t$$

相机朝向（Z轴）在世界坐标系下的方向为：
$$r^c=Z^c-O_{cam}^c,Z^c=(0,0,1{)}^T,O_{cam}^c=(0,0,0{)}^T$$

$$r^w=(R^T{Z}^c-R^{T}t)-(R^T{O}_{cam}^c-R^{T}t)=R^T\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right)$$

即旋转矩阵的第三行。

---

2.2 旋转向量

任意旋转可以用一个旋转轴和一个旋转角表示。定义旋转轴为单位向量$n=[a_1,a_2,a_3{]}^T$，满足：
$$n^{T}n=a_1^2+a_2^2+a_3^2=1$$

定义旋转角为$\theta$，得到三维的旋转向量$\theta n$（或角轴Angle-Axis）来描述旋转。

从旋转向量到旋转矩阵的转换有罗德里格斯公式：
$$R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta n^{\wedge }$$

---

罗德里格斯公式的证明：
假设初始向量 $v$ 绕旋转轴 $k$（单位向量） 旋转 $\theta$ 角得到 $v_{rot}$。

对v进行向量分解：$v=v_{\perp }+v_{\parallel }$，其中$v_{\parallel }=(v\cdot k)k$

由旋转过程平行向量不变性得：$v_{rot\parallel }=v_{\parallel }$

而$v_{rot\perp }=cos\theta v_{\perp }+(sin\theta |v_{\perp }|)k\times \frac{v_{\perp }}{|v_{\perp }|}=cos\theta v_{\perp }+sin\theta k\times v$

因此

$$v_{rot}=v_{rot\parallel }+v_{rot\perp }=v_{\parallel }+cos\theta v_{\perp }+sin\theta k\times v$$

$$=cos\theta v+(1-cos\theta )(v\cdot k)k+sin\theta k\times v$$

其中$(v\cdot k)k=k(v\cdot k)=k(k^{T}v)=k{k}^{T}v$
所以$$v_{rot}=cos\theta v+(1-cos\theta )k{k}^{T}v+sin\theta k^{\wedge }v=Rt$$

$$R=cos\theta I+(1-cos\theta )k{k}^T+sin\theta k^{\wedge }$$

如果用叉乘来表示$v_{\perp }$，有：$v_{\perp }=-k\times (k\times v)$

$$v_{rot}=v_{rot\parallel }+v_{rot\perp }=v_{\parallel }+cos\theta v_{\perp }+sin\theta k\times v$$

$$=v+(1-cos\theta )k\times (k\times v)+sin\theta k\times v$$

$$=(I+(1-cos\theta )k^{\wedge }{k}^{\wedge }+sin\theta k^{\wedge })v$$

罗德里格斯公式也可以写成：

$$R=I+(1-cos\theta )k^{\wedge }{k}^{\wedge }+sin\theta k^{\wedge }$$

---

同时可以得到旋转矩阵到旋转向量的变换：
两边取迹：

$$\begin{gathered} tr(R)=cos\theta tr(I)+(1-cos\theta )tr(n{n}^T)+sin\theta tr(n^{\wedge }) \\ =3cos\theta +1-cos\theta =1+2cos\theta \end{gathered}$$

因此：

$$\theta =arccos(\frac{tr(R)-1}{2})$$

对于转轴$n$，由于旋转轴上的向量在旋转后不变，有：
$$Rn=n$$

因此也可以说，转轴是旋转矩阵的特征值1所对应的特征向量。

2.3 欧拉角

欧拉角（Euler Angles），将旋转分解为三个方向上的转动。分解方式有多种，同时根据绕固定轴还是绕旋转后的轴旋转也会有不一样的定义方式。
比如按Z-Y-X顺序转动得到yaw（偏航角）-pitch（俯仰角）-roll（滚转角），用$[r,p,y{]}^T$表示

欧拉角的缺点是会遇到万向锁问题：在俯仰角为±90°时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失一个自由度。这被成为奇异性问题。因此欧拉角不适于插值和迭代，往往用于人机交互中。

理论可以证明，对于只有三个参数的表示形式，则必然会存在奇异点。所以没有完美的旋转表示形式。

2.4 四元数

四元数的相关知识推荐看《Quaternion kinematics for the error-state Kalman filter》，这里只对重要的性质做简单介绍。

四元数具有一个实部和三个虚部，常用一个标量和一个向量来表示：
$$q=q_0+q_{1}i+q_{2}j+q_{3}k=[s,v{]}^T，s=q_0\in R，v=[q_1,q_2,q_3{]}^T\in R^3$$

四元数的性质

这里主要讨论四元数的左右乘和求导。

四元数的乘法有：
$$p\otimes q=\left[\begin{array}{c}{s}_p{s}_q-v_p^T{v}_q\\ s_p{v}_q+s_q{v}_p+v_p\times v_q\end{array}\right]=[p{]}_{L}q=[q{]}_{R}p$$

其中：
$$[p{]}_L=\left[\begin{array}{cc}{s}_p& -v_p^T\\ v_p& s_{p}I+v_p^{\wedge }\end{array}\right]=s_{p}I+\left[\begin{array}{cc}0& -v_p^T\\ v_p& v_p^{\wedge }\end{array}\right]，[p{]}_R=\left[\begin{array}{cc}{s}_q& -v_q^T\\ v_q& s_{q}I-v_q^{\wedge }\end{array}\right]=s_{q}I+\left[\begin{array}{cc}0& -v_q^T\\ v_q& -v_q^{\wedge }\end{array}\right]$$

注意如果写成虚部在前实部在后的四元数形式，左乘和右乘矩阵的符号有所变化。

同时有性质：
$$[p{]}_L[q{]}_R=[q{]}_R[p{]}_L$$

单位四元数满足：$q^{T}q=1$

四元数与旋转

假设某旋转是绕着单位向量$n=[n_x,n_y,n_z]$进行角度为$\theta$的旋转，那么四元数可表示为：
$$q=[cos\frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2},n_{z}sin\frac{\theta }{2}{]}^T$$

反之：
$$\theta =2arccos{q}_0，n=[q_1,q_2,q_3{]}^T/sin\frac{\theta }{2}$$

在四元数中，任意的旋转都可以由两个相反的四元数表示。假设空间三维点$p=[x,y,z{]}^T\in R^3$经过轴角$n\theta$变为$p^{\prime }$。首先将三维空间点用虚四元数表示：
$$p=[0,x,y,z{]}^T=[0,v{]}^T$$

用四元数表示旋转：
$$q=[cos\frac{\theta }{2},nsin\frac{\theta }{2}]，p^{\prime }=qp{q}^{-1}$$

结果仍为纯虚四元数，虚部的三个分量表示旋转后的3D点坐标。

四元数的导数

首先需要注意的是，对于用任何方式表示旋转时，对一个旋转再进行旋转有左乘和右乘两种形式，分别代表不同的意义。

首先定义四元数的导数有：

$$\dot{q}\triangleq \underset{\Delta t\to 0}{\lim }\frac{q(t+\Delta t)-q(t)}{\Delta t}=\underset{\Delta t\to 0}{\lim }\frac{q\otimes \Delta q_L-q}{\Delta t}=\underset{\Delta t\to 0}{\lim }\frac{\Delta q_G\otimes q-q}{\Delta t}$$

其中$\Delta q_L$表示局部坐标系中的角度扰动，而$\Delta q_G$表示全局（世界）坐标系中的角度扰动。我们实际测量的角速度往往是在物体坐标系下的角速度$w$（例如VIO中通过IMU陀螺仪获得），故常采用四元数右乘形式。

测量的角速度可以定义为：

$$w_L(t)\triangleq \frac{d{\varphi }_L(t)}{dt}\triangleq \underset{\Delta t\to 0}{\lim }\frac{\Delta {\varphi }_L}{\Delta t}$$

那么有：

$$\dot{q}\triangleq \underset{\Delta t\to 0}{\lim }\frac{q\otimes \Delta q_L-q}{\Delta t}=\underset{\Delta t\to 0}{\lim }\frac{q\otimes (\left[\begin{array}{c}1\\ \frac{\Delta {\varphi }_L}{2}\end{array}\right]-\left[\begin{array}{c}1\\ 0\end{array}\right])}{\Delta t}$$

$$=\underset{\Delta t\to 0}{\lim }\frac{q\otimes \left[\begin{array}{c}0\\ \frac{\Delta {\varphi }_L}{2}\end{array}\right]}{\Delta t}=\frac{1}{2}q\otimes \left[\begin{array}{c}0\\ w_L\end{array}\right]$$

因此有：

$$\dot{q}=\frac{1}{2}q\otimes w_L=\frac{1}{2}\Omega (w_L)q，\Omega (w)\triangleq [w{]}_R=\left[\begin{array}{cc}0& -w^T\\ w& -w^{\wedge }\end{array}\right]$$

如果是左乘形式，即将角速度转换到世界坐标系下，有：

$$\dot{q}=\frac{1}{2}{w}_G\otimes q$$

3、坐标变换性质

3.1 2D坐标变换

变换	矩阵	自由度	保持	图标
平移	$[I|t{]}_{2\times 3}$	2	方向	正方形
欧式	$[R|t{]}_{2\times 3}$	3	长度	旋转的正方形
相似	$[sR|t{]}_{2\times 3}$	4	夹角	旋转缩放的正方形
仿射	$[A{]}_{2\times 3}$	6	平行性	平行四边形
投影	$[\tilde{H}{]}_{3\times 3}$	8	直线性	四边形

其中$R=\left[\begin{array}{cc}cos\theta & sin\theta \\ -sin\theta & cos\theta \end{array}\right]$

3.2 3D坐标变换

变换名称	矩阵形式	自由度	不变性之
平移变换	$\left[\begin{array}{cc}I& t\\ 0^T& 1\end{array}\right]$	3	方向
欧式变换	$\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]$	6	长度、体积
相似变换	$\left[\begin{array}{cc}sR& t\\ 0^T& 1\end{array}\right]$	7	夹角
仿射变换	$\left[\begin{array}{cc}A& t\\ 0^T& 1\end{array}\right]$	12	平行性、体积比
射影变换	$\left[\begin{array}{cc}A& t\\ a^T& v\end{array}\right]$	15	接触平面的相交和相切

从世界坐标系到相机照片的变换是一个射影变换，如果相机焦距无穷远则为仿射变换。