PnP算法

PnP （Perspective-n-Point）是求解3D到2D点对运动的方法。解决的问题是：当我们知道n个3D空间点以及他们的投影位置时，估算相机所在的位姿。特征点的3D位置可以由三角化或者RGB-D相机的深度图确定。因此，在双目或者RGB-D的视觉里程计中，可以直接使用PnP估计相机运动。

直接线性变换（DLT）：

空间点P，齐次坐标为。投影到归一化平面的特征点齐次坐标为，此时相机的位姿R，t是未知的。定义一个增广矩阵为一个3X4的矩阵，包含了旋转与平移信息。将投影矩阵展开如下：

用最后一行消去s后，可以得到两个约束：

                 

定义T的行向量为：

         

于是有：

        

假设有N个特征点：可以求出线性方程组为

由于t一共是12维度，所以最少需要六对匹配点，即可实现矩阵T的线性求解，这种方法称为直接线性变换。当匹配点对大于六对时，可以使用SVD等方法对超定方程求最小二乘解。