视觉里程计：2D到2D，对极几何法

对于经特征匹配得到的两图像间配对的特征点，可恢复得到两帧图像间相机的运动。

对极约束

名词解释

如上图，对于两帧图像$I_1、I_2$，P在其上投影分别为$p_1、p_2$。相机的中心分别为$O_1、O_2$，则有如下定义：

• 极平面（Epipolar plane） ：由$O_1、O_2、P$组成的平面
• 极点（Epipoles） ：$O_1{O}_2$连线同像平面$I_1、I_2$的交点$e_1、e_2$
• 基线 ：$O_1{O}_2$连线
• 极线（Epipolar line） ：极平面同像平面的交线$p_1{e}_1、p_2{e}_2$，记为$l_1、l_2$

记两帧图像间的变换为$T_{12}$，实际求解中，特征点$p_1、p_2$通过特征匹配得到，$P、e_1、e_2$未知，待求变换$T_{12}$。

像素坐标

对于世界坐标下某点$P={\left[\begin{array}{ccc}X& Y& Z\end{array}\right]}^T$，由针孔相机模型可知其在图像坐标系下的像素坐标位置：
$$s_1{p}_1=KP{s}_2{p}_2=K(RP+t)$$
其中，$K$为相机内参矩阵，$R、t$为两帧图像间的旋转、平移变换。

齐次坐标

通常采用齐次坐标表示像素坐标，也即一个向量同它自身乘以任意非零整数意义相同。通常用于表达一个投影变换。如$s_1{p}_1$同$p_1$成投影关系，他们在齐次坐标下意义相同。称其为尺度意义下相等，记作：
$$sp\simeq p$$
则可得：
$$p_1\simeq KP{p}_2\simeq K(RP+t)$$
取归一化平面上坐标$x_1,x_2$：
$$x_1=K^{-1}{p}_1{x}_2=K^{-1}{p}_2$$
则可得：
$$x_2\simeq R{x}_1+t$$

对极约束

对上式两侧左乘$t^{\wedge }$再左乘$x_2^T$：
$$\begin{gathered} t^{\wedge }{x}_2\simeq t^{\wedge }R{x}_1 \\ {x}_2^T{t}^{\wedge }{x}_2\simeq x_2^T{t}^{\wedge }R{x}_1 \end{gathered}$$
对于上述公式左侧向量$t^{\wedge }{x}_2$方向同$t$以及$x_2$垂直，故而再和$x_2$进行内积计算时，结果为$0$，由此对其进行公式简化：
$$x_2^T{t}^{\wedge }R{x}_1=0$$
带入$p_1、p_2$：
$$p_2^T{K}^{-T}{t}^{\wedge }R{K}^{-1}{p}_1=0$$
称其为对极约束，其中同时包含了旋转和平移两部分。其物理意义为刻画了$O_1、O_2、P$共面的事实。

为简化公式，定义两个矩阵：基础矩阵（Fundamental Matrix）F和本质矩阵（Essential Matrix）E：
$$E=t^{\wedge }RF=K^{-T}E{K}^{-1}$$
则有：
$$x_2^{T}E{x}_1=0p_2^{T}F{p}_1=0$$
由此，相机位姿估计问题可分为如下两步：

• 根据匹配特征的像素位置求E或F
• 根据E或F求R，t

本质矩阵

本质矩阵$E=t^{\wedge }R$是一个$3\times 3$的矩阵，满足如下规则约束：

• 本质矩阵E的尺度等价：E乘以任意非零常数后，对极约束依然满足
• 本质矩阵E的内在性质：E的奇异值必然是${\left[\begin{array}{ccc}\sigma & \sigma & 0\end{array}\right]}^T$形式的
• 由于尺度等价性，E只有5个自由度（$t^{\wedge }R$为6自由度）

八点法求解

本质矩阵E为$3\times 3$的矩阵，具有九个维度。此处仅考虑尺度等价性，则可用8对点估计本质矩阵E。

