对极几何方法——2D图片特征点估计运动

引入

如图所示，在相邻两帧图片$I_1,I_2$中：

• $p_1,p_2$是两个匹配的特征点，
• $o_1,o_2$是两帧图片的光心，即小孔成像的小孔，
• 连接$o_1{p}_1$和$o_2{p}_2$，相交于现实中的点p。
• 连接$o_1{o}_2$与两帧图片分别交于$e_1,e_2$。
  显然，$p,p_1,o_1,e_1,e_2,o_2,p_2$在同一平面，这个约束就是对极几何约束

对极几何约束

由针孔相机模型可知，对于第一帧照片：
$$z_1\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=K\cdot P$$
两帧之间相机有一个旋转R和平移t运动，则对于第二帧照片：
$$z_2{p}_2=K(RP+t)$$
$p_2=[u_2,v_2,1{]}^T$
则有：
$$\{\begin{array}{rl}& z_1{K}^{-1}{p}_1=P\\ & z_2{K}^{-1}{p}_2=RP+t\end{array}$$
令$K^{-1}{p}_1=x_1,K^{-1}{p}_2=x_2$则有：
$$\{\begin{array}{rl}& z_1{x}_1=P\\ & z_2{x}_2=RP+t\end{array}$$
解得：
$$z_2{x}_2=R(z_1{x}_1)+t$$
两边同时左乘$t^{\wedge }$("∧"表示向量的正交矩阵，相当于叉乘)得：
$$z_2{t}^{\wedge }{x}_2=z_1{t}^{\wedge }R{x}_1$$
两边左乘$x_2^T$得：
$$z_2{x}_2^T{t}^{\wedge }{x}_2=z_1{x}_2^T{t}^{\wedge }R{x}_1$$
左式中$t^{\wedge }{x}_2$与t和$x_2$均垂直，故再与$x_2$点乘结果为0。因此：
$$0=x_2^T{t}^{\wedge }R{x}_1$$
重新代入$p_1,p_2$有：
$$0=(K^{-1}{p}_2{)}^T{t}^{\wedge }R{K}^{-1}{p}_1$$
即：
$$0=p_2^T{K}^{-T}{t}^{\wedge }R{K}^{-1}{p}_1$$
这就是对极几何约束。它的几何意义是$o_1,P,o_2$三者共面。
我们把中间部分记作两个矩阵：基础矩阵(Fundamental Matrix)F和本质矩阵(Essential Matrix)E，于是可以进一步简化对极约束：
$$\{\begin{array}{rl}& E=t^{\wedge }R\\ & F=K^{-T}E{K}^{-1}\\ & 0=x_2^{T}E{x}_1,0=p_2^{T}F{p}_1\end{array}$$
于是，相机位姿估计问题变为以下两步：

1. 根据配对点的像素位置求出E或者F。
2. 根据E或F求出R,t。

由于E和F只相差了相机内参，而内参在SLAM中通常是已知的，所以实践中往往使用形式更简单的E。因此以E为例，介绍上面两个问题如何求解。

利用对极几何约束估计相机运动

E为3×3的矩阵，有9个未知数。为了便于计算只考虑它的尺度等价性（即矩阵同时除以其中一个未知数，可以使得这个未知数变成1，这样的E也是等价的，从而可以减少一个未知数），使用8对点来估计E——这就是经典的八点法(Eight-point-algorithm)。
考虑一对匹配点，它们的归一化坐标为x1=[u1,v1,1]T,x2=[u2,v2,1]T。根据对极约束，有：

把矩阵E展开，写成向量的形式：

那么，对极约束等价于(可以自己矩阵运算一下就明白了)：

同理，对于其他点对也有相同的表示。我们把所有点都放到一个方程中，变成线性方程组：

如果这八个点互不相关，即系数满秩，那么E的各元素就可由上述方程解得。
对E进行奇异值分解(SVD)即可求得R和t。
奇异值分解如果不明白可以参看其他资料，这里不多赘述了。
这个方法在opencv中有成熟的代码实现。如果大家感兴趣，我也会抽空把代码和注释附上。