跟隨這高博的腳步走到了pose estimation 求相機的外參, 參考了幾篇blog和paper, 總算稍微有點管中窺豹的感覺, 暫且記錄下來以待後續
PNP 相機位姿估計pose estimation就是通過幾個已知座標(世界座標)的特徵點,結合他們在相機照片中的成像(像素座標),求解出相機所在的世界座標以及旋轉角度(這兩個貨叫相機的外參), 用旋轉矩陣(R)和平移矩陣(t)表示. PnP, which is short for perspective n point就是其中的一種2D-3D解決上述問題的算法, 也就是說, 這個算法的輸入是一組點的三維世界座標和二維像素座標,輸出是相機的旋轉矩陣(R)和平移矩陣(t). OpenCV有相應的庫函數solvePnP()和solvePnPRansac(),後者利用了ransac的思想計算更加精確的結果,不過據說速度很慢,完全沒法做到實時.
PNP具體細分爲幾種, 直接線性變換 P3P EPnP UPnP等等, OpenCV目前提供了3種方法進行PNP計算, CV_P3P, CV_ITERATIVE, CV_EPNP.
____P3P____ P3P的方法,是用非常經典的<Complete solution classification for the perspective-three-point problem> published by Gao in 2003的一篇論文,用空間中3個已知世界座標和像素座標的點,通過構造四個三角形的邊角關係聯立方程組,求出4組可能的解,再通過第四個已知點對四組解進行驗證,返回重投影誤差最小的點.
單純看這個圖真的會很費解,其實就是三角形三邊關係公式 a²+b²-2ab*cos∠c=c²,假如我們做特徵匹配的兩幀圖是frame1和frame2,那麼ABC三個點就是匹配的3對特徵點的世界地圖座標位置,即frame1上的A1.B1.C1匹配frame2上的A2B2C2. P點是frame2的相機所在位置的相機光心. 這個方程組裏的已知條件是|AB||BC||AC|的長度,因爲ABC三點的世界座標已知;
和∠α.∠β,∠γ,因爲A2B2C2的像素座標已知,(通過像素座標和內參矩陣求到光心距離然後構造三角形求角).
右側的make是一堆爲了化簡式子所做的assumptions, 最後得到的二元二次方程組裏,除了x,y剩下的量都是已知,解這個方程組需要用到二十世紀八十年代的一篇論文介紹的吳消元法, 簡單看了一下,大體上的思路基本都沒看懂,就交給opencv吧...反正我知道這個方程組最多有四組x,y的解, 然後用frame1和frame2上的另外一組匹配點D1D2代入四組解驗證就算重投影誤差最小的一組解,即爲輸出的結果.