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. 摄像机位姿的高精度快速求解