如果EPnP論文不太好找的話可以從我的百度網盤下：鏈接：https://pan.baidu.com/s/1pY18HkLMsjfW3Zxe9Sis3A
提取碼：mzek
看了一些講EPnP的博客，感覺大家都是上來就推公式，也沒有個前戲啥的，乾巴巴，我覺得對那些之前沒接觸過但是想深入瞭解的童鞋來說不太友好，我想換個思路來講這個O(n)的方法，注重理解。另外提前說明一下，我希望我的博客大多都是白話，就像在聊天一樣，最終把它講清楚，所以可能有的地方會“廢話”多一些，比如開個車啥的，不影響我們的共同目的就好！
總述：
EPnP說白了就是在解決一個問題，如何將世界座標系下的3D點和各自投影到相機平面的2D點結合起來，計算得到世界座標系到相機座標系的R和t。EPnP的思路也比較簡單，就是想先把將2D恢復到相機座標系下的3D，然後再和世界座標系的3D進行ICP，求出來R和t就完了，是不是挺簡單，但是這有一個問題，2D怎麼回到3D，如果簡簡單單的用這個圖片就能回去那還搞什麼前端啊對吧，所以整個問題的核心就是這裏，我們需要給2D加一些別的約束，來求出3D。那麼EPnP是這樣想的：Rt是一個剛體變換，並沒有失去結構信息，比如一個杯子，杯子的底座在世界座標系下相對於杯子中心的位置在變到相機座標系之後並不變，因爲剛體變換不改變物體本身的樣子，所以我們就從這個點開始說起！
我們重新梳理一下：
EPnP的輸入：
- 世界座標系下的3D點
- 對應投影到相機屏幕的2D點
- 相機內參
EPnP輸出：
- 世界座標系到相機座標系的轉換關係R和t
EPnP思路：
- 利用結構信息不變性將2D恢復到3D座標（相機座標系下）
- 與世界座標系的3D座標進行ICP，求得R和t
現在我們開始：
第一步：找出世界座標系下的控制點wC
上面這張圖我把兩個座標系下的點列出來了，如果要我們找結構信息，那我們首先會想到歐氏距離，也就是他們的座標，或者說是相對位置，好像我們可以明顯感覺到同一個點在兩個座標系下的座標有一種強相關，但是直接用座標來計算距離真的是太麻煩了，而且那麼多點，把每個點都計算一遍明顯不現實。這時候EPnP站出來了，他說既然都是算相對距離，那我乾脆找幾個標誌物，讓每個點都相對於這些個標誌物有一個相對位置，那麼不管座標系怎麼變，相對位置肯定不會變的對吧，這樣就不用處理那麼多點了，好，說幹就幹，怎麼選標誌物呢，中心肯定算一個，那剩下的呢？我既然在三維上，那我就找點集相對比較集中的3個方向好了，交給PCA來做好了，現在我就得到了世界座標系下的4個標誌點c1,c2,c3,c4，我們把它叫做控制點。
第二步：找出世界座標系下每個點關於控制點的相對座標α
有了控制點，我們需要找每個點相對於控制點的位置，也就是需要4個權重來表示這每個點，點座標我知道，控制點座標我也知道，通過上圖這麼一算我就知道了每個點的相對位置，用α來表示就是α1,α2,α3,α4，也就相當於知道了他們的結構信息，🆗，由總述我們知道相對位置是不變的，也就是相機座標系下相對位置也是α；上圖中wXi表示世界座標系下的3D座標，c0~c3是世界座標系下的控制點（上一步已經求出來的），α0~α3是相對座標，wc0是中心座標，通過減去中心座標，可以利用公式求出來α1~α3，然後我不能修改3D點距離中心點的長度，所以權重肯定得爲1，所以α0就用1減去他們三個就好了；
第三步：由兩個約束構建等式MX=0
上上圖中紅筆寫的代表還未求出來的，藍筆代表已求出來的，可以看到相機座標系下的3D點的結構約束已經被我們找出來了，再利用投影約束就就可以寫出等式了，如下圖所示。構造這樣的等式可以將相機座標系下的3D點消掉（下圖的紅框），從而只留下相機座標系下的控制點作爲未知量，而且是12維的。
每對3D-2D匹配點能夠提供2個約束，而我們的未知量是一個12維的向量，所以看樣子我們就可以用ICP求出來R和t，大功告成了！
第四步：其實還沒完,我們還需要求解尺度
我們求出來這有一個向量(12x1)之後，還缺少尺度因子，因爲我們求出來的向量大多會歸一化操作，之後就不知道該向量真實的模長．所以這裏還需要再求一下各個向量的係數，首先可以解決向量模長的問題，其次可以解出各個向量的權重問題，所以這樣的話就得去找M的零空間了（因爲是求Mx=0，M是矩陣，x是向量，把構成這種等式的向量張成的空間叫做矩陣M的零空間，所以向量x應該處於M的零空間內纔可以出現等於0），對M求SVD和對MTM求特徵值是一樣的，而且整個EPnP的複雜度體現在這裏，所以是O(n)。而且我現在要求的x是MTM的零空間，所以我要找MTM的最小特徵值對應的特徵向量，纔是x，那麼問題來了，難道這裏只取這麼一個特徵值很小的特徵向量嗎？
第五步：處理特徵向量的問題
EPnP說：非也！當我採用6對匹配點的時候，剛好只留下一個爲0的特徵值，所以這種情況下用一個特徵向量就行了，但是我用4對的時候、或者焦距變得很大、或者有噪聲的時候，最多可以求出來4個爲0的特徵值（當噪聲很大的時候可能特徵值不是準確的爲0，但是已經很小很小了），像上圖所示，焦距f=100時只有第12個特徵值是0，所以只要最後一個特徵向量就可以了，但是當焦距f=10000時，第9，10，11，12特徵值都是0了，也就是說我只用一個特徵向量是沒有辦法表示的，那我就只能把這4個加權來表示下。（我沒太想明白作者是怎麼確定最多一定只有4個，會不會更多）然後很自然的給特徵向量的權重起名爲β，且最多有4個，即N最大爲4。
第六步：已知特徵向量，求取權重β
我們再來梳理一下：現在我已經把這些特徵向量求出來了，接下來要求一個權重因子，把特徵向量加權來表示未知量x，再次聲明：x是12x1的向量，表示相機座標系下的4個控制點座標，所以這一步我們要用β來表示這4個控制點座標，然後再找約束把β求出來。上一步知道N可以取1，2，3，4，取不同的N得到的β也是不一樣的，所以EPnP分4種情況來討論這個，但是約束從哪裏來呢，我們知道結構信息是不變的，所以控制點之間的距離也是不變的，所以C42有6個距離，那就是世界座標系下的某兩個點距離與相機座標系下同樣點之間的距離是一樣的，所以一共有6個約束。
N=1的時候就一個β，直接就可以求出來，N=2的時候我們把距離表示出來發現有平方項呢，所以是3個未知量，也能求，N=3的時候6個未知數也可以hold住，N=4的時候就不行了，約束不夠了，EPnP說不怕我們用線性化方法，直接令β中的某幾個項爲0，比如我直接令上圖β向量中的後幾個都爲0，把未知量數量控制住，當然也可以把其他的某幾項設爲0，反正能夠用6個約束求出來一些未知量就行，但是我們知道這樣肯定不準，沒關係，我們還有一個萬金油方法：優化。
第七步：GN優化得到最後的β
我們把上一步求出來的不太準的β放到GN優化器裏面去當一個比較靠譜的初始值，依舊利用控制點之間的距離平方差作爲殘差，然後採用樸素的GN方法實現最後的β的求解，最後得到相機座標系的控制點座標，再通過ICP和世界座標系下的控制點座標求出來R和t，就算大功告成了。
總結：
看了挺多次論文，之前總是不懂爲什麼這樣做，看別的博主覺得公式講的很清楚，但是不知道爲什麼這樣做，所以看完跟沒看一樣，直到最近聽大佬將，對EPnP的思想有了自己的理解，再深入去想一想，就能明白很多東西。俗話說能講出來纔是真正懂了，我就試着用博客來講一講，如果有講的比較亂的地方希望大家多多指正！