视觉里程计 三维空间点的投影

3D-2D: PnP

优化：先定义一个误差，然后不断优化该误差。

DLT

需要解释的是，我们这里的 x1 使用了归一化平面坐标，去掉了内参矩阵 K 的影响——这是因为内参 K 在 SLAM 中通常假设为已知。如果内参未知，那么我们也能用 PnP去估计 K, R, t 三个量。然而由于未知量的增多，效果会差一些。

P3P

从 P3P 的原理上可以看出，为了求解 PnP，我们利用了三角形相似性质，求解投影点 a, b, c 在相机坐标系下的 3D 坐标，最后把问题转换成一个 3D 到 3D 的位姿估计问题。后文将看到，带有匹配信息的 3D-3D 位姿求解非常容易，所以这种思路是非常有效的。
例如 EPnP，亦采用了这种思路。然而，P3P 也存在着一些问题：

1. P3P 只利用三个点的信息。当给定的配对点多于 3 组时，难以利用更多的信息。
2. 如果 3D 点或 2D 点受噪声影响，或者存在误匹配，则算法失效。

在 SLAM 当中，通常的做法是先使用 P3P/EPnP 等方法估计相机位姿，然后构建最小二乘优化问题对估计值进行调整（Bundle Adjustment）。

Bundle Adjustment光束平差法