视觉SLAM——两视图对极几何 本质矩阵 基础矩阵 单应矩阵 三角测量

前言

本博客主要为学习《视觉SLAM十四讲》第7讲第3、4节、《多视图几何》第二篇双视图几何、《计算机视觉-算法与应用》第7章由运动到结构 等其他相关SLAM内容的总结与整理。这块内容比较重要所以写的比较详细。

本博客主要讨论一下内容：
1、对极几何的构建，F、E矩阵的推导、性质与求解；
2、由E恢复Rt，三角测量；
3、单应性，单应矩阵H的求解；
4、F、E、H矩阵的适用性讨论。

在特征匹配后，得到了特征点之间的关系，如果是两张图像匹配，可通过对极几何得到2D-2D之间的关系。

一、对极几何

对极几何是两幅视图之间内在的射影几何，它独立于景物结构，只依赖于摄像机的内部参数和相对姿态。对极几何约束了一幅图上的点在另一幅图上的对应位置应在对极线上。

1、问题描述

假设两帧的图像平面为I₁、I₂，其相机中心分别为O₁、O₂，这两帧之间的变换为T₁₂=[R|t]，现在3维空间中有一个点P，它在这两幅图上的投影分别为特征点p₁和p₂。
显然点P、p₁、p₂、O₁、O₂是共面的，这便构成了对极几何。

几个概念：

• 基线：连接两个相机中心的连线。图中直线O₁O₂。
• 对极平面：一张包含基线的平面。图中如平面PO₁O₂。
• 对极点：基线与像平面的交点，等价的，对极点是在一幅视图中另一个相机中心的像，也是基线方向的消影点。图中e₁和e₂。
• 对极线：对极平面与两个图像平面的交线。一个图像中所有对极线相交于对极点。图中p₁e₁和p₂e₂。

在SLAM过程中，通过特征匹配得到了特征点坐标p₁、p₂，希望求得相机位姿T₁₂和特征点三维坐标P。

2、本质矩阵和基础矩阵的推导

这里推导和视觉SLAM十四讲有点区别：并没有使用乘以非零常数下成立的（up to a scale）等式，看起来更容易理解；

推导1：

在世界坐标系下的点P，根据针孔模型，得到两个像素点p₁、p₂的位置为：

$$s_1{p}_1=KP\Rightarrow s_1{K}^{-1}{p}_1=P$$

$$s_2{p}_2=K(RP+t)\Rightarrow s_2{K}^{-1}{p}_2=RP+t=s_{1}R{K}^{-1}{p}_1+t$$

取归一化图像坐标 $x_1=K^{-1}{p}_1$，$x_2=K^{-1}{p}_2$：

$$s_2{x}_2=RP+t=s_{1}R{x}_1+t$$

左乘t^（即与t做外积）： $$s_2{t}^{\wedge }{x}_2=s_1{t}^{\wedge }R{x}_1+t^{\wedge }t$$

再左乘$x_2^T$：$$s_2{x}_2^T{t}^{\wedge }{x}_2=s_1{x}_2^T{t}^{\wedge }R{x}_1$$

因为左侧为零，s₁s₂为标量，得到：$$x_2^T{t}^{\wedge }R{x}_1=0$$

重新带入p₁、p₂：$$p_2^T{K}^{-T}{t}^{\wedge }R{K}^{-1}{p}_1=0$$

以上两式便称为对极约束。

基础矩阵F（Fudamental）：$$F=K^{-T}{t}^{\wedge }R{K}^{-1}$$

本质矩阵E（Essential）：$$E=t^{\wedge }R$$

对极约束：$$x_2^{T}E{x}_1=p_2^{T}F{p}_1=0$$

推导2

相机中心O₁、O₂与点P共面，那么连接两个相机中心的向量$-R^{-1}t$和与像素点p₁、p₂对应的射线$x_1$、$R^{-1}{x}_2$($x_1、x_2为$归一化图像坐标)，肯定是共面的。
那么就要求混合积为0：
$$(x_1,-R^{-1}{x}_2,R^{-1}t)=(R{x}_1,x_2,-t)=x_1^T(t^{\wedge }R)x_2=0$$

3、本质矩阵的性质

• 对E乘以任意非零常数后，对极约束仍然满足，即E在不同尺度下是等价的；
• 本质矩阵E的充要条件为：3X3矩阵的奇异值为[σ,σ,0]^T的形式；
• 本质矩阵共有5个自由度：平移和旋转各3个，尺度等价性减去一个。

