PnP 單目相機位姿估計（一）：初識PnP問題
IDE：visual studio 2013
使用庫：Eigen opencv2.4.9
文件版本：1.1
簡介
PnP問題是求解3D-2D點對運動的方法。他描述了當知道n個三維空間點座標及其二維投影位置時，如何估計相機的位姿。我們可以想象，在一幅影象中，最少只要知道三個點的空間座標即3D座標，就可以用於估計相機的運動以及相機的姿態。
而特徵點的空間座標，我們可以通過深度圖來確定，亦可在程式初始化時，通過設定世界座標系的方法解得，具體請看後篇幾篇博文。
因此，PnP方法不需要使用對極約束，又可以在很少的匹配點中獲得較好的運動估計。是一種重要的姿態估計方法。
而PnP常見的解法有 直接線性變換、P3P、BA等，在後續幾篇博文中，將以基於opencv的solvepnp（P4P）同時藉助簡單的二維碼QRcode來實現相機位姿的估計。
理解
以下討論中設相機位於點Oc，P1、P2、P3……為特徵點。
當N=1時
當只有一個特徵點P1，我們假設它就在影象的正中央，那麼顯然向量OcP1就是相機座標系中的Z軸，此事相機永遠是面對P1，於是相機可能的位置就是在以P1為球心的球面上，再一個就是球的半徑也無法確定，於是有無數個解。
當N=2時
現在多了一個約束條件，顯然OcP1P2形成一個三角形，由於P1、P2兩點位置確定，三角形的變P1P2確定，再加上向量OcP1，OcP2從Oc點射線特徵點的方向角也能確定，於是能夠計算出OcP1的長度=r1，OcP2的長度=r2。於是這種情況下得到兩個球：以P1為球心，半徑為r1的球A；以P2為球心，半徑為r2的球B。顯然，相機位於球A，球B的相交處，依舊是無數個解。
當N=3時
與上述相似，這次又多了一個以P3為球心的球C，相機這次位於ABC三個球面的相交處，終於不再是無數個解了，這次應該會有4個解，其中一個就是我們需要的真解了。
當N大於3時
N=3時求出4組解，好像再加一個點就能解決這個問題了，事實上也幾乎如此。說幾乎是因為還有其他一些特殊情況，這些特殊情況就不再討論了。N>3後，能夠求出正解了，但為了一個正解就又要多加一個球D顯然不夠”環保”，為了更快更節省計算機資源地解決問題，先用3個點計算出4組解獲得四個旋轉矩陣、平移矩陣。根據公式：
將第四個點的世界座標代入公式，獲得其在影象中的四個投影（一個解對應一個投影），取出其中投影誤差最小的那個解，就是我們所需要的正解。
PNP問題的求解原理大致就是上面這樣了，在後續幾篇博文中，我們將以二維碼（QRcode）的形式，來進行相機位姿的計算。