SLAM入門之視覺里程計(4)：基礎矩陣的估計

在上篇文章中，介紹了三位場景中的同一個三維點在不同視角下的像點存在着一種約束關系：對極約束，基礎矩陣是這種約束關系的代數表示，並且這種約束關系獨立與場景的結構，只依賴與相機的內參和外參（相對位姿）。這樣可以通過通過匹配的像點對計算出兩幅圖像的基礎矩陣，然后分解基礎矩陣得到相機的相對位姿。

通過匹配點對估算基礎矩陣

基礎矩陣表示的是圖像中的像點\(p_1\)到另一幅圖像對極線\(l_2\)的映射，有如下公式：

\[l_2 = Fp_1 \]

而和像點\(P_1\)匹配的另一個像點\(p_2\)必定在對極線\(l_2\)上，所以就有：

\[p_2^Tl_2 = p_2^TFp_1 = 0 \]

這樣僅通過匹配的點對，就可以計算出兩視圖的基礎矩陣\(F\)。
基礎矩陣\(F\)是一個\(3\times3\)的矩陣，有9個未知元素。然而，上面的公式中使用的齊次坐標，齊次坐標在相差一個常數因子下是相等，\(F\)也就只有8個未知元素，也就是說，只需要8對匹配的點對就可以求解出兩視圖的基礎矩陣\(F\)。下面介紹下8點法 Eight-Point-Algorithm計算基礎矩陣的過程。

假設一對匹配的像點\(p_1=[u_1,v_1,1]^T,p_2=[u_2,v_2,1]^T\)，帶入式子中，得到：

\[[u_1,v_1,1]\left[\begin{array}{ccc}f_1&f_2&f_3\\f_4&f_5&f_6\\f_7&f_8&f_9\end{array}\right] \left[\begin{array}{c}u_2\\v_2\\1\end{array}\right] = 0 \]

把基礎矩陣\(F\)的各個元素當作一個向量處理

\[f = [f_1,f_2,f_3,f_4,f_5,f_6,f_7,f_8,f_9] \]

那么上面式子可以寫為

\[[u_1u_2,u_1v_2,u_1,v_1u_2,v_1v_2,v_1,u_2,v_2,1]\cdot f = 0 \]

