VO是什么？

视觉里程计的主要任务是根据图像对相机的运动（包括旋转和平移）进行估计。它根据图像序列序列，估计相机的运动轨迹和场景的空间结构，而SLAM需要再此基础上通过后端进一步优化，通过闭环检测改善飘移的情况，从而构建地图。

相机成像模型

（1）世界坐标系（World Coordinate System）。用户定义的三维世界的坐标系，以某个点为原点，为了描述物体在真实世界中的位置而被引入，单位为m。$(X_w,Y_w,Z_w)$

（2）相机坐标系（Camera Coordinate System）。以相机为原点建立的坐标系，为了从相机的角度描述物体的位置而被定义，作为沟通世界坐标系和图像坐标系的一环。单位为m。$(X_c,Y_c,Z_c)$

（3）图像坐标系（Image Coordinate System）。为了描述成像过程中物体从相机坐标系到图像坐标系的投影透视关系而被引入，方便进一步得到像素坐标系下的坐标。单位为m。$(x,y)$

（4）像素坐标系（Pixel Coordinate System）。为了描述物体成像后的像点在数字图像上的坐标而被引入，是我们真正从相机内读取到的图像信息所在的坐标系。单位为像素。$(u,v)$

相机坐标系的Z轴和光轴重合，且垂直于图像坐标系平面并通过图像坐标系的原点；像素坐标系和图像坐标系平面重合，但图像坐标系的原点位于图像的正中心，而像素坐标系的原点位于图像的左上角，这是因为虽然成像时是以图像中点为原点的，但是图像数据是从左上角以二维数组的形式存储的，如一张宽度为640像素，高度为480像素分辨率的灰度图，即为$480\times 640$，第一个表示行（高），第二个表示列（宽）。

坐标系之间的转化

1、从世界坐标系到相机坐标系

世界坐标系用来描述相机相对于世界坐标系原点的位置。

假设世界坐标系下的一个三维点 $P_w=(X_w,Y_w,Z_w)$，通过旋转矩阵$R$和平移向量$t$组成的变化矩阵 $T_{cw}$ 变换到相机坐标系下，得到相机坐标系下的三维点 $P_c=(X_c,Y_c,Z_c)$。

$T_{cw}$表示从世界坐标系到相机坐标系的变化，它的定义是
$$T_{cw}=\left[\begin{array}{cc}R& t\\ O& 1\end{array}\right]$$
使用齐次坐标系表示
$$\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ O& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]$$

2、从相机坐标系到图像坐标系

针孔相机成像模型（小孔成像），从相机坐标系下的三维点 $P_c=(X_c,Y_c,Z_c)$ 在相机成像平面上成的像为 $(x,y)$，具体推导步骤：

记相机的焦距为 $f$，根据相似三角形，有
$$\frac{f}{Z_c}=\frac{x}{X_c}=\frac{y}{Y_c}$$
整理后得
$$\begin{gathered} x=f\frac{X_c}{Z_c} \\ y=f\frac{Y_c}{Z_c} \end{gathered}$$

3、从图像坐标系到像素坐标系

记 $c_x,c_y$ 分别代表两个坐标平移，一般是图像长度和宽度的一般，$u,v$ 都是像素坐标系下的坐标，则有
$$\{\begin{array}{c}u=\alpha x+c_x\\ v=\beta y+c_y\end{array}$$
其中，$\alpha 、\beta$ 指图像坐标系到像素坐标系的缩放比例，单位为 $m/像素$，$f$ 的单位是 $m$。将2整理后的公式代入上式，有
$$\{\begin{array}{c}u=\alpha f\frac{X_c}{Z_c}+c_x\\ v=\beta f\frac{Y_c}{Z_c}+c_y\end{array}$$
记 $f_x=\alpha f$, $f_y=\beta f$，上式改写为
$$\{\begin{array}{c}u=f_x\frac{X_c}{Z_c}+c_x\\ v=f_y\frac{Y_c}{Z_c}+c_y\end{array}$$
用矩阵表示
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{Z_c}\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}_c\\ Y_c\\ Z_c\end{array}\right]=\frac{1}{Z_c}K{P}_c$$
其中，最左侧的像素坐标 $P_{uv}=[u,v,1{]}^T$ 是像素齐次坐标， 三维点 $P_c=[X_c,Y_c,Z_c{]}^T$ 是非齐次坐标。

矩阵 $K$ 是内参矩阵（相机出厂后固定，不会在使用过程中发生变化），$P_c$ 是相机坐标系下的点。

最右侧有一个系数 $\frac{1}{Z_c}$, $Z_c$ 是相机坐标系下三维点 $P_c$ 在 $z$ 轴上的坐标，将 $\frac{1}{Z_c}$ 与 $P_c=(X_c,Y_c,Z_c)$ 相乘，得到相机坐标系下 $P_c$ 的归一化坐标 $\tilde{P_{c}}=(X_{c}/Z_{c},Y_{c}/Z_{c},1) $，位于相机前方 $z=1$ 的平面上，可写为
$$P_{uv}=K\widetilde{P_c}$$
结合前面世界坐标到相机坐标系的变换，得到从世界坐标系到像素坐标系的变换，如下
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{Z_c}{\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]}_{3\times 3}{\left[\begin{array}{ll}\mathbf{I}& \mathbf{O}\end{array}\right]}_{3\times 4}{\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ \mathbf{O}& 1\end{array}\right]}_{4\times 4}{\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]}_{4\times 1}$$
其中，$I$ 表示 $3\times 3$ 的单位矩阵。

