高博14讲--第七讲 视觉里程计-7.3 2D-2D:对极几何

基本问题

    当相机为单目时，只知道2D的像素坐标，根据两组2D点估计相机的运动，用对极几何来解决。

对极约束

  两个相机之间的变换为$T_{12}$：$p_1=T_{12}\cdot p_2$，即：通过$P_2$的坐标乘以$T_{12}$，即可求得$P_1$的坐标 。
 
  在实践中：

• 已知：匹配点对$p_1$，$p_2$的像素坐标。
• 给定：两张二维图像，二维图像上特征点的匹配关系。
• 未知：P的三维空间坐标，$I_1$到$I_2$​的变换矩阵($T_{12}$, 即$R,T$)。
  
  
• 一般是用于单目SLAM的初始化，用对极几何可求出位姿，在用三角测量估计三维空间点的位置后，就能用其他更准确的方法继续求解了。

对极约束推导过程

  设P在图1的相机坐标系下，坐标为：$$P=[X,Y,Z{]}^T$$
  $p_1$，$p_2$的像素坐标（单位像素）：$$s_1{p}_1=KP,s_2{p}_2=K(RP+t)$$
    $K$为相机的内参矩阵，$R,t$为两个坐标系的相机运动。
  使用齐次坐标，上式乘以非零常数都成立：$$p_1=KP,p_2=K(RP+t)$$
  其归一化平面坐标（单位米）：$$x_1=K^{-1}{p}_1,x_2=K^{-1}{p}_2$$
  得到：$$x_2=R{x}_1+t$$
  这里的$x_1,x_2$是齐次坐标，等式表达了一个齐次关系。

  两边同时左乘$t$^，即：两侧同时与$t$做外积，即，与$t$作叉乘：$$t\times x_2=t\times R{x}_1+t\times t$$
  $t$与$t$作叉乘，夹角为0，则结果为0，即：$t\times t=0$，上式化简为：$$t\times x_2=t\times R{x}_1$$
  两侧同时左乘$x_2^T$：$$x_2^{T}t\times x_2=x_2^{T}t\times R{x}_1$$
  左式中，$t\times x_2$是一个与$t$ 和 $x_2$都垂直的向量，把它再和$x_2$做内积时，将得到0。

  所以得到对极约束，$x_1,x_2为归一化坐标$：$$x_2^{T}t\times R{x}_1=0$$
  重新带入$p_1,p_2$，$p_1,p_2为像素坐标$：$$p_2^T{K}^{-T}t\times R{K}^{-1}{p}_1=0$$
  上面两式子为对极约束，几何意义是：$O_1，P,O_2$三者共面。

  定义本质矩阵$E$（Essential Matrix）和基础矩阵$F$（Fundamental Matrix）:$$E=t\times R，F=K^{-T}EK-1$$
  所以对极约束简写为：$$x_2^{T}E{x}_1=p_2^{T}F{p}_1=0$$
  根据对极约束，相机位姿估计问题变为以下两步，即估计$R,t$的方法为：

    1.根据匹配点的位置，求出$E$或者$F$。

    2.根据$E$或者$F$，求出$R,t$。

本质矩阵

  本质矩阵长什么样子：
    · $E=t\times R$,$E$是一个3×3的矩阵，内有9个未知数。

    · $E$在不同尺度下是等价的（$E$具有尺度等价性）：由于对极约束是等式为零的约束，所以对$E$乘以任意非零常数后，对极约束依然满足。

    · 平移和旋转各有3个自由度，所以 $t\times R$共6个自由度。由于$E$具有尺度等价性，这是由于对极约束 的性质，其乘任意非零向量依然满足，即增加一个约束条件，所以6个自由度中可以去掉一个， 所以$E$实际有5个自由度。

八点法

  · $E$具有5个自由度，最少使用5对点来求解$E$。但是，$E$具有一种非线性性质，在求解线性方程时会很麻烦。
  · 把$E$当成一个3×3的普通矩阵，共9个自由度，由于$E$的尺度等价性，$E$具有8个自由度，使用8对点来估计$E$，这就是“八点法”。

八点法推导过程

  一对匹配点，它们的归一化坐标为$x_1=[u_1,v_1,1{]}^T,x_2=[u_2,v_2,1{]}^T$，根据对极约束，有：
$$[u_1,v_1,1]\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$$
把$E$展开，写成向量的形式：
$$e=[e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9{]}^T$$
把对极约束写成与$e$有关的线性形式：
$$[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$$
同理，对于其他店对也有相同的表示。把所有点都放在一个方程中，变成线性方程组（$u^i,v^i$表示第$i$个特征点）：
$$\left[\begin{array}{ccccccccc}{u_1}^1{u_2}^1& {u_1}^1{v_2}^1& {u_1}^1& {v_1}^1{u_2}^1& {v_1}^1{v_2}^1& {v_1}^1& {u_2}^1& {v_2}^1& 1\\ {u_1}^2{u_2}^2& {u_1}^2{v_2}^2& {u_1}^2& {v_1}^2{u_2}^2& {v_1}^2{v_2}^2& {v_1}^2& {u_2}^2& {v_2}^2& 1\\ ...& ...& ...& ...& ...& ...& ...& ...& ...\\ {u_1}^8{u_2}^8& {u_1}^8{v_2}^8& {u_1}^8& {v_1}^8{u_2}^8& {v_1}^8{v_2}^8& {v_1}^8& {u_2}^8& {v_2}^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$$
由上式可以求得$E$，根据$E$求取相机的运动$R，t$。

本质矩阵$E$的SVD分解

  根据$E$求取相机的运动$R，t$，对$E$进行奇异值分解（$SVD$）：
  设$E$的SVD分解为：
$$E=U\Sigma V^T$$
  其中 $U,V$为正交阵，$\Sigma$为奇异值矩阵。根据本质矩阵$E$的内在性质，$E$的奇异值必定为$[\sigma ,\sigma ,0{]}^T$, 所以$\Sigma =diag[\sigma ,\sigma ,0]$

  在$SVD$分解中，对于任意一个$E$，存在两个可能的$t,R$与它对应：
$$t_1^{\Lambda }=U{R}_Z(\frac{\pi }{2})\Sigma U^T,R_1=U{R}_Z^T(\frac{\pi }{2})V^T$$
$$t_2^{\Lambda }=U{R}_Z(-\frac{\pi }{2})\Sigma U^T,R_2=U{R}_Z^T(-\frac{\pi }{2})V^T$$
 其中$R_Z^T(\frac{\pi }{2})$表示沿$Z$轴旋转$90^{\circ }$得到的旋转矩阵。


  由于$-E$和$E$等价，所以任意一个$t$取负号，也会得到相同的结果。因此，从$E$分解到$t,R$时，一共存在4个可能的解。
（1）$P$在$O_1，O_2$两个相机的前面，深度都为正。
（2）$P$在$O_1，O_2$两个相机的后面，深度都为负。
（3）$P$在$O_1$相机的后面，深度为负；在$O_2$相机的前面，深度为正。
（4）$P$在$O_1$相机的前面，深度为正；在$O_2$相机的后面，深度为负。

单目SLAM的一些问题

尺度不确定性

  t的尺度不确定性。t是平移向量，对t乘任意倍数，对极约束依然成立。
  这说明把地图和轨迹同时缩放任意倍，得到的观测值仍然是一样的。图7-7中，$P,O_1,O_2$点同时缩放任意倍，$p_1,p_2$点不变。

纯旋转问题

  若是纯旋转，t为0，而$E=t\times R$，则E为0，无法求解R。
  单目初始化不能只有旋转，必须要有平移。