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

前言

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

三角测量

上篇计算得到本质矩阵 E E E以及相机位姿 R , t R,t R,t后，就可以通过三角测量获得匹配点与相机的距离。 如上图所示 O 1 , O 2 O_1,O_2 O1​,O2​是相机经过的两个相邻位置，以 O 1 O_1 O1​为参考， O 2 O_2 O2​的位姿为 R , t R,t R,t，空间上的一点 P P P在归一化物理成像面上的位置为 p 1 , p 2 p_1,p_2 p1​,p2​，如果无投影投影误差，则 O 2 p 1 O_2p_1 O2​p1​与 O 1 p 1 O_1p_1 O1​p1​应当交于同一点，然而传感器内部的噪声以及匹配算法的误差会导致两条射线不会完全相交，可以通过最小二乘求得 P P P点的最佳位置。

没有投影误差时，对于 O 1 , O 2 , p 1 , p 2 , P O_1,O_2,p_1,p_2,P O1​,O2​,p1​,p2​,P有如下关系： s 1 p 1 = P s 2 p 2 = R P + t = s 1 R p 1 + t ( 1 ) s 2 p 2 × p 2 = ( s 1 R p 1 + t ) × p 2 = 0 ( 2 ) s_1p_1 = P \\ s_2p_2 = RP+t = s_1Rp_1+t \quad (1) \\ s_2p_2\times p_2 = (s_1Rp_1+t) \times p_2=0 \quad (2) \\ s1​p1​=Ps2​p2​=RP+t=s1​Rp1​+t(1)s2​p2​×p2​=(s1​Rp1​+t)×p2​=0(2) 由式（2）计算得到 s 1 s_1 s1​，然后把 s 1 s_1 s1​代入式（1），得到 s 2 s_2 s2​。

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

单应变换与单应矩阵

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

假设三维空间中的相机中心为 O O O，距离某一平面 S S S的距离为 d d d， S S S的法向量为 n n n，则 S S S上的点 P P P必然满足： n ⋅ P + d = 0 − n T P d = 1 {n \cdot P}+d=0 \\ -\frac {{n}^TP}{d} = 1 n⋅P+d=0−dnTP​=1 P P P在像素平面上的坐标为 p p p，代入成像方程，得到单应性关系： s 2 p 2 = K ( R P + t ) = K ( R P − t n T P d ) = K ( R − t n T d ) P = s 1 K ( R − t n T d ) K − 1 p 1 → p 2 = s 1 s 2 H p 1 s_2p_2=K(RP+t) \\ = K(RP-t\frac{{n}^TP}{d}) \\ = K(R-\frac{{t n}^T}{d})P\\ = s_1K(R-\frac{{t n}^T}{d})K^{-1}p_1 \\ \quad \\ \to p_2=\frac{s_1}{s_2}Hp_1 s2​p2​=K(RP+t)=K(RP−tdnTP​)=K(R−dtnT​)P=s1​K(R−dtnT​)K−1p1​→p2​=s2​s1​​Hp1​ H H H表示单应矩阵。由于投影过程中，深度 s 1 , s 2 s_1,s_2 s1​,s2​丢失，因此 H H H缺乏尺度约束。如果平面 S S S的参数已知，那么 P P P在 O 1 O_1 O1​相机坐标系下深度就可以求得： n ⋅ s 1 K − 1 p 1 + d = 0 s 1 = − d n T K − 1 p 1 n\cdot s_1K^{-1}p_1 + d = 0 \\ s_1 = -\frac{d}{n^TK^{-1}p_1} n⋅s1​K−1p1​+d=0s1​=−nTK−1p1​d​

单应矩阵计算

单应矩阵 H H H具有八个自由度，因此需要八个方程进行求解。对于一对匹配点，根据上面的单应性关系，有： [ u 2 v 2 1 ] = [ h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9 ] [ u 1 v 1 1 ] s e t h 9 = 1 t h e n u 2 = u 1 h 1 + v 1 h 2 + h 3 u 1 h 7 + v 1 h 8 + h 9 a n d v 2 = u 1 h 4 + v 1 h 5 + h 6 u 1 h 7 + v 1 h 8 + h 9 \begin{bmatrix} u_2 \\ v_2 \\ 1 \\ \end{bmatrix} = \begin{bmatrix} h_1 & h_2 & h_3 \\ h_4 & h_5 & h_6 \\ h_7 & h_8 & h_9 \\ \end{bmatrix} \begin{bmatrix} u_1 \\ v_1 \\ 1 \\ \end{bmatrix} \\ \quad \\ set \quad h_9 =1 \\ then \quad u_2=\frac{u_1h_1+v_1h_2+h_3}{u_1h_7+v_1h_8+h_9} \\ \quad \\ and \quad v_2=\frac{u_1h_4+v_1h_5+h_6}{u_1h_7+v_1h_8+h_9} ⎣⎡​u2​v2​1​⎦⎤​=⎣⎡​h1​h4​h7​​h2​h5​h8​​h3​h6​h9​​⎦⎤​⎣⎡​u1​v1​1​⎦⎤​seth9​=1thenu2​=u1​h7​+v1​h8​+h9​u1​h1​+v1​h2​+h3​​andv2​=u1​h7​+v1​h8​+h9​u1​h4​+v1​h5​+h6​​ 因此，一对匹配点提供了两个方程，则最少四对匹配点可以求出单应矩阵 H H H。 H H H的线性方程组如下：（偷个懒直接放十四讲的公式）

具体计算就不展开了。

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