三角化公式推导&手撕代码

前言

三角测量是在已知相机参数和图像中匹配点的情况下，求解这些匹配点对应的空间点三维坐标的方法。针对单目与双目系统，三角测量的使用方法有所不同。双目视觉测距原理参见：双目视觉测距原理深度剖析：一个被忽略的小问题，与双目相机相比，单目相机拍摄的两张图片没有固定的位姿，也就没有准确的基线，所以其三角化是指根据相机的运动来估计特征点的深度信息。

这里主要介绍直接线性变换法（Direct Linear Transform，DLT）。

直接线性变换法

设三维空间点$P$在世界坐标系下的齐次坐标为$X=[x,y,z,1{]}^T$，相应的在两个视角的投影点分别为$p1$和$p2$，它们在相机坐标系下的坐标为：
$$\overrightarrow{x_1}=[x_1,y_1,1{]}^T,\overrightarrow{x_2}=[x_2,y_2,1{]}^T$$

两幅图像对应的相机投影矩阵分别为$P_1、P_2$（3*4维），
$$P_1=[P_{11},P_{12},P_{13}{]}^T,P_2=[P_{21},P_{22},P_{23}{]}^T$$其中，$P_{11}、P_{12}、P_{13}$分别是投影矩阵$P_1$的第1-3行；$P_{21}、P_{22}、P_{23}$分别是投影矩阵$P_2$的第1-3行；在理想情况下，有：
x 1 → = P 1 X , x 2 → = P 2 X \overrightarrow {x_1}=P_1X,\overrightarrow {x_2}=P_2X