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