三角测量原理推导

三角测量原理推导

接着对极几何求解本质矩阵E后，通过E分解出四组R和t，通过三角测量原理求出深度，只有在两个相机下面深度均为正时，才是E分解出的正确R和t。下面是推导过程。

设\(p_1\)为世界坐标P点在相机1下面看到的像素坐标，\(p_2\)为相机2下面的像素坐标，以相机1的光心为世界坐标，K为相机标定参数矩阵，\(s_1\)和\(s_2\)分别为相机1坐标系和相机2坐标系下的z坐标。(其中，R为\(R_{21}\),t为\(t_{21}\))

\[\begin{align} s_1p_1=&KP \tag{} \\ s_2p_2=&K(RP+t) \tag{}\\ K^{-1}p_1 =& \frac{1}{s_1}P = 令x_1 \tag{}\\ K^{-1}p_2=&\frac{1}{s_2}(RP+t)=令x_2 \tag{}\\ \frac{1}{s_2}(Rs_1x_1+t)=&x_2 \tag{}\\ s_1Rx_1+t=&s_2x_2，两边同时叉乘x_2 \tag{1} \\ x_2^{\wedge}(s_1Rx_1+t)=&s_2x_2^{\wedge}x_2=0 \tag{}\\ s_1x_2^{\wedge}Rx_1+x_2^{\wedge}t=&0 \tag{} \end{align} \]

先求出\(s_1\)，带入(1)公式里面，再求\(s_2\)，\(s_1和s_2\)就是P点在相机1和相机2坐标系下面的深度。