视觉SLAM十四讲学习5 位姿估计（3）三角测量，单应矩阵

前言

本篇继续位姿估计章节，记录三角测量的原理和单应矩阵的计算。

三角测量

上篇计算得到本质矩阵$E$以及相机位姿$R,t$后，就可以通过三角测量获得匹配点与相机的距离。

如上图所示$O_1,O_2$是相机经过的两个相邻位置，以$O_1$为参考，$O_2$的位姿为$R,t$，空间上的一点$P$在归一化物理成像面上的位置为$p_1,p_2$，如果无投影投影误差，则$O_2{p}_1$与$O_1{p}_1$应当交于同一点，然而传感器内部的噪声以及匹配算法的误差会导致两条射线不会完全相交，可以通过最小二乘求得$P$点的最佳位置。

没有投影误差时，对于$O_1,O_2,p_1,p_2,P$有如下关系：
$$\begin{gathered} s_1{p}_1=P \\ {s}_2{p}_2=RP+t=s_{1}R{p}_1+t(1) \\ {s}_2{p}_2\times p_2=(s_{1}R{p}_1+t)\times p_2=0(2) \end{gathered}$$
由式（2）计算得到$s_1$，然后把$s_1$代入式（1），得到$s_2$。

三角测量和对极几何约束实际上都是通过$O_1,O_2,p_1,p_2,P$这五个点在同一平面上的性质，进行推导。

单应变换与单应矩阵

在三维空间中，将同一平面的点投影到另一个平面上，称为单应变换。用于约束这两个平面之间的投影关系的，就是单应矩阵。

假设三维空间中的相机中心为$O$，距离某一平面$S$的距离为$d$，$S$的法向量为$n$，则$S$上的点$P$必然满足：
$$\begin{gathered} n\cdot P+d=0 \\ -\frac{n^{T}P}{d}=1 \end{gathered}$$
$P$在像素平面上的坐标为$p$，代入成像方程，得到单应性关系：
$$\begin{gathered} s_2{p}_2=K(RP+t) \\ =K(RP-t\frac{n^{T}P}{d}) \\ =K(R-\frac{{tn}^T}{d})P \\ =s_{1}K(R-\frac{{tn}^T}{d})K^{-1}{p}_1 \\ \to p_2=\frac{s_1}{s_2}H{p}_1 \end{gathered}$$
$H$表示单应矩阵。由于投影过程中，深度$s_1,s_2$丢失，因此$H$缺乏尺度约束。如果平面$S$的参数已知，那么$P$在$O_1$相机坐标系下深度就可以求得：
$$\begin{gathered} n\cdot s_1{K}^{-1}{p}_1+d=0 \\ {s}_1=-\frac{d}{n^T{K}^{-1}{p}_1} \end{gathered}$$

单应矩阵计算

单应矩阵$H$具有八个自由度，因此需要八个方程进行求解。对于一对匹配点，根据上面的单应性关系，有：
$$\begin{gathered} \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] \\ set{h}_9=1 \\ then{u}_2=\frac{u_1{h}_1+v_1{h}_2+h_3}{u_1{h}_7+v_1{h}_8+h_9} \\ and{v}_2=\frac{u_1{h}_4+v_1{h}_5+h_6}{u_1{h}_7+v_1{h}_8+h_9} \end{gathered}$$
因此，一对匹配点提供了两个方程，则最少四对匹配点可以求出单应矩阵$H$。$H$的线性方程组如下：（偷个懒直接放十四讲的公式）

具体计算就不展开了。

OpenCV中实际上已经写好了本质矩阵，基础矩阵，单应矩阵，三角测量的求法，直接调用就行。