SLAM-三角化

---

一、三角化公式

在VSLAM中，我们可以通过对极几何、PnP等方法估计相机运动，之后我们需要利用相机运动估计特征点的空间位置。在单目SLAM中，仅通过单张图像无法获得像素的深度信息，我们常用三角化（三角测量）的方法估计地图点的3D坐标。

如图所示，世界坐标系中的点$P_j$，在$k$时刻的图像上投影，得到像素点$p_{j,k}$，设$P_j$的坐标为${\left[\begin{array}{cccc}{X}_w& Y_w& Z_w& 1\end{array}\right]}^T$，$p_{j,k}$的坐标为${\left[\begin{array}{ccc}u& v& 1\end{array}\right]}^T$，从$P_j$到$p_{j,k}$的投影过程可以用下式表示
λ [ u v 1 ] = K [ R ∣ t ] c w [ X w Y w Z w 1 ] \lambda \begin{bmatrix}u \\v \\1\end{bmatrix}=K\left [ R|t\right ]_{cw} \begin{bmatrix}X_{w} \\Y_{w} \\Z_{w} \\1\end{bmatrix} λ ​uv1​ ​=K[R∣t]cw​ ​Xw​Yw​Z