SLAM十四讲之第七讲：2D-2D对极几何与3D-2DPnP问题推导

第七讲：视觉里程计2

2 2D-2D对极几何

​ 作者应该是基于ORB-SLAM写的后续内容。

2.1 对极约束

​ 在第一帧坐标系下，三维点P的坐标如下：

$$\mathbf{P}=[X,Y,Z{]}^{\mathrm{T}}$$

​ K是相机内参矩阵，把相机系得点转换为像素坐标系点。这里我们以第一帧坐标系为参考坐标系(世界系)，把第1帧投影到第2帧当中。

$$s_1{\mathbf{p}}_1=\mathbf{K}\mathbf{P},s_2{\mathbf{p}}_2=\mathbf{K}({\mathbf{R}}_{21}\mathbf{P}+{\mathbf{t}}_{21})$$

​ 在相机模型中，s一般都是指三维点P得深度Z，也就是一个常数，$sp和p$形成了投影关系，它们在尺度意义下是相等的！
$$sp\simeq p$$

​ 把投影方程改为尺度意义下等式：
$$p_1\simeq KP,p_2\simeq K\left(RP+t\right)$$
​ p是像素点坐标，乘以$K^{-1}$后即为相机坐标系下得归一化坐标$x$，
$$\begin{array}{rl}& \\ & {\mathbf{x}}_1={\mathbf{K}}^{-1}{\mathbf{p}}_1,{\mathbf{x}}_2={\mathbf{K}}^{-1}{\mathbf{p}}_2.\end{array}$$

​ 把第1帧下得归一化坐标投影到第二帧下，得到尺度意义下等式（或利用上面得式子带入到尺度意义下投影方程）

$${\mathbf{x}}_2\simeq \mathbf{R}{\mathbf{x}}_1+\mathbf{t}$$

​ 注意向量t反对称矩阵也即向量与向量得叉积，对上述式子左乘向量t，可得：

$$\begin{array}{rl}& {\mathbf{t}}^{\wedge }{\mathbf{x}}_2\simeq {\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{x}}_1\end{array}$$

​ 向量$t^{\wedge }x$即叉积，得到得结果是一个垂直于t和x得向量，所以在乘以x，那么向量得点击必然为0.
$${\mathbf{x}}_2^{\mathrm{T}}{\mathbf{t}}^{\wedge }{\mathbf{x}}_2\simeq {\mathbf{x}}_2^{\mathrm{T}}{\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{x}}_1=0$$
​ 此时，得到得下面这两个式子就是对极约束，分别由本质矩阵E和基础矩阵F组成
$$\begin{array}{rl}& {\mathbf{x}}_2^{\mathrm{T}}{\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{x}}_1=0\\ & {\mathbf{p}}_2^{\mathrm{T}}{\mathbf{K}}^{-\mathrm{T}}{\mathbf{t}}^{\wedge }\mathbf{R}{\mathbf{K}}^{-1}{\mathbf{p}}_1=0\end{array}$$

​ 也能够推断出，本质矩阵E和基础矩阵F之间相差一个内参矩阵K
$$\mathbf{E}={\mathbf{t}}^{\wedge }\mathbf{R},\mathbf{F}={\mathbf{K}}^{-\mathrm{T}}\mathbf{E}{\mathbf{K}}^{-1},{\mathbf{x}}_2^{\mathrm{T}}\mathbf{E}{\mathbf{x}}_1={\mathbf{p}}_2^{\mathrm{T}}\mathbf{F}{\mathbf{p}}_1=0$$

2.2 本质矩阵

2.2.1 本质矩阵性质

​ $\mathbf{E}={\mathbf{t}}^{\wedge }\mathbf{R}$是一个3*3矩阵

1. 对极约束得条件是$0$，所以该等式乘以任意得常数后仍然为$0$，也即对E乘以任意的常数后，对极约束依然成立。即$E$在不同尺度下是等价的。
2. 本质矩阵$E$经过$SVD$分解后的奇异值必然是$[\sigma ,\sigma ,0{]}^{\mathrm{T}}$.
3. 本质矩阵实际只有5个自由端(旋转3+偏移3-尺度1)

2.2.2 八点法求解本质矩阵

​ 考虑一对匹配点，它们的归一化坐标为${\mathbf{x}}_1=[u_1,v_1,1{]}^{\mathrm{T}},{\mathbf{x}}_2=[u_2,v_2,1{]}^{\mathrm{T}}$。根据对极约束，有

$$\left(u_2,v_2,1\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写成一个向量的形式

$$\mathbf{e}=[e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9{]}^{\mathrm{T}}$$
​ 则对极约束可以写成于e有关的线性形式：
$$[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]\cdot \mathbf{e}=0$$
​ 就是说，一对点可以得到一个上面的式子，我们利用八对点就可得到下面的方程。这个方程可以看成9*9的矩阵，最后一行为0，就是这个矩阵不满秩，那么它一定有非0解且唯一！
$$\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}{l}{e}_1\\ e_2\\ e_3\\ e_4\\ e_5\\ e_6\\ e_7\\ e_8\\ e_9\end{array}\right)=0.$$

2.2.3 怎么利用E来求解位姿

​ 我们最关系的是利用$E$来估计相机运动$R，t$，通过奇异值分解计算
$$E=U\Sigma V^{\mathrm{T}}$$
参考计算机视觉中的多视图几何200页结论
$$\begin{array}{rl}{\mathbf{t}}_1^{\wedge }& =\mathbf{U}{\mathbf{R}}_Z\left(\frac{\pi }{2}\right)\mathbf{\Sigma }{\mathbf{U}}^{\mathrm{T}},{\mathbf{R}}_1=\mathbf{U}{\mathbf{R}}_Z^{\mathrm{T}}\left(\frac{\mathbf{\pi }}{2}\right){\mathbf{V}}^{}\\ {\mathbf{t}}_2^{\wedge }& \end{array}$$