SLAM入門之視覺里程計(3)：兩視圖對極約束 基礎矩陣

在上篇相機模型中介紹了圖像的成像過程，場景中的三維點通過“小孔”映射到二維的圖像平面，可以使用下面公式描述：

\[x = MX $$其中，$c$是圖像中的像點，$M$是一個$3\times4$的相機矩陣，$X$是場景中的三維點。 通過小孔相機模型，可知假如從像點$x$向相機的中心$C$反投影一條射線$\overrightarrow{xC}$，則該射線必定經過對應像點的三維空間點$X$，但顯然僅僅通過一個像點無法確定$X$的具體位置，因為在射線$\overrightarrow{xC}$上的任意空間點都可以通過“小孔”映射為像點$x$。一個像點不行，那么如果兩個相匹配的像點呢？ 設$x'$是三維點$X$的另一個像點，其對應相機的中心為$C'$，那么從像點也反投影一條射線$\overrightarrow{x'c'}$，並且該射線也必定經過$X$，也就是說從一對相匹配的像點反投影兩條射線，必定相交於空間三維點$X$，如下圖  很顯然，一對匹配的像點之間存在這某種約束關系，這種約束被稱為**兩視圖的對極約束**。 本文主要介紹了兩視圖的對極約束-基礎矩陣，並用幾何和代數兩種形式推導基礎矩陣的表達形式以及從基礎矩陣可以分解出相機的位姿（旋轉和平移）。 ### 對極幾何 對極幾何描述的是兩視圖之間的內存射影關系，同一個三維點在兩個不同的視角下的像點存在着約束關系，如下圖三維點$X$在兩幅圖像的像點分分別為$x,x'$  對極幾何主要涉及以下幾個元素： - 基線（Baseline），兩個相機中心的連線$CC'$稱為基線 - 對極點 epipolar ，$e,e'$是對極點，是基線和兩個成像平面的交點，也就是兩個相機在另一個成像平面的像點；$e$是右邊的相機中心$C'$在左邊相機的像點，同樣$e'$是左邊相機中心$C$在右邊相機的像點。 - 對極平面 epipolar plane，任何過基線的平面都被才稱為對極平面，兩個相機的中心$C$和$C'$，三維點$X$，以及其在兩個相機的像點$x,x'$，這5點必定在同一個對極平面上。當三維點$X$變化時，對極平面繞着基線旋轉，形成對極平面束。 - 對極線 epipolar line ,對極平面和成像平面的交線，所有的對極線相交於極點。 從上面的幾何元素可知，對極幾何和相機的內參、外參沒有關系，和場景的結構也沒有關系，**僅和場景的一對匹配的像點有關系**。 ### 基礎矩陣F 給定一對圖像（同一場景不同視角得到的圖像），從上面的圖可知，對於第一幅圖像上的任一像點$x$，在第二幅圖像中都有一條與之對應的對極線$l'$，該對極線是像點$x$與過第一個相機中心$C$射線在第二幅圖像上的投影，並且第二幅圖像中與$x$相匹配的像點$x'$必定在該對極線上。因此，存在一個像點$x$到另一個圖像上對極線$l'$的映射： $$x \rightarrow l'\]

基礎矩陣\(F\)表示的就是這種從點到直線的映射。

F的幾何推導

該推導來自《計算機視覺中的多視圖幾何》。

要將一幅圖像上的像點\(x\)映射到另一幅圖像對應的對極線\(l'\)可以分為兩步：第一步，將像點\(x\)映射到另一幅圖像上與之對應的對極線\(l'\)上的某點\(x'\)上，\(x'\)是\(x\)的匹配點；第二步，連接對極點\(e'\)與點\(x'\)得到的直線就是對極線\(l'\)。

• 點通過平面轉移 如下圖，平面\(\pi\)不通過兩相機中心，過第一個相機的中心\(C\)和像點\(x\)的射線與\(\pi\)相交於點\(X\)。該點\(X\)再投影到第二幅圖像上得到像點\(x'\),這個過程稱為點通過平面的轉移。
  
  點\(X\)位於像點\(x\)和相機中心確定的射線上，其在另一幅圖像上的像點\(x'\)必然位於該射線在另一幅圖像的投影也就是對極線\(l'\)上。點\(x\)和點\(x'\)都是三維點\(X\)的像點，這樣第一副圖像上的像點集合\(x_i\)和第二幅圖像上的像點集合\(x'_i\)是射影等價的，它們都射影等價於共面的三維點集合\(X_i\)。因此，存在一個2D單應\(H_{\pi}\)，把每一個點\(x\)映射到對應的點\(x'\)上。

2D單應\(H_{\pi}\)指的是將射影平面上的點集\(x_i\)映射到另一個射影平面的點集\(x'_i\)上的射影變換，在實際情況中\(x_i\)和\(x'_i\)通常是兩幅圖像上的像點，每幅圖像都被視為一個射影平面。

• 構造對極線 給點點\(x'\)，通過\(x'\)和對極點\(e'\)的對極線\(l'\)可以表示為：$$l'=e'\times x'=[e']{\times}\cdot x'$$.
  又由於\(x'=H_{\pi}x\)（\(H_{\pi}\)是將\(x\)變換為\(x'\)的單應）,帶入上式可得： $$l'=[e']{\times}H_{\pi} x$$
  定義$$F=[e']{\times}H{\pi}$$
  這樣就得到了從點\(x\)到對極線\(l'\)的變換$$l'=Fx$$
  以上就是基礎矩陣\(F\)的推導過程。

• 基礎矩陣\(F\)可以記為:\(F=[e']_{\times}H_{\pi}\),其中\(H_{\pi}\)是一幅圖像上像點到另一幅圖像上的2D單應，\([e']_{\times}\)是對極點\(e'\)的反對稱矩陣。由於\([e']_{\times}\)秩為2，\(H_{\pi}\)的秩為3，基礎矩陣\(F\)的秩為2.

• 對極約束
  由於\(l'=Fx\)，而像點\(x'\)在對極線\(l'\)上，故可得到

\[(x')^Tl'=(x')^TFx=0 \]

上式被稱為對極約束，僅通過匹配的點對就可以求出兩視圖的基礎矩陣\(F\)。

向量的叉積，反對稱矩陣\([e']_{\times}\) 在射影幾何中，一條直線有兩點確定，如上面對極線有像點\(x'\)和對極點\(e'\)確定，並且可以表示為這兩個點的叉積形式，有\(l'=e'\times x'\)。
設有兩個向量\(\overrightarrow{a},\overrightarrow{b}\)，則這兩個向量的叉積仍然是一個向量，並且垂直於\(\overrightarrow{a},\overrightarrow{b}\)所在的平面。其坐標表示如下：

\[\overrightarrow{a} \times \overrightarrow{b} = \left[\begin{array}{ccc}0&-z_1&y_1\\z_1&0&-x_1\\-y_1&x_1&0\end{array}\right] \left[\begin{array}{c}x_2\\y_2\\z_2\end{array}\right]=(\overrightarrow{a}_{\times})\cdot \overrightarrow{b} \]

其中\(\overrightarrow{a}_{\times}\)稱為向量\(\overrightarrow{a}\)的反對稱矩陣

\[\overrightarrow{a}_{\times}=\left[\begin{array}{ccc}0&-z_1&y_1\\z_1&0&-x_1\\-y_1&x_1&0\end{array}\right] \]

通過引入反對稱矩陣，向量的叉積可以使用矩陣乘法的表示，形式非常簡潔。

F的代數推導

該推導來自《視覺SLAM十四講》

設以第一個相機作為坐標系三維空間的點\(P=[X,Y,Z]\)，其在兩個相機的像點分別為\(p_1,p_2\)。由於第一個相機的中心作為世界坐標系的原點，也就是說第一個相機沒有旋轉和平移，通過小孔相機模型可得：

\[p_1 = KP，p_2=K(RP+t) \]

其中,\(K\)是相機的內參，\(R,t\)是第二個相機相對於第一個相機的旋轉和平移。
從\(p_1 = KP\)可以得到\(P=K^{-1}p_1\)，帶入第二個式子可得到：

\[p_2=K(RK^{-1}p_1 + t) \]

兩邊同時左乘\(K^{-1}\)，得到

\[K^{-1}p_2=RK^{-1}p_1 + t \]

設\(x_1=K^{-1}p_1,x_2=K^{-1}p_2\)，代入

\[x_2=Rx_1+t \]

兩邊同時左乘向量\(t\)的反對稱矩陣\(t_{\times}\),由於\(t_{\times}t=0\)，消除\(t\)

\[t_{\times}x_2=t_{\times}Rx_1 \]

兩邊再同時左乘\(x_2^T\)

\[x_2^Tt_{\times}x_2=x_2^Tt_{\times}Rx_1 \]

由於\(t_{\times}x_2\)是向量\(t\)和向量\(x_2\)的叉積，同時垂直於向量\(t\)和向量\(x_2\)，所以左邊\(x_2^Tt_{\times}x_2=0\)，得到

\[x_2^Tt_{\times}Rx_1=0 \]

再將\(x_1,x_2\)換掉

\[p_2^TK^{-T}t_{\times}RK^{-1}p_1=0 \]

上式是對極約束的另一種表示，該式子中僅包含像點，相機的旋轉和平移，中間的矩陣就是基礎矩陣\(F\)。

\[p_2^TFp_1 = 0 , 其中 F=K^{-T}t_{\times}RK^{-1} \]

式子\(F=K^{-T}t_{\times}RK^{-1}\)，可知假如相機的內參數\(K\)是已知的，提取中間的矩陣可到

\[E = t_{\times}R \]

E被稱為本質矩陣，其和基礎矩陣相差相機的內參\(K\)。

通過匹配的點對計算相機的位姿

通過上面的知道，對於匹配的像點\(p_1,p_2\)以及基礎矩陣\(F\)有如下關系：

\[p_2^TFp_1 = 0 , 其中 F=K^{-T}t_{\times}RK^{-1} \]

也就是說，僅通過匹配的點對（最少7對）可以計算出兩視圖的基礎矩陣\(F\)，然后再從\(F\)中分解得到相機的位姿勢。

相機的相對位姿可以通過特征點匹配估計出來：

• 提取兩幅圖像的特征點，並進行匹配
• 利用匹配得像點計算兩視圖的基礎矩陣\(F\)
• 從基礎矩陣\(F\)中分解得到相機的選擇矩陣\(R\)和平移向量\(t\)