ORB-SLAM（十一）EPnP

EPnP在ORB-SLAM中主要用于Tracking线程中的重定位Relocalization模块，需要通过当前关键帧Bow与候选帧匹配上的3D地图点，迅速建立当前相机的初始姿态。

PnP问题解决了已知世界参考系下地图点以及相机参考系下投影点位置时3D-2D相机位姿估计问题，不需要使用对极约束（存在初始化，纯旋转和尺度问题，且一般需要8对点），可以在较少的匹配点（最少3对点，P3P方法）中获得较好的运动估计，是最重要的一种姿态估计方法。最后，如果知道世界参考系下的地图点，同时知道相机参考系下的地图点，可以通过ICP的方法去求解姿态。

这样就构成了立体视觉中最重要的对极几何，PnP和ICP三种最常用的姿态估计方法。

---

求解PnP问题目前主要有直接线性变换（DLT），P3P，EPnP，UPnP以及非线性优化方法。

DLT

直接构建一个12个未知数的[R|t]增广矩阵（先不考虑旋转矩阵的自由度只有3），取六个点对，去求解12个未知数（每一个3D点到归一化平面的映射给出两个约束），最后将[R|t]左侧3*3矩阵块进行QR分解，用一个旋转矩阵去近似（将3*3矩阵空间投影到SE(3)流形上）。

P3P

P3P方法是通过3对3D/2D匹配点，求解出四种可能的姿态，在OpenCV calib3d模块中有实现，但是对于相机远离3D平面（low parallax）或者视角垂直于3D平面的情况下效果不佳，不知道最近有没有更新过。论文Complete Solution Classification for the Perspective-Three-Point Problem中提到了一种改进的方法，可以消除这种退化的情况。

将世界坐标系下的ABC三点和图像坐标系下的abc三点匹配，其中AB，BC，AC的长度已知，<a,b>，<b,c>，<a,c>也是已知，通过余弦定理可以求出A，B，C在相机参考系中的坐标，然后使用类似ICP的坐标系对齐，就可以求得当前相机薇姿。

通过余弦定理构建二元二次方程组【2】可以求解出OA，OB，OC之间的长度比例，从而确定世界坐标系下的相机位姿。可以想象一下为什么会出现四个解，在空间中的位置是什么样的，以及为什么在远离3D点平面或者视角垂直3D点平面时，会出现退化情况。

EPnP

需要4对不共面的（对于共面的情况只需要3对）3D-2D匹配点，是目前最有效的PnP求解方法。

The aim of the Perspective-n-Point problem—PnP in short—is to determine the position and orientation of a camera given its intrinsic parameters and a set of n correspondences between 3D points and their 2D projections. It has many applications in Computer Vision, Robotics, Augmented Re- ality and has receivedmuch attention in both the Photogrammetry and Computer Vision communities. In particular, applications such as feature point-based camera tracking require dealing with hundreds of noisy feature points in real-time, which requires computationally efficient methods.

空间中任意3D点的坐标可以用4个不共面的3D点坐标的权重表示

通常选取世界坐标下的四个控制点坐标为C^w=[0,0,0,1]^T, [1,0,0,1]^T,[0,1,0,1]^T,[0,0,1,1]^T；通过n个3D点在相机平面的投影关系，以及与这四个控制点的权重关系，构建一个12*12方阵，求得其零空间特征向量，可以得到虚拟控制点的相机平面坐标，然后使用POSIT算法即可求出相机位姿。

通常在用EPnP求得四对点下的封闭解后，可以将该解作为非线性优化的初值，优化提高精度。

主要参考：

1. EPnP:An Accurate O(n) Solution to the PnP problem

2. 高翔，视觉SLAM十四讲

3. http://iplimage.com/blog/p3p-perspective-point-overview/

4. 摄像机位姿的高精度快速求解