视觉SLAM笔记(四)-对极几何
对极几何和PnP都是在已知若干对匹配点的情况下计算相机的运动，其中对极几何是计算2D-2D匹配点间的运动，而PnP是求解3D-2D匹配点对运动的方法
对极几何
对极约束
如图所示为对极几何约束，其中 I_1,I_2为两帧图像，p_1,p_2为两帧图像中所匹配的像素点，O_1,O_2为相机的光心，连接O_1O_2交I_1,I_2于e_1,e_2点，e_1,e_2称为极点，当p_1,p_2代表同一空间点在两幅图像中的投影时，理论上连接O_1p_1和O_2p_2，两条直线必交于空间中的一点P，O_1O_2P所确定的平面成为极平面，O_1O_2称为基线，极平面与像平面的交线l_1,l_2称为极线。
下面解释对极约束:
设P=[X,Y,Z]，以第一帧坐标系为起点，第二帧相对第一帧的旋转矩阵和平移向量分别为R,t，根据相机模型，易知
Zp_1 = KP,Zp_2=K(RP+t)
当p_1,p_2为使用齐次坐标时，忽略上式中的Z：
p_1 = KP,p_2=K(RP+t)
设x_1,x_2分别为P在两个归一化平面上的投影(非像素坐标，与像素坐标相差一个相机内参矩阵)
p_1 = Kx_1, p_2 = Kx_2
所以可得
x_2 = Rx_1+t
两侧左乘\hat{t}，得
\hat{t}x_2=\hat{t}Rx_1
两侧左乘x_2^T，得
x_2^T\hat{t}x_2=x_2^T\hat{t}Rx_1
\hat{t}x_2与x_2和t均垂直，故x_2^T\hat{t}x_2=0，得到如下约束
x_2^T\hat{t}Rx_1=0
这个约束即为对极几何约束，将p_1,p_2带入可得
p_2^T K^{-T} \hat{t} R K^{-1} p_1=0
令E=\hat{t}R，称为本质矩阵(Essential Matrix)，F= K^{-T} \hat{t} R K^{-1}为基础矩阵(Fundamental Matrix)，对极约束如下:
x_2^T E x_1=0, p_2^T F p_1=0
可见E和F间主要相差了相机内参K，但通常认为相机内参已知(已标定)，故通常使用E
求解
E是一个3\times3的矩阵，具有如下几个特性:
- 对E进行SVD分解后，E=U\Sigma V^T，\Sigma的形式为\Sigma=diag\lbrace \sigma,\sigma,0\rbrace
- E具有尺度不变性，并且由于R,t各有三个自由度，故E共有5个自由度
求解E有两种方式，一种是在本质矩阵所属的本质空间(Essential space)内求解E，或者可以在对极几何约束下求解E，再将其投影到本质空间中，通常情况下第二种方式较为简单，这里我们只考虑其尺度不变性，认为E具有8个自由度，使用8对点来估计E，即所谓的八点法
设x_1=[u_1,v_1,1]^T，x_2=[u_2,v_2,1]^T
故
\left(u_{1}, v_{1}, 1\right)\left(\begin{array}{lll}{e_{1}} & {e_{2}} & {e_{3}} \\ {e_{4}} & {e_{5}} & {e_{6}} \\ {e_{7}} & {e_{8}} & {e_{9}}\end{array}\right)\left(\begin{array}{l}{u_{2}} \\ {v_{2}} \\ {1}\end{array}\right)=0
将E写成向量的形式，e=\left[e_{1}, e_{2}, e_{3}, e_{4}, e_{5}, e_{6}, e_{7}, e_{8}, e_{9}\right]^{T}
对极约束可改写为如下形式:
\left[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\right] \cdot e=0
将8对点放到一个方程中，若8个方程线性不想关，则可按下面的方程解出E的各元素
\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} \\ {\vdots} & {\vdots} & {\vdots} & {\vdots} & {\vdots} & {\vdots} & {\vdots} & {\vdots} & {} \\ {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}{l}{e_{1}} \\ {e_{2}} \\ {e_{3}} \\ {e_{5}} \\ {e_{5}} \\ {e_{6}} \\ {e_{7}} \\ {e_{8}} \\ {e_{9}}\end{array}\right)=0
接下来可通过SVD分解求得R,t
{t}_{1}^{\wedge}={U} {R}_{Z}\left(\frac{\pi}{2}\right) {\Sigma} {U}^{T},{R}_{1}={U} {R}_{Z}^{T}\left(\frac{\pi}{2}\right) {V}^{T}\\ {t}_{1}^{\wedge}={U} {R}_{Z}\left(-\frac{\pi}{2}\right) {\Sigma} {U}^{T},{R}_{1}={U} {R}_{Z}^{T}\left(-\frac{\pi}{2}\right) {V}^{T}
其中{R}_{Z}^{T}\left(\frac{\pi}{2}\right)表示一个旋转矩阵(沿Z轴旋转90°的旋转矩阵)，由于E和-E等价，对t取负号结果不变，故共有四个解，但只有一种解P在两个相机中都具有正的深度，带入检验即可
但是这种方式求得的E，可能不满足\Sigma=diag\lbrace \sigma,\sigma,0\rbrace，将E进行SVD分解后，得到\Sigma=diag\lbrace \sigma_1,\sigma_2,\sigma_3\rbrace，设\sigma_1\geq \sigma_2\geq \sigma_3，取
E=Udiag(\frac{\sigma_1+\sigma_2}{2},\frac{\sigma_1+\sigma_2}{2},0)V^T
单应矩阵
单应矩阵(Homography)通常描述平面上的点在不同图像之间的变换关系，故也被称为planar homography，假设一对特征点p_1,p_2落在平面P上，平面P上的点满足方程
n^TP+d=0 \Rightarrow -\frac{n^TP}{d}=1
根据相机投影模型:
p_2=K(RP+t)\\ =K(RP-t\frac{n^TP}{d})\\ =K(R-\frac{tn^T}{d})P\\ =K(R-\frac{tn^T}{d})K^{-1}p_1
令H=K(R-\frac{tn^T}{d})K^{-1} \in R^{3\times3}，H即为单应矩阵，p_2=Hp_1
首先根据匹配点对计算H：
\left(\begin{array}{c}{u_{2}} \\ {v_{2}} \\ {1}\end{array}\right)=\left(\begin{array}{lll}{h_{1}} & {h_{2}} & {h_{3}} \\ {h_{4}} & {h_{5}} & {h_{6}} \\ {h_{7}} & {h_{8}} & {h_{9}}\end{array}\right)\left(\begin{array}{l}{u_{1}} \\ {v_{1}} \\ {1}\end{array}\right)
解得
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}}
整理得
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}
故每个点对可提供两个方程得约束，共需要四个点对即可解出单应矩阵H(H具有尺度等价性)，在通过分解单应矩阵可以恢复出R,t以及平面法向量
三角测量
三角测量得目的是在已知相机位姿的情况下根据对极几何估计出特征点在相机坐标系下的深度，根据对极几何，两相互匹配点的归一化坐标有如下关系:
s_2x_2=s_1Rx_1+t
两边左乘\hat{x_2}，得
\hat{x_2}s_2x_2=s_1\hat{x_2}Rx_2 + \hat{x_2}t=0
即得到了一个关于s_1的关系式，解出s_1，可继续解出s_2，至此完成深度恢复。
参考文献
[1] 高翔, 张涛, 颜沁睿, 刘毅, 视觉SLAM十四讲：从理论到实践, 电子工业出版社, 2017
[2] https://vision.in.tum.de/teaching/ss2019/mvg2019