對於其他的點對也使用同樣的表示方法。這樣將得到的所有方程放到一起，得到一個線性方程組(\((u^i,v^i)\)表示第\(i\)個特征點）

\[\left[ \begin{array}{ccccccccc} u_1^1u_2^1&u_1^1v_2^1&u_1^1&v_1^1u_2^1&v_1^1v_2^1&v_1^1&u_2^1&v_2^1&1\\ u_1^2u_2^2&u_1^2v_2^2&u_1^2&v_1^2u_2^2&v_1^2v_2^2&v_1^2&u_2^2&v_2^2&1\\ u_1^3u_2^3&u_1^3v_2^3&u_1^3&v_1^3u_2^1&v_1^3v_2^1&v_1^3&u_2^3&v_2^3&1\\ \cdots&\cdots&\cdots&\cdots&\cdots&\cdots&\cdots&\cdots&\cdots \\ u_1^8u_2^8&u_1^8v_2^8&u_1^8&v_1^8u_2^8&v_1^8v_2^8&v_1^8&u_2^8&v_2^8&1 \end{array} \right] \left[ \begin{array}{c} f_1\\f_2\\f_3\\f_4\\f_5\\f_6\\f_7\\f_8\\f_9 \end{array} \right]=0 \]

求解上面的方程組就可以得到基礎矩陣各個元素了。當然這只是理想中的情況，由於噪聲、數值的舍入誤差和錯誤的匹配點的影響，僅僅求解上面的線性方程組得到的基礎矩陣非常的不穩定，因此在8點法的基礎上有各種改進方法。

圖像坐標歸一化 Normalizing transformation

將上面公式中由匹配的點對坐標組成的矩陣記為系數矩陣\(A\)

\[Af = 0 \]

系數矩陣\(A\)是利用8點法求基礎矩陣的關鍵，所以Hartey就認為，利用8點法求基礎矩陣不穩定的一個主要原因就是原始的圖像像點坐標組成的系數矩陣\(A\)不好造成的，而造成\(A\)不好的原因是像點的齊次坐標各個分量的數量級相差太大。基於這個原因，Hartey提出一種改進的8點法，在應用8點法求基礎矩陣之前，先對像點坐標進行歸一化處理，即對原始的圖像坐標做同向性變換，這樣就可以減少噪聲的干擾，大大的提高8點法的精度。

預先對圖像坐標進行歸一化有以下好處：

• 能夠提高運算結果的精度
• 利用歸一化處理后的圖像坐標，對任何尺度縮放和原點的選擇是不變的。歸一化步驟預先為圖像坐標選擇了一個標准的坐標系中，消除了坐標變換對結果的影響。

歸一化操作分兩步進行，首先對每幅圖像中的坐標進行平移（每幅圖像的平移不同）使圖像中匹配的點組成的點集的形心（Centroid）移動到原點；接着對坐標系進行縮放是的點\(p=(x,y,w)^T\)中的各個分量總體上有一樣的平均值，各個坐標軸的縮放相同的，最后選擇合適的縮放因子使點\(p\)到原點的平均距離是\(\sqrt{2}\)。 概括起來變換過程如下：

1. 對點進行平移使其形心位於原點。
2. 對點進行縮放，使它們到原點的平均距離為\(\sqrt{2}\)
3. 對兩幅圖像獨立進行上述變換

上圖左邊是原始圖像的坐標，右邊是歸一化后的坐標，\(H\)是歸一化的變換矩陣，可記為如下形式：

\[H = S\left[\begin{array}{ccc} 1&0&-\bar{\mu} \\0&1&-\bar{\nu}\\0&0&\frac{1}{S} \end{array}\right] \]

其中，\(\bar{\mu},\bar{\nu}\)是圖像像點坐標兩個分量的平均值

\[\bar{\mu} = \frac{1}{N}\sum_{i=1}^{N}\mu_i,\bar{\nu} = \frac{1}{N}\sum_{i=1}^{N}\nu_i \]

\(S\)表示尺度，其表達式為：

\[S = \frac{\sqrt{2\cdot N}}{\sqrt{\sum_{i=1}^{N}(\mu_i-\bar{\mu})^2 + (\nu_i-\bar{\nu})^2}} \]

這樣，首先對原始的圖像坐標進行歸一化處理，再利用8點法求解基礎矩陣，最后將求得的結果解除歸一化，得到基礎矩陣\(F\)，總結如下：

1. 對圖像1進行歸一化處理，計算一個只包含平移和縮放的變換\(H_1\)，將圖像1中的匹配點集\({p_i^1}\)變換到新的點集\(\hat{p_i^1}\)，新點集的形心位於原點\((0,0)^T\)，並且它們到原點的平均距離是\(\sqrt{2}\)。
2. 對圖像2，計算變換矩陣\(H_2\)進行相同的歸一化處理
3. 使用8點法利用變換后的點集估計基礎矩陣\(\hat{F}\)
4. 根據變換\(F = H_2^T\hat{F}H_1\)

使用歸一化的坐標雖然能夠在一定程度上消除噪聲、錯誤匹配帶來的影響，但還是不夠的。

最小二乘法

僅僅對圖像坐標進行歸一化處理，能在一定程度上提高計算的精度，但在實踐中還是不夠的。8點法中，只使用8對匹配的點對估計基礎矩陣，但通常兩幅圖像的匹配的點對遠遠多於8對，可以利用更多匹配的點對來求解上面的方程。
由於基礎矩陣\(F\)在一個常量因子下是等價的，這樣可以給基礎矩陣\(F\)的元素組成的向量\(f\)施加一個約束條件

\[\parallel f \parallel = 1 \]

這樣由\(K \ge 8\)個匹配的點對，組合成一個矩陣\(Q_{K\times9}\)，求解上面方程就變成了求解如下問題的最小二乘解

\[\min_{\parallel f \parallel = 1}\parallel Qf \parallel ^2 \]

其中，矩陣\(Q\)的每一行來自一對匹配點；\(f\)是基礎矩陣\(F\)元素構成的待求解的向量，根據2-范數的定義

\[\parallel Qf \parallel^2 = (Qf)^T(Qf)=f^T(Q^TQ)f \]

將上式的中間部分提取出來得到矩陣\(M=Q^TQ\),這是一個\(9\times9\)的矩陣。基於拉格朗日-歐拉乘數優化定理，在\(\parallel f \parallel = 1\)約束下，\(Qf=0\)的最小二乘解，為矩陣\(M=Q^TQ\)的最小特征值對應的特征向量。所以可以對矩陣\(Q\)進行奇異值分解（SVD）

\[Q = U\Sigma V^T \]

其中，\(Q\)是\(K\times9\)的矩陣；\(U\)是\(K\times K\)的正交陣；\(\Sigma\)是\(K \times 9\)的對角矩陣，對角線的元素是奇異值；\(V^T\)是\(9 \times 9\)的正交陣，每一個列向量對應着\(\Sigma\)中的奇異值。所以，最小二乘解就是\(V^T\)的第9個列向量，也就是可由向量\(f=V_9\)構造基礎矩陣\(F\)。
基礎矩陣\(F\)還有一個重要的性質，這里可以作為進一步的約束條件。那就是基礎矩陣\(F\)的秩為2，

\[Rank(F) = 2 \]

上述使用列向量\(V_9\)構造的基礎矩陣的秩通常不為2，需要進一步的優化。在估計基礎矩陣時，設其最小奇異值為0，對上面方法取得的基礎矩陣進行SVD分解

\[F = SVD = S\left[\begin{array}{ccc} v_1&0&0\\0&v_2&0\\0&0&v_3 \end{array}\right]D \]

其最小特征值\(v_3\)被設為0，以使得\(F\)的秩為2.這樣得到

\[F = SVD = S\left[\begin{array}{ccc} v_1&0&0\\0&v_2&0\\0&0&0 \end{array}\right]D \]

上述\(F\)就是最終得到的基礎矩陣。

對上面進行總結，使用最小二乘法估算基礎矩陣的步驟如下：

1. 對取得兩幅圖像的匹配點進行歸一化處理，轉換矩陣分別是\(H_1,H_2\)
2. 有對應的匹配點(\(K\ge8\))構造系數矩陣\(Q\)
3. 對矩陣\(Q\)進行SVD分解,\(Q=U\Sigma V^T\)，由向量\(f=V_9\)構造矩陣\(\hat{F}\)
4. 對得到的矩陣\(\hat{F}\)進行秩為2的約束，即對\(\hat{F}\)進行SVD分解，\(\hat{F}=S\cdot diag(v_1,v_2,v_3) \cdot V^T\)，令\(v_3=0\)得到基礎矩陣的估計\(F'=S\cdot diag(v_1,v_2,0) \cdot V^T\)
5. 對上一步得到的解進行反歸一化處理，得到基礎矩陣\(F=H_2^TF'H_1\)

隨機采樣一致性 RANSAC

基於匹配點對估算兩視圖的基礎矩陣，唯一的已知條件就是匹配的點對坐標。在實踐中，點對的匹配肯定是存在誤差的，主要有兩種類型的誤差：

1. 不精確的測量點位置引起的系統誤差，通常服從高斯分布
2. 錯誤匹配引起的誤差，這些不匹配的點被稱為外點，通常不服從高斯分布

對於基礎矩陣的估算，不匹配的點能夠造成很大的誤差，即使是只有一對錯誤的匹配都能使估算值極大的偏離真實值。因此，需要找到一種方法，從包含錯誤點（外點）的匹配點對集合中，篩選出正確的匹配點（內點）。

RANSAC(Random Sample Consensus)隨機采樣一致性從一組含有外點的數據集中，通過迭代的方式估計出符合該數據集的數學模型的參數。因此，它也可以用來檢測出數據集中的外點。
RANSAC有兩個基本的假設：

• 數據集中包含的內點，內點的分布符合一個數學模型；而數據集中的外點不復合該數學模型
• 能夠一組內點（通常很少）集合能夠估計出其符合的數據模型

RANSAC的具體思想是：給定\(N\)個數據點組成的集合\(P\)，假設集合中大多數的點都是可以通過一個模型來產生的，且最少通過\(n\)個點可以擬合出模型的參數，則可以通過以下的迭代方式擬合該參數：

1. 從集合\(P\)中隨機選擇\(n\)個點
2. 使用這\(n\)個點估計出一個模型\(M\)
3. 對\(P\)中剩余的點，計算每個點與模型\(M\)的距離，距離超過閾值則認為是外點；不超過閾值則認為是內點，並記錄該模型\(M\)所對應的內點的個數\(m_i\)

將上面步驟重復\(k\)次，選擇最大\(m_i\)所對應的模型\(M\)作為最終結果。
在迭代次數\(k\)的選擇是一個關鍵，可以通過理論的方式計算出\(k\)的取值。在選擇\(n\)個點估計模型時，要保證選擇的\(n\)的個點都是內點的概率足夠的高。假設，從數據集中選擇一個點為內點的概率為\(\varpi\)，則選擇的\(n\)個點都是內點的概率為\(\varpi^n\);則\(1-\varpi^n\)表示選擇的\(n\)個點至少有一個是外點，用包含外點估算的模型顯然是不正確的，則迭代\(k\)次均得不到正確模型的概率為\((1-\varpi^n)^k\)。設\(p\)表示\(k\)次迭代中至少有一次選擇的點都是內點的概率，也就是估計出了正確的模型，則\(1-p\)就表示\(k\)次跌點都得到正確的模型，所以有

\[1-p = 1-\varpi^n)^k \]

兩邊同時取對數，則有

\[k = \frac{log(1-p)}{log(1-\varpi^n)} \]

一般要求\(p\)大於95%即可。

使用RANSAC估算基礎矩陣時，首先需要確定判斷點是內點還是外點的依據。通過上一篇的兩視圖的對極幾何可知，像點總是在對極線，因此可以選擇像點到對極線的距離作為判斷該點是內點還是外點的依據，設點到對極的距離的閾值為\(d\)。則使用RANSAC的方法估算基礎矩陣的步驟：

• 從匹配的點對中選擇8個點，使用8點法估算出基礎矩陣\(F_i\)
• 計算其余的點對到其對應對極線的距離\(d_n\)，如果\(d_n\le d\)則該點為內點，否則為外點。記下符合該條件的內點的個數為\(m_i\)
• 迭代k次，或者某次得到內點的數目\(m_i\)占有的比例大於等於95%，則停止。選擇\(m_i\)最大的基礎矩陣作為最終的結果。

RANSAC能夠不依賴於任何額外信息的情況下，將數據划分為內點和外點。但也有其相應的缺點，RANSAC並不能保證得到正確的結果，需要提高迭代的次數；另一個是，內點外點的判斷需要設置閾值。

OpenCV 計算基礎矩陣

上面寫了那么多基礎矩陣的計算方法，在OpenCV中也就是一個函數的封裝

Mat cv::findFundamentalMat(InputArray points1,
InputArray     points2,
int     method = FM_RANSAC,
double     param1 = 3.,
double     param2 = 0.99,
OutputArray     mask = noArray() 
)

其中，points1,points2是匹配點對的像素坐標，並且能夠一一對應。method表是使用那種方法，默認的是FM_RANSAC也就是RANSAC的方法估算基礎矩陣。param1表示RANSAC迭代過程中，判斷點是內點還是外點的閾值（到對極線的像素距離）；param2表示內點占的比例，以此來判斷估計出的基礎矩陣是否正確。

//////////////////////////////////////////////////////
    // 
    // 利用已經匹配的點對，使用RANSAC的方法，估計兩視圖的基礎矩陣
    //
    //////////////////////////////////////////////////////
    
    // 1. 對齊匹配的點對
    vector<KeyPoint> alignedKps1, alignedKps2;
    for (auto i = 0; i < good_matches.size(); i++)
    {
        alignedKps1.push_back(keypoints1[good_matches[i].queryIdx]);
        alignedKps2.push_back(keypoints2[good_matches[i].trainIdx]);
    }
    // 2. 取得特征點的像素坐標
    vector<Point2f> ps1, ps2;
    for (auto i = 0; i < alignedKps1.size(); i++)
    {
        ps1.push_back(alignedKps1[i].pt);
        ps2.push_back(alignedKps2[i].pt);
    }
    // 3. RANSAC 計算基礎矩陣F
    Mat F;
    F = findFundamentalMat(ps1, ps2, FM_RANSAC);
    cout << "基礎矩陣F:" << endl << F << endl;

首先需要將匹配的點對放在兩個數組中，並且一一對應（匹配的點在兩個數組中的下標是一樣的），然后再取得匹配點的像素坐標，最后調用findFundamentalMat使用RANSAC方法估計基礎矩陣。

通過兩視圖的對極幾何可知，所有的對極線相交於對極點，可以以此來驗證估計的基礎矩陣是否正確。在OpenCV中也封裝了計算對極線的方法computeCorrespondEpilines，

vector<Vec3f> linesl;
    computeCorrespondEpilines(ps1, 1, F, linesl);

    for (auto it = linesl.begin(); it != linesl.end(); it++)
    {
        line(img1, Point(0, -(*it)[2] / (*it)[1]), Point(img1.cols, -((*it)[2] + (*it)[0] * img1.cols) / (*it)[1]), Scalar(255, 255, 255));
    }
    imshow("第一幅圖像的對極線", img1);

    vector<Vec3f> lines2;
    computeCorrespondEpilines(ps2, 2, F, lines2);
    for (auto it = lines2.begin(); it != lines2.end(); it++)
    {
        line(img2, Point(0, -(*it)[2] / (*it)[1]), Point(img2.cols, -((*it)[2] + (*it)[0] * img2.cols) / (*it)[1]), Scalar(255, 255, 255));
    }
    imshow("第二幅圖像的對極線", img2);

上面代碼執行的結果如下：

可以看出所有的對極線都相交於一點，該點就是對極點。