4、基础矩阵的性质

• F是一个秩为2，自由度为7的齐次矩阵：对于3X3的齐次矩阵拥有8个自由度，此外约束$detF=0$再减去一个自由度，奇异值为$[{\sigma }_1,{\sigma }_2,0{]}^T$
• 转置：如果F是对(O₁,O₂)的基本矩阵，则F^T是对反序列(O₂,O₁)的基本矩阵。
• 对极线：与第一幅图上任意一点p₁对应的对极线是 $l_2=F{p}_1$，反之同理$l_1=F^T{p}_2$。
• 对极点：任何不同于e₁的点p₁的对极线 $l_2=F{p}_1$包含对极点e₂，因此对于所有p₁，e₂都满足 $e_2^T(F{p}_1)=(e_2^{T}F)p_1=0$，从而推出$e_2^{T}F=0$，$F{e}_1=0$

5、特殊运动的基础矩阵

• 纯平移：假设相机的运动为没有旋转且内参不变的纯平移，即R=I，此时F是反对称矩阵且只有2个自由度。
  $$F=K^{-T}{t}^{\wedge }R{K}^{-1}=t^{\wedge }KR{K}^{-1}=t^{\wedge }$$

• 纯平面：旋转轴与平移方向正交，给运动增加了一个约束，此时F自由度为6.

6、基础矩阵的求解

一般使用8点法进行本质矩阵或基础矩阵的求解。这里以基础矩阵为例：

假设一对匹配点的齐次坐标为$p_1=[u_1,v_1,1]$，$p_2=[u_2,v_2,1]$，根据对极约束有：

$$\left(\begin{array}{ccc}{u}_1& v_1& 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}_2\\ v_2\\ 1\end{array}\right)=0$$

将F矩阵展开到9维向量形式：$$e=[e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9{]}^T$$

此时对极约束为：$$[u_1{u}_2,u_1{v}_2,u_1,v_1{u}_2,v_1{v}_2,v_1,u_2,v_2,1]e=0$$

将8对匹配点构成一个线性方程组，其系数矩阵大小为8X9，如果系数矩阵满秩（8），那么它的零空间维数为1，即e构成一条线，以此得到E的各元素。

需要注意的是由于测量和计算存在误差，对基本矩阵直接线性变换无法保证基础矩阵的奇异值约束：$[{\sigma }_1,{\sigma }_2,0{]}^T$，需根据奇异值约束对矩阵进行重构：
对八点法得到的基础矩阵进行奇异值分解：$\hat{F}=Udiag({\sigma }_1,{\sigma }_2,{\sigma }_3)V^T$ 其中${\sigma }_1\ge {\sigma }_2\ge {\sigma }_3$，
取：$F=Udiag({\sigma }_1,{\sigma }_2,0)V^T$

同理，从基础矩阵求得的本质矩阵$\hat{E}=K^{T}FK$（或者直接由归一化坐标八点法得到），也需要满足E的奇异值为[σ,σ,0]^T的形式。
对本质矩阵进行奇异值分解：$\hat{E}=Udiag({\sigma }_1,{\sigma }_2,{\sigma }_3)V^T$，其中${\sigma }_1\ge {\sigma }_2\ge {\sigma }_3$，
取$E=Udiag(\frac{{\sigma }_1+{\sigma }_2}{2},\frac{{\sigma }_1+{\sigma }_2}{2},0)V^T$。

对八点法的一些讨论：

• 2D-2D问题常常用于单目SLAM的初始化
• 由于存在尺度不确定性：通常采用归一化t或特征点的平均深度
• 纯旋转问题：当t=0时无法求解
• 当多于八对点时：可以最小二乘法或随机抽样一致算法（RANSAC）
  

7、恢复相机矩阵

即如何从已经估计出的本质矩阵E，恢复出相机的外参R和t。注意由于尺度等价性，这里的t只是估计其方向。

由于E的SVD分解为：$$E=U\sum V^T=Udiag(\sigma ,\sigma ,0)V^T$$

可得对于任意一个E，存在2个可能的t,R与其对应：

$t^{\wedge }=U{R}_z(\frac{\pi }{2})\sum U^T$，$R_1=U{R}_z^T(\frac{\pi }{2})V^T$

