SLAM前端:PnP(一)DLS、P3P

PnP是一類問題的統稱,是指通過多對點的3D位置及2D投影座標,來估計相機位姿R、t。

場景一:視覺slam中在初始化後可以知道空間中一些點在世界座標系下的座標,在下一幀圖像進行特徵點匹配後,利用這些特徵點的3D座標及2D像素座標,PnP可以計算出當前幀圖像的R、t,即在世界座標系下的R、t。

場景二:輸入匹配特徵點在上一幀的2D像素座標、在上一幀相機座標系下的3D座標、在當前幀的2D像素座標,求兩幀之間的位姿R、t。

PnP是一大類問題,根據問題不同解法有P3P、DLS、EPnP、UPnP等。

1 P3P

對於場景一問題可以用P3P問題求解,需要三個點即可完成。需要條件:三個點的世界座標、當前幀下的2D像素座標。求解思路是首先求解三個點在相機座標系下的座標,再通過世界座標系到像素座標系的轉換公式計算相機在世界座標系下的R、t。

 

 

場景如上圖所示,三個空間點ABC、圖像投影點abc、相機光心O。計算過程如下:

首先根據餘弦定理可得:

                                                \\ OA^2 + OB ^2-2OA\cdot OB\cdot cos<a,b> = AB^2\\ OB^2 + OC ^2-2OB\cdot OC\cdot cos<b,c> = BC^2\\ OA^2 + OC ^2-2OA\cdot OC\cdot cos<a,c> = AC^2\\

等式兩邊同時除以OC^2,並令x = \frac{OA}{OC},y=\frac{OB}{OC}得到:

                                                           \\ x^2 + y^2-2xycos<a,b>=\frac{AB^2}{OC^2}\\ y^2 + 1-2ycos<c,b>=\frac{BC^2}{OC^2}\\x^2 + 1-2xcos<a,c>=\frac{AC^2}{OC^2} \\

u = \frac{AB^2}{OC^2},v = \frac{BC^2}{AB^2},w = \frac{AC^2}{AB^2}得到:

                                                             \\ x^2 + y^2-2xycos<a,b>=u\\ y^2 + 1-2ycos<c,b>=uv\\x^2 + 1-2xcos<a,c>=uw \\

方程中已知量爲u,v,w,cos<a,b>,cos<a,c>,cos<b,c>,未知量爲x,y,兩個未知量將上述方程化簡得到:

求解cos<a,b>:

 

 

得到角度爲:

同理可以得到:cos<a,c>,cos<b,c>

2 DLS(直接線性變換)

設空間中某點P的世界座標爲[X,Y,Z,1],在圖像I_1中的2D座標爲[u,v,1],現在求解R、t。

首先定義曾廣矩陣[R|t],由世界座標系轉像素座標系有:

                                                          s $\left[ \begin{matrix} u \\ v \\ 1 \end{matrix} \right] $= $\left[ \begin{matrix} t_1 & t_2 &t_3 &t_4\\ t_5 & t_6 &t_7&t_8\\ t_9 & t_{10} & t_{11}&t_{12} \end{matrix} \right] $\cdot $\left[ \begin{matrix} X \\ Y \\ Z \\1 \end{matrix} \right] $

消去s得到:

                                  u = \frac{t_1 X + t_2Y+t_3Z+t_4}{t_9X+t_{10}Y+t_{11}Z+t_{12}},v = \frac{t_5 X + t_6Y+t_7Z+t_8}{t_9X+t_{10}Y+t_{11}Z+t_{12}}

令等式右邊爲0得到:

                                                $\\$ [t_1 t_2 t_3 t_4]\left[ \begin{matrix} X \\ Y \\ Z \\1 \end{matrix} \right] -[t_9 t_{10} t_{11} t_{12}]\left[ \begin{matrix} X \\ Y \\ Z \\1 \end{matrix} \right]\cdot u =0 $\\$ $\\$[t_5 t_6 t_7 t_8]\left[ \begin{matrix} X \\ Y \\ Z \\1 \end{matrix} \right] - [t_9 t_{10} t_{11} t_{12}]$\left[ \begin{matrix} X \\ Y \\ Z \\1 \end{matrix} \right]\cdot v =0

每個特徵點提供兩個方程,求解t_1-t_{12}需要6個點。

 

 

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章