对于一对匹配的特征点，设其归一化坐标为$x_1=\left[\begin{array}{ccc}{u}_1& v_1& 1\end{array}\right]$和$x_2=\left[\begin{array}{ccc}{u}_2& v_2& 1\end{array}\right]$，则可根据对极约束得到：
$$\left[\begin{array}{ccc}{u}_2& v_2& 1\end{array}\right]\left[\begin{array}{ccc}{e}_1& e_2& e_3\\ e_4& e_5& e_6\\ e_7& e_8& e_9\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=0$$
对本质矩阵E，将其展开写为向量形式：
$$e={\left[\begin{array}{ccccccccc}{e}_1& e_2& e_3& e_4& e_5& e_6& e_7& e_8& e_9\end{array}\right]}^T$$
则对极约束可写为关于$e$的线性形式：
$$\left[\begin{array}{ccccccccc}{u}_2{u}_1& u_2{v}_1& u_2& v_2{u}_1& v_2{v}_1& v_2& u_1& v_1& 1\end{array}\right]\cdot e=0$$
同样，对所有8对匹配特征点做上述计算，则可得到如下线性方程组：
$$\left[\begin{array}{ccccccccc}{u}_2^1{u}_1^1& u_2^1{v}_1^1& u_2^1& v_2^1{u}_1^1& v_2^1{v}_1^1& v_2^1& u_1^1& v_1^1& 1\\ u_2^2{u}_1^2& u_2^2{v}_1^2& u_2^2& v_2^2{u}_1^2& v_2^2{v}_1^2& v_2^2& u_1^2& v_1^2& 1\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ u_2^8{u}_1^8& u_2^8{v}_1^8& u_2^8& v_2^8{u}_1^8& v_2^8{v}_1^8& v_2^8& u_1^8& v_1^8& 1\end{array}\right]\left[\begin{array}{c}{e}_1\\ e_2\\ e_3\\ e_4\\ e_5\\ e_6\\ e_7\\ e_8\\ e_9\end{array}\right]=0$$
其中，$u^i{v}^i$表示第$i$对匹配的特征点。当系数矩阵满秩（Rank=8）时，E的各个元素可求解。此时e构成一条线，同尺度等价性一致。

通过求解上述线性方程组，即可得到本质矩阵E。

SVD分解

针对估计所得的本质矩阵E，计算对应的相机运动$R、t$，应使用SVD分解进行求得。有关SVD分解的基础内容，可以学习如下文章：奇异值分解（SVD）

设本质矩阵E的SVD如下：
$$E=U\Sigma V^T$$
根据SVD定义：矩阵$U、V$为正交阵，$\Sigma$为奇异值矩阵。由本质矩阵的内在性质知：$\Sigma =\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}(\sigma ,\sigma ,0)$，则在SVD分解中，任意本质矩阵E具有两个可能的$t、R$与之对应（此处直接给出了最终解，未推导）：
$$\begin{gathered} t_1^{\wedge }=U{R}_Z(\frac{\pi }{2})\Sigma U^T{R}_1=U{R}_Z^T(\frac{\pi }{2})V^T \\ {t}_2^{\wedge }=U{R}_Z(-\frac{\pi }{2})\Sigma U^T{R}_2=U{R}_Z^T(-\frac{\pi }{2})V^T \end{gathered}$$
其中，$R_Z(\frac{\pi }{2})$表示绕Z轴旋转$\frac{\pi }{2}$得到旋转矩阵。同时由于E和$-$E等价，对任意一个$t$取负，可得相同结果。则存在四组可能的解：

如上图，用蓝色线表示相机，红色点（蓝线同黑线交点）表示空间点在相机上的投影。上述四种解中，仅第一种解中P在两个相机中都具备正向深度，故而可排除其余四种。

内在性质

根据线性方程组求解得到的E可能不满足其内在性质，也即奇异值不一定为${\left[\begin{array}{ccc}\sigma & \sigma & 0\end{array}\right]}^T$形式。

通常，刻意将$\Sigma$矩阵调整为上述形式，也即对八点法求得的E进行SVD分解后，得到的奇异值矩阵$\Sigma =\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}({\sigma }_1,{\sigma }_2,{\sigma }_3)$，假设${\sigma }_1\ge {\sigma }_2\ge {\sigma }_3$，则取：
$$E=U\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}(\frac{{\sigma }_1+{\sigma }_2}{2},\frac{{\sigma }_1+{\sigma }_2}{2},0)V^T$$
也即将求得的矩阵投影至E所在的流形上。

八点法存在的问题

尺度不确定性

由于E本身具备尺度等价性，分解计算得到的$R、t$也具备尺度等价性。此时由于$R\in SO(3)$存在约束，故而认为$t$具备一个尺度。由此，对t进行归一化处理，使其长度为1。由于归一化，将直接导致单目视觉尺度不确定。

初始化纯旋转

若相机在初始化时发生的运动为纯旋转，也即由E分解得到的$R、t$中$t=0$时，将导致无法直接求解$R$