$t^{\wedge }=U{R}_z(-\frac{\pi }{2})\sum U^T$，$R_2=U{R}_z^T(-\frac{\pi }{2})V^T$

其中$R_z(-\frac{\pi }{2})$表示沿Z轴旋转90°的得到的旋转矩阵。因此一共存在4个可能的解。

只有一种解中P在两个相机中都具有正的深度，因此只需将任意一点带入4种解中，检测该点在两个相机下的深度，就可以确定哪个是正确的。
（手性检验：所有的观测点必须在摄像机前面，即从摄像机发出的视线上距离为正。）

二、三角测量

双视图几何的一个重要结果是：仅由图像对应点，不需要其他的信息，就可以计算摄像机和景物结构的重构。

求解方法

1、十四讲中的双视图方法

在之前已经推出归一化图像坐标之间的几何关系：
$$s_1{x}_1=P，s_2{x}_2=RP+t=s_{1}R{x}_1+t$$

两侧左乘x₂^：$s_2{x}_2^{\wedge }{x}_2=0=s_1{x}_2{t}^{\wedge }R{x}_1+x_2^{\wedge }t$
即可得到s₂，通过s₂可得到s₁。

由于存在误差，一般采用最小二乘法求解：$s_2{x}_2-s_{1}R{x}_1=t$

对于$Ax=b$的最小二乘法解为：$x=(A^{T}A{)}^{-1}{A}^{T}b$

2、SVD分解

同样，对于齐次坐标$P=[X,Y,Z,1]$，归一化图像坐标$x_i=[u_i,v_i,1]$

存在关系：$s_i{x}_i=T_{i}P$

其中从世界坐标系到相机坐标系的变换矩阵采用3x4的矩阵表示：$T_i=[R|t]=[T_i^1,T_i^2,T_i^3{]}^T$

那么有：$$s_i{x}_i\times T_{i}P=x_i^{\wedge }{T}_{i}P=0$$

展开有：
$$x_i^{\wedge }{T}_{i}P=\left[\begin{array}{ccc}0& -1& v\\ 1& 0& -u\\ -v& u& 0\end{array}\right]\left[\begin{array}{c}{T}_i^1\\ T_i^2\\ T_i^3\end{array}\right]P=\left[\begin{array}{c}-T_i^2+v_i{T}_i^3\\ T_i^1-u_i{T}_i^3\\ -v_i{T}_i^1+u_i{T}_i^2\end{array}\right]P$$

其中第三行与前两行线性相关，只需要保留前两行即可。
因此，已知一个归一化图像坐标x和变换矩阵T，可以构建两个关于P的线性方程组。有两个以上的图像观测，即可求出P：
$$\left[\begin{array}{c}-T_0^2+v_i{T}_0^3\\ T_0^1-u_i{T}_0^3\\ -T_1^2+v_i{T}_1^3\\ T_1^1-u_i{T}_1^3\\ ...\end{array}\right]P=0$$

上述方程没有非零解，使用SVD求最小二乘解，得到其最小奇异值对应的单位奇异向量(x,y,z,w)。
$$P=\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]=\left[\begin{array}{c}x/w\\ y/w\\ z/w\\ w/w\end{array}\right]$$

3、基于RANSAC的计算过程

如果存在外点（匹配点或姿态求解时）的情况，如何得到准确的三维点坐标？
采用RANSAC算法：
1）计算采样次数
2）随机采样1对视角，计算三维点坐标
3）统计内点个数
4）重复2）3）直到满足采样次数，选择内点数最多的三维点坐标
5）利用所有内点重新计算三维点坐标

4、存在的问题

1、三角化的矛盾：解得深度的质量与平移相关，增大平移会使三角化更精确，但是平移大时特征匹配可能不成功

2、系数矩阵伪逆不可靠（行列式近零）
例如：相机前进时，虽然有位移，但位于图像中心的点无法三角化（没有视差）。

三、单应性

单应(Homography)是射影几何中的概念，又称为射影变换。它把一个射影平面上的点(三维齐次矢量)映射到另一个射影平面上，并且把直线映射为直线，具有保线性质。总的来说，单应是关于三维齐次矢量的一种线性变换，可以用一个3×3的非奇异矩阵H表示：$x_2=H{x}_1$。H乘以一个非零的比例因子上述等式仍然成立，即H是一个3×3齐次矩阵，具有8个未知量。
单应性可用于图像的校正、对齐以及在SLAM中估计两个相机间的运动。

