视觉SLAM笔记(一)-相机模型与三维刚体运动

相机是视觉SLAM中所利用到的主要传感器之一，本文主要介绍了单目相机模型的成像原理以及三维刚体运动在视觉SLAM中的表达方式。

单目相机模型

单目相机模型的映射关系主要由针孔模型描述，光线投影到成像平面上的畸变过程由畸变模型来描述。空间中的三维点通过这两个模型投影到相机的成像平面上，再转换为像素坐标系，构成了一个完整的映射关系。

针孔模型

针孔模型所描述的是一束光线通过针孔后在针孔背后投影成像的关系，其原理类似于物理学中的小孔成像，这里先定义几个坐标系/平面:

1. 相机坐标系: 这个坐标系即在相机视角下看世界坐标系，设其中的空间点$P$的坐标为$[X,Y,Z]$
2. 成像平面: 是指空间坐标经过针孔投影到的相机内部的平面上，这个平面通常在相机内部，与空间坐标点位于相机光心(针孔)的两侧，简化模型时，可将其对称到相机前方，与空间点位于一侧，成像平面到光心的距离即为焦距$f$，成像平面上的坐标点$P^{\prime }$的坐标为$[X^{\prime },Y^{\prime },Z^{\prime }]$
3. 像素坐标系: 可认为与相机成像平面重合，但是是将成像平面内的坐标点采样到一个个的像素点上，像素坐标通常由$[u,v]$表示，其原点位于图像左上角，且与成像平面间相差了一个缩放和一个平移

由针孔模型的相似关系，成像平面上的点与相机坐标系下点的变换关系如下:
$$\frac{Z}{f}=\frac{X}{X^{\prime }}=\frac{Y}{Y^{\prime }}$$
整理得
$$X^{\prime }=f\frac{X}{Z}{Y}^{\prime }=f\frac{Y}{Z}$$
再假设像素坐标系与成像平面之间的关系为: 在$u$轴上缩放了$\alpha$倍，在$v$轴上缩放了$\beta$倍，同时原点平移了$[c_x,c_y{]}^T$(成像平面的原点位于平面中心)，得到关系如下:
$$u=\alpha X^{\prime }+c_{x}v=\beta Y^{\prime }+c_y$$
令$\alpha f=f_x$，$\beta f=f_y$，得
$$u=f_x\frac{X}{Z}+c_{x}v=f_y\frac{Y}{Z}+c_y$$
用矩阵表示
$$Z\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=KP$$
这里的$K$即为所谓的相机内参矩阵，$P$为相机坐标系下的空间坐标点的坐标，其与世界坐标系下的坐标还存在一部空间变换，这将在下面的三维刚体运动中讲到。这里像素坐标使用了齐次坐标表示，即将原来的二维坐标拓展成了三维坐标(第三维为1)以方便矩阵运算。

畸变模型

畸变模型主要分为径向畸变和切向畸变。径向畸变是由透镜形状对光线传播的影响所引起的，透镜使得实际中的直线在图片中变成了曲线，且通常径向对称，越靠近图像边缘越明显；切向畸变是由于相机组装过程中透镜与成像平面之间的平行度误差所引起的。

径向畸变

$$\begin{gathered} x=x_d(1+k_1{r}^2+k_2{r}^4+k_3{r}^6) \\ y=y_d(1+k_1{r}^2+k_2{r}^4+k_3{r}^6) \end{gathered}$$
其中$x_d$表示畸变点坐标，$r=\sqrt{x_d^2+y_d^2}$，一般$k_1$在中心区域起作用，$k_2$在边缘区域起作用，对一般相机而言$k_1$和$k_2$足够纠正畸变，畸变较大的镜头(鱼眼等)可引入$k_3$

切向畸变

$$\begin{gathered} x=x_d+2p_1{x}_d{y}_d+p_2(r^2+2x_d^2) \\ y=y_d+2p_2{x}_d{y}_d+p_1(r^2+2y_d^2) \end{gathered}$$

三维刚体运动

三维刚体运动即为rigid body motion，其主要特点在于运动物体本身的特性不发生变化，变化的仅是其空间位置与朝向(即其位姿)，rigid body motion作用下的向量的不变量为其模以及两个向量的叉乘结果($g_t$表示rigid body motion，$u,v$为向量)
$$\begin{gathered} |g_t(v)|=|v|,\forall v\in R^3 \\ {g}_t(u)\times g_t(v)=g_t(u\times v),\forall u,v\in R^3 \end{gathered}$$
刚体运动通常可用旋转和平移来表示，通过旋转矩阵$R$和平移向量$t$来描述，$g_t(X)=RX+t$由于需要满足刚体运动的两个不变量，$R$为一个行列式为1的正交矩阵，其集合为SO(3) (Special Orthogonal Group):
S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } SO(n) = \lbrace R \in R^{n\times n} | RR^T=I,det(R)=1 \rbrace SO(n)={R∈Rn×n∣RRT=I,det(R)=1}
将旋转和平移合并成一个矩阵形式如下:
$$T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]$$
则$X^{\prime }=TX$，注意这里的$X$及$X^{\prime }$均为四维的齐次坐标，即在原来三维坐标的末尾添加一个1
此处的变换矩阵$T$也构成了一个特殊的集合SE(3)(Special Euclidean Group):
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3) = \lbrace T = \begin{bmatrix} R & t\\ 0^T & 1 \end{bmatrix} \in R^{4 \times 4} | R \in SO(3), t \in R^3 \rbrace SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}
$SO(3)$和$SE(3)$在之后将要介绍的李群和李代数中也扮演了重要的角色。

综上所述，将一个世界坐标系中的点转换为像素坐标系上的点的步骤如下:

1. 将世界坐标系下的点$P_w$转换为相机坐标系下点$P$，$P=T{P}_w$，$T$为刚体变换矩阵
2. 将相机坐标系下点$P$转换为像素坐标系下点$P_{uv}$，$Z{P}_{uv}=KT{P}_w$，K为相机内参矩阵

参考文献

[1] 高翔, 张涛, 颜沁睿, 刘毅, 视觉SLAM十四讲：从理论到实践, 电子工业出版社, 2017
[2] https://vision.in.tum.de/teaching/ss2019/mvg2019