一个三维点投影到图像平面上的二维像素坐标是一个从三维到二维的降维过程，这是不可逆的。

相机畸变模型

为了获得更好的效果，会在相机的前方加上透镜，透镜会对成像过程中的光线传播造成影响：

• 透镜自身的形状对光线传播的影响
• 在机械组装过程中，透镜和成像平面不可能完全平行，这也会使光线穿过透镜投影到成像平面时的位置发生变化

径向畸变

产生原因与透镜形状相关。

图像畸变程度在以图像中心点 $O$ 为圆心，距离 $OB$ 为半径的圆上都是相同的。（在入射光线和透镜光轴夹角保持相同的情况下，不管光线是从哪个方向入射，折射率都是一样的，即在图像平面上的成像点距离图像中心点的半径 $OB$ 也是固定的）

桶形畸变呈现中间凸起的趋势，凸起程度随着与图像几何中心距离的增大而减小；

枕形畸变呈现中间凹下的趋势。

切向畸变

产生原因是由透镜和成像传感器的安装位置引起的。

畸变模型

假设径向畸变或切向畸变可以用多项式来描述，假设归一化平面上的任意一点 $p$，其坐标为 $[x,y{]}^{\top }$，用极坐标表示为 $[r,\theta {]}^{\top }$，$r$ 表示半径，$\theta$ 表示与水平坐标系的夹角。

• 径向畸变可以看成坐标点沿着长度方向发生了变化，也就是其距离原点的长度发生了变化；
• 切向畸变可以看成坐标点沿着切线方向发生了变化，也就是水平夹角发生了变化。

通常假设这些畸变呈多项式关系，即

径向畸变模型可以描述为
$$\begin{gathered} x_{distorted}=x(1+k_1{r}^2+k_2{r}^4+k_3{r}^6) \\ {y}_{distorted}=y(1+k_1{r}^2+k_2{r}^4+k_3{r}^6) \end{gathered}$$
切向畸变模型可以描述为
$$\begin{gathered} x_{distorted}=x+2p_{1}xy+p_2(r^2+2x^2) \\ {y}_{distorted}=y+p_1(r^2+2x^2)+2p_{2}xy \end{gathered}$$
其中，$[x_{distorted},y_{distorted}{]}^{\top }$ 是畸变后点的归一化坐标。$k_1,k_2,k_3,p_1,p_2$ 是畸变模型中的参数。

对于相机坐标系下的一点 $P$，通过5个畸变系数找到这个点在像素平面上的正确位置：

• 将三维空间点投影到归一化图像平面。设它的归一化坐标为 $[x,y{]}^{\top }$。
• 对归一化平面上的点计算径向畸变和切向畸变

$$\begin{array}{rl}& x_{\text{distorted }}=x\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)+2p_{1}xy+p_2\left(r^2+2x^2\right)\\ & y_{\text{distorted }}=y\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)+p_1\left(r^2+2y^2\right)+2p_{2}xy\end{array}$$

• 将畸变后的点通过内参矩阵投影投影到像素平面，得到该点在图像上的正确位置。

$$\begin{gathered} u=f_x{x}_{distorted}+c_x \\ v=f_x{y}_{distorted}+c_y \end{gathered}$$