视觉SLAM理论入门——（8）视觉里程计之特征点法—PnP

PnP是求解 3D - 2D 点对运动的方法。它描述了当我们知道n 个 3D 空间点以及它们的投影位置时，如何估计相机所在的位姿

2D-2D的对极几何方法需要八个或八个以上的点对（以八点法为例），且存在着初始化、纯旋转和尺度的问题

然而，如果两张图像中，其中一张特征点的 3D 位置已知，那么最少只需三个点对（需要至少一个额外点验证结果）就可以估计相机运动。特征点的 3D 位置可以由三角化，或者由 RGB-D 相机的深度图确定

因此，在双目或 RGB-D 的视觉里程计中，我们可以直接使用 PnP 估计相机运动。而在单目视觉里程计中，必须先进行初始化，然后才能使用 PnP

PnP 问题有很多种求解方法，如：P3P、DLT、EPnP、UPnP等。除此以外还可以使用非线性优化方法，构建最小二乘问题并迭代求解（BA）

1、直接线性变换（DLT）

考虑某个空间点 P，它的齐次坐标为  。在图像  中，投影到特征点 （以归一化平面齐次坐标表示）。此时相机的位姿 R,t 是未知的。定义增广矩阵  （T不是变换矩阵） 为一个 3 × 4 的矩阵，包含了旋转与平移信息，展开形式如下：

用最后一行把 s 消去，得到两个约束（因为三个约束中只有两个线性无关）：

将 T 按行分解为  ，上面两个约束可以化为

假设一共有 N 个特征点，可以列出线性方程组：