多于8对点时

当给定的匹配特征点多于8对时，可使用最小二乘计算对极约束，记系数矩阵为$A$则有：
$$Ae=0$$
对于八点法，$A$大小为$8\times 9$。多于8对点时可以构造超定方程：
$$\underset{e}{\min }{\Vert \begin{array}{c}Ae\end{array}\Vert }_2^2=\underset{e}{\min }{e}^T{A}^{T}Ae$$
从而，可以求解最小二乘意义下的E矩阵。当存在误匹配问题时，则可采用**随机采样一致性（RANSAC）**代替最小二乘进行计算。

单应矩阵

单应矩阵（Homography）H用于描述两平面间的映射关系。若场景内特征点都落至某一平面上（墙、地面），则可使用单应性估计运动。

对于像平面$I_1、I_2$上匹配的两特征点$p_1、p_2$，若特征点落在某平面P上：
$$\begin{gathered} n^{T}P+d=0 \\ -\frac{n^{T}P}{d}=1 \end{gathered}$$
其中$n$为平面的法向量，d为截距。同样在相机平面中，存在投影关系：
$$\begin{array}{rl}{p}_2\simeq & K(RP+t)\\ \simeq & K(RP+t\cdot 1)\\ \simeq & K(RP+t\cdot (-\frac{n^{T}P}{d}))\\ \simeq & K(R-\frac{t{n}^T}{d})P\\ \simeq & K(R-\frac{t{n}^T}{d})K^{-1}{P}_1\end{array}$$
由此得到对应匹配特征点间的变换关系：
$$H=K(R-\frac{t{n}^T}{d})K^{-1}$$
称矩阵H为单应矩阵，为一个$3\times 3$的矩阵。

将$p_1、p_2$的归一化像素坐标带入：
$$\begin{gathered} p_2\simeq H{p}_1 \\ \left[\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right]\simeq \left[\begin{array}{ccc}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right]\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right] \end{gathered}$$
符号$\simeq$表示了尺度意义下的相等，故而矩阵H乘以任意非零常数依旧等价于其本身。实际处理中，取$h_9=1$从而展开：
$$\begin{gathered} h_1{u}_1+h_2{v}_1+h_3-h_7{u}_1{u}_2-h_8{v}_1{u}_2=u_2 \\ {h}_4{u}_1+h_5{v}_1+h_6-h_7{u}_1{v}_2-h_8{v}_1{v}_2=v_2 \end{gathered}$$
也即，一堆匹配特征点可以提供两个约束条件，则自由度为8的单应矩阵H只需要4对点即可求解得到：
$$\left[\begin{array}{cccccccc}{u}_1^1& v_1^1& 1& 0& 0& 0& -u_1^1{u}_2^1& -v_1^1{u}_2^1\\ 0& 0& 0& u_1^1& v_1^1& 1& -u_1^1{v}_2^1& -v_1^1{v}_2^1\\ u_1^2& v_1^2& 1& 0& 0& 0& -u_1^2{u}_2^2& -v_1^2{u}_2^2\\ 0& 0& 0& u_1^2& v_1^2& 1& -u_1^2{v}_2^2& -v_1^2{v}_2^2\\ u_1^3& v_1^3& 1& 0& 0& 0& -u_1^3{u}_2^3& -v_1^3{u}_2^3\\ 0& 0& 0& u_1^3& v_1^3& 1& -u_1^3{v}_2^3& -v_1^3{v}_2^3\\ u_1^4& v_1^4& 1& 0& 0& 0& -u_1^4{u}_2^4& -v_1^4{u}_2^4\\ 0& 0& 0& u_1^4& v_1^4& 1& -u_1^4{v}_2^4& -v_1^4{v}_2^4\end{array}\right]\left[\begin{array}{c}{h}_1\\ h_2\\ h_3\\ h_4\\ h_5\\ h_6\\ h_7\\ h_8\end{array}\right]=\left[\begin{array}{c}{u}_2^1\\ v_2^1\\ u_2^2\\ v_2^2\\ u_2^3\\ v_2^3\\ u_2^4\\ v_2^4\end{array}\right]$$
计算得到单应矩阵H后，应同本质矩阵一样，进一步分解计算$R、t$

小结

在2D-2D的情况下，仅知道图像坐标间对应关系：

• 当特征点在平面上（俯视、仰视），使用H计算得到$R、t$
• 否则，使用E\F计算$R、t$
• 获得$R、t$后，采用三角化计算其深度信息。