1、单应矩阵

单应矩阵H可用于描述处于共同平面的点在两张图像之间的变换关系。考虑在图像I₁和I₂有一对匹配好的特征点p₁和p₂。这些特征点落在平面P上，设平面满足方程：
$$n^{T}P+d=0\Rightarrow -\frac{n^{T}P}{d}=1$$

$$p_1=KP\Rightarrow K^{-1}{p}_1=P$$

$$p_2=K(RP+t)=K(RP-\frac{t{n}^{T}P}{d})=K(R-\frac{t{n}^T}{d})P=K(R-\frac{t{n}^T}{d})K^{-1}{p}_1$$

令$H=K(R-\frac{t{n}^T}{d})K^{-1}$，得到$p_2=H{p}_1$。

2、单应矩阵的求解

$$\left(\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right)=\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)$$

在实际处理时通常乘以一个非零因子使得$h_9=1$，然后根据第三行去掉这个非零因子：
$$u_2=\frac{h_1{u}_1+h_2{v}_1+h_3}{h_7{u}_1+h_8{v}_1+h_9}，v_2=\frac{h_4{u}_1+h_5{v}_1+h_6}{h_7{u}_1+h_8{v}_1+h_9}$$

这样一组匹配点可以构造2个约束，自由度为8的单应矩阵至少需要通过4对匹配特征点求出（不能有三点共线）。匹配点大于4对时，可以通过最小二乘法和RANSAC方法进行估计。

四、基础矩阵与单应矩阵的一些讨论

1、相机做纯旋转时

假设相机只进行纯旋转运动：$t=[0,0,0{]}^T$
有：$p_1=KP$，$p_2=KRP$
得到：$p_2=KR{K}^{-1}{p}_1$
又有两个像素的单应性：$p_2=H{p}_1$，因此$H=KR{K}^{-1}$。

从这里可以看出：
1）当相机进行纯旋转时，$H=K(R-\frac{t{n}^T}{d})K^{-1}=KR{K}^{-1}$，与深度d没有关系，此时无论三维点是否在一个平面上，H矩阵都能完美的符合转换关系。
2）当$t=[0,0,0{]}^T$时，按之前方法得到的本质矩阵和基础矩阵F为零矩阵（这是因为在推导的时候与t做了外积使等号两边都变成0）。因此此时两视图的对极约束就不再适用，这称之为退化现象。
3）此时无法使用三角法重构点在世界坐标系中的三维坐标

2、单应矩阵在非平面情形

当相机的平移向量t相对于场景深度而言足够小时，x′2和x2之间的误差是可被接受的，即这种情况下依然可以使用H矩阵来算坐标的一一对应，比如orbslam中提到的低视差情形。

3、为什么基础矩阵不能用于平面情形

在平面情形中，我们得到了p₁和p₂的对应关系：$p_2=H{p}_1$
我们知道，任意一个向量u与p₂进行外积一定垂直于p₂：
$$u^{\wedge }{p}_2\perp H{p}_1\Rightarrow (u^{\wedge }{p}_2{)}^{T}H{p}_1=0\Rightarrow p_2^T{u}^{\wedge }H{p}_1=0$$

所以我们可以得到$E=u^{\wedge }H$，这说明在平面情况下，存在无穷多解满足$p_2^{T}E{p}_1=0$。显然不正确。（换言之就是平面情形提供了匹配点更多的约束导致本质矩阵退化）

总结

• 基础矩阵表示的是两视图的对极约束，独立于景物结构，只依赖于摄像机的内部参数和相对姿态；本质矩阵是基础矩阵在归一化图像坐标下的表示；单应矩阵表示点和点之间的映射，但要求场景点在同一个平面上或者是相机纯旋转。

• 基础矩阵无法得到这个点对应点在另一幅图像上的确切位置（映射的是对极线）；单应矩阵可以找到点在另一幅图像上对应点的确切位置。

• 当相机进行纯旋转时，或者场景中的点在同一平面上或近平面上，必须使用单应矩阵H而不能使用基础矩阵F；当相机的平移距离相对于场景的深度较小的时候可以使用单应矩阵H。因此通常会同时估计基础矩阵F和单应矩阵H，选择重投影误差较小的作为运动估计矩阵（ORB-SLAM的初始化方法）。