SLAM入門之視覺里程計(1)：特徵點的匹配

SLAM 主要分為兩個部分：前端和後端，前端也就是視覺里程計（VO），它根據相鄰影象的資訊粗略的估計出相機的運動，給後端提供較好的初始值。VO的實現方法可以根據是否需要提取特徵分為兩類：基於特徵點的方法，不使用特徵點的直接方法。 基於特徵點的VO執行穩定，對光照、動態物體不敏感。

影象特徵點的提取和匹配是計算機視覺中的一個基本問題，在視覺SLAM中就需要首先找到相鄰影象對應點的組合，根據這些匹配的點對計算出相機的位姿（相對初始位置，相機的旋轉和平移）。
本文對這段時間對特徵點的學習做一個總結，主要有以下幾方面的內容：

• 特徵點概述
• 常用的特徵點演算法，如SIFT,SURF，FAST等
• OpenCV3中特徵點的提取和匹配

特徵點概述

如何高效且準確的匹配出兩個不同視角的影象中的同一個物體，是許多計算機視覺應用中的第一步。雖然影象在計算機中是以灰度矩陣的形式存在的，但是利用影象的灰度並不能準確的找出兩幅影象中的同一個物體。這是由於灰度受光照的影響，並且當影象視角變化後，同一個物體的灰度值也會跟著變化。所以，就需要找出一種能夠在相機進行移動和旋轉（視角發生變化），仍然能夠保持不變的特徵，利用這些不變的特徵來找出不同視角的影象中的同一個物體。

為了能夠更好的進行影象匹配，需要在影象中選擇具有代表性的區域，例如：影象中的角點、邊緣和一些區塊，但在影象識別出角點是最容易，也就是說角點的辨識度是最高的。所以，在很多的計算機視覺處理中，都是提取交掉作為特徵，對影象進行匹配，例如SFM，視覺SLAM等。

但是，單純的角點並不能很好的滿足我們的需求，例如：相機從遠處得到的是角點，但是在近處就可能不是角點；或者，當相機旋轉後，角點就發生了變化。為此，計算機視覺的研究者們設計了許多更為穩定的的特徵點，這些特徵點不會隨著相機的移動，旋轉或者光照的變化而變化。例如：SIFT,SURF,ORB等

一個影象的特徵點由兩部分構成：關鍵點（Keypoint）和描述子（Descriptor）。 關鍵點指的是該特徵點在影象中的位置，有些還具有方向、尺度資訊；描述子通常是一個向量，按照人為的設計的方式，描述關鍵點周圍畫素的資訊。通常描述子是按照外觀相似的特徵應該有相似的描述子設計的。因此，在匹配的時候，只要兩個特徵點的描述子在向量空間的距離相近，就可以認為它們是同一個特徵點。

特徵點的匹配通常需要以下三個步驟：

• 提取影象中的關鍵點，這部分是查詢影象中具有某些特徵（不同的演算法有不同的）的畫素
• 根據得到的關鍵點位置，計算特徵點的描述子
• 根據特徵點的描述子，進行匹配

這裡先介紹下特徵點的描述子，一個好的描述子是準確匹配的基礎，關鍵點的提取和特徵點的匹配，在後面介紹。

特徵點描述子

從影象中提取到特徵的關鍵點資訊，通常只是其在影象的位置資訊（有可能包含尺度和方向資訊），僅僅利用這些資訊無法很好的進行特徵點的匹配，所以就需要更詳細的資訊，將特徵區分開來，這就是特徵描述子。另外，通過特徵描述子可以消除視角的變化帶來影象的尺度和方向的變化，能夠更好的在影象間匹配。

特徵的描述子通常是一個精心設計的向量，描述了關鍵點及其周圍畫素的資訊。為了能夠更好的匹配，一個好的描述子通常要具有以下特性：

• 不變性 指特徵不會隨著影象的放大縮小旋轉而改變。
• 魯棒性 對噪聲、光照或者其他一些小的形變不敏感
• 可區分性 每一個特徵描述子都是獨特的，具有排他性，儘可能減少彼此間的相似性。

其中描述子的可區分性和其不變性是矛盾的，一個具有眾多不變性的特徵描述子，其區分區域性影象內容的能力就比較稍弱；而如果一個很容易區分不同區域性影象內容的特徵描述子，其魯棒性往往比較低。所以，在設計特徵描述子的時候，就需要綜合考慮這三個特性，找到三者之間的平衡。

特徵描述子的不變性主要體現在兩個方面：

• 尺度不變性 Scale Invarient
  指的是同一個特徵，在影象的不同的尺度空間保持不變。匹配在不同影象中的同一個特徵點經常會有影象的尺度問題，不同尺度的影象中特徵點的距離變得不同，物體的尺寸變得不同，而僅僅改變特徵點的大小就有可能造成強度不匹配。如果描述子無法保證尺度不變性，那麼同一個特徵點在放大或者縮小的影象間，就不能很好的匹配。為了保持尺度的不變性，在計算特徵點的描述子的時候，通常將影象變換到統一的尺度空間，再加上尺度因子。
• 旋轉不變性 Rotation Invarient
  指的是同一個特徵，在成像視角旋轉後，特徵仍然能夠保持不變。和尺度不變性類似，為了保持旋轉不變性，在計算特徵點描述子的時候，要加上關鍵點的方向資訊。

為了有個更直觀的理解，下面給出SIFT,SURF,BRIEF描述子計算方法對比

從上表可以看出，SIFT,SURF和BRIEF描述子都是一個向量，只是維度不同。其中，SIFT和SURF在構建特徵描述子的時候，儲存了特徵的方向和尺度特徵，這樣其特徵描述子就具有尺度和旋轉不變性；而BRIEF描述子並沒有尺度和方向特徵，不具備尺度和旋轉不變性。

常用的特徵點演算法

上面提到影象的特徵點包含兩個部分：

• 特徵點的提取，在影象檢測到特徵點的位置
• 特徵點的描述，也就是描述子。

在影象中提取到關鍵點的位置資訊後，為了能夠更有效的匹配（主要是保證尺度和旋轉不變性），通常使用一個向量來描述關鍵點及其周圍的資訊。特徵的描述子，在特徵點的匹配中是非常重要的，上一小節中對其應該具有的性質做了介紹。但具體到一個演算法來說，可能其既有特徵點的提取演算法也有特徵點描述子的演算法，也有可能其僅僅是一個特徵點提取演算法或者是特徵點的描述子演算法。在本小節就常用的特徵點演算法做一個簡要的說明。

SIFT

提到特徵點演算法，首先就是大名鼎鼎的SIFT演算法了。SIFT的全稱是Scale Invariant Feature Transform，尺度不變特徵變換，2004年由加拿大教授David G.Lowe提出的。SIFT特徵對旋轉、尺度縮放、亮度變化等保持不變性，是一種非常穩定的區域性特徵。
SIFT演算法主要有以下幾個步驟：

• 高斯差分金字塔的構建
  使用組和層的結構構建了一個具有線性關係的金字塔（尺度空間），這樣可以在連續的高斯核尺度上查詢影象的特徵點；另外，它使用一階的高斯差分來近似高斯的拉普拉斯核，大大的減少了運算量。
• 尺度空間的極值檢測及特徵點的定位
  搜尋上一步建立的高斯尺度空間，通過高斯差分來識別潛在的對尺度和旋轉不變的特徵點。但是，在離散空間中，區域性極值點可能並不是真正意義的極值點，真正的極值點有可能落在離散點的間隙中，SIFT通過尺度空間DoG函式進行曲線擬合尋找極值點。
• 特徵方向賦值
  基於影象區域性的梯度方向，分配給每個關鍵點位置一個或多個方向，後續的所有操作都是對於關鍵點的方向、尺度和位置進行變換，從而提供這些特徵的不變性。
• 特徵描述子的生成
  通過上面的步驟已經找到的SIFT特徵點的位置、方向、尺度資訊，最後使用一組向量來描述特徵點及其周圍鄰域畫素的資訊。

SIFT演算法中及包含了特徵點的提取演算法，也有如何生成描述子的演算法，更進一步的SIFT演算法介紹可參看SIFT特徵詳解

SURF

SURF全稱 Speeded Up Robust Features，是在SIFT演算法的基礎上提出的，主要針對SIFT演算法運算速度慢，計算量大的缺點進行了改進。
SURF的流程和SIFT比較類似，這些改進體現在以下幾個方面：

• 特徵點檢測是基於Hessian矩陣，依據Hessian矩陣行列式的極值來定位特徵點的位置。並且將Hession特徵計算與高斯平滑結合在一起，兩個操作通過近似處理得到一個核模板。
• 在構建尺度空間時，使用box filter與源影象卷積，而不是使用DoG運算元。
• SURF使用一階Haar小波在x、y兩個方向的響應作為構建特徵向量的分佈資訊。

FAST特徵點提取演算法

SIFT和SURF是非常好的，穩定的特徵點演算法，但運算速度是其一大弊端，無法做到實時的特徵提取和匹配，其應用就有了很大的侷限性。FAST特徵提取演算法彌補了這一侷限，檢測區域性畫素灰度變化明顯的地方，以速度快而著稱，其全稱為：Features From Accelerated Segment Test。在FAST演算法的思想很簡單：如果一個畫素與周圍鄰域的畫素差別較大（過亮或者過暗），那麼可以認為該畫素是一個角點。和其他的特徵點提取演算法相比，FAST演算法只需要比較畫素和其鄰域畫素的灰度值大小，十分便捷。
FAST演算法提取角點的步驟：

• 在影象中選擇畫素p，假設其灰度值為：\(I_p\)
• 設定一個閾值T，例如：\(I_p\)的20%
• 選擇p周圍半徑為3的圓上的16個畫素，作為比較畫素
• 假設選取的圓上有連續的N個畫素大於\(I_p + T\)或者\(I_p - T\)，那麼可以認為畫素p就是一個特徵點。（N通常取12，即為FAST-12；常用的還有FAST-9,FAST-11）。

FAST演算法只檢測畫素的灰度值，其運算速度極快，同時不可避免的也有一些缺點

• 檢測到的特徵點過多並且會出現“扎堆”的現象。這可以在第一遍檢測完成後，使用非最大值抑制（Non-maximal suppression），在一定區域內僅保留響應極大值的角點，避免角點集中的情況。
• FAST提取到的角點沒有方向和尺度資訊

上面的介紹的SIFT和SURF演算法都包含有各自的特徵點描述子的計算方法，而FAST不包含特徵點描述子的計算，僅僅只有特徵點的提取方法，這就需要一個特徵點描述方法來描述FAST提取到的特徵點，以方便特徵點的匹配。下面介紹一個專門的特徵點描述子的計算演算法。

BRIEF描述子

BRIEF是一種二進位制的描述子，其描述向量是0和1表示的二進位制串。0和1表示特徵點鄰域內兩個畫素（p和q）灰度值的大小：如果p比q大則選擇1，反正就取0。在特徵點的周圍選擇128對這樣的p和q的畫素對，就得到了128維由0，1組成的向量。那麼p和q的畫素對是怎麼選擇的呢？通常都是按照某種概率來隨機的挑選畫素對的位置。
BRIEF使用隨機選點的比較，速度很快，而且使用二進位制串表示最終生成的描述子向量，在儲存以及用於匹配的比較時都是非常方便的，其和FAST的搭配起來可以組成非常快速的特徵點提取和描述演算法。

ORB演算法

ORB的全稱是Oriented FAST and Rotated BRIEF，是目前來說非常好的能夠進行的實時的影象特徵提取和描述的演算法，它改進了FAST特徵提取演算法，並使用速度極快的二進位制描述子BRIEF。
針對FAST特徵提取的演算法的一些確定，ORB也做了相應的改進。

• 使用非最大值抑制，在一定區域內僅僅保留響應極大值的角點，避免FAST提取到的角點過於集中。
• FAST提取到的角點數量過多且不是很穩定，ORB中可以指定需要提取到的角點的數量N，然後對FAST提取到的角點分別計算Harris響應值，選擇前N個具有最大響應值的角點作為最終提取到的特徵點集合。
• FAST提取到的角點不具有尺度資訊，在ORB中使用影象金字塔，並且在每一層金字塔上檢測角點，以此來保持尺度的不變性。
• FAST提取到的角點不具有方向資訊，在ORB中使用灰度質心法(Intensity Centroid)來保持特徵的旋轉不變性。

OpenCV3中特徵點的提取和匹配

OpenCV中封裝了常用的特徵點演算法（如SIFT,SURF，ORB等），提供了統一的介面，便於呼叫。 下面程式碼是OpenCV中使用其feature 2D 模組的示例程式碼

    Mat img1 = imread("F:\\image\\1.png");
    Mat img2 = imread("F:\\image\\2.png");

    // 1. 初始化
    vector<KeyPoint> keypoints1, keypoints2;
    Mat descriptors1, descriptors2;
    Ptr<ORB> orb = ORB::create();

    // 2. 提取特徵點
    orb->detect(img1, keypoints1);
    orb->detect(img2, keypoints2);

    // 3. 計算特徵描述符
    orb->compute(img1, keypoints1, descriptors1);
    orb->compute(img2, keypoints2, descriptors2);
    
    // 4. 對兩幅影象的BRIEF描述符進行匹配，使用BFMatch，Hamming距離作為參考
    vector<DMatch> matches;
    BFMatcher bfMatcher(NORM_HAMMING);
    bfMatcher.match(descriptors1, descriptors2, matches);

1. 獲取檢測器的例項
   在OpenCV3中重新的封裝了特徵提取的介面，可統一的使用Ptr<FeatureDetector> detector = FeatureDetector::create()來得到特徵提取器的一個例項，所有的引數都提供了預設值，也可以根據具體的需要傳入相應的引數。
2. 在得到特徵檢測器的例項後，可呼叫的detect方法檢測影象中的特徵點的具體位置，檢測的結果儲存在vector<KeyPoint>向量中。
3. 有了特徵點的位置後，呼叫compute方法來計算特徵點的描述子，描述子通常是一個向量，儲存在Mat中。
4. 得到了描述子後，可呼叫匹配演算法進行特徵點的匹配。上面程式碼中，使用了opencv中封裝後的暴力匹配演算法BFMatcher，該演算法在向量空間中，將特徵點的描述子一一比較，選擇距離（上面程式碼中使用的是Hamming距離）較小的一對作為匹配點。

上面程式碼匹配後的結果如下：

特徵點的匹配後的優化

特徵的匹配是針對特徵描述子的進行的，上面提到特徵描述子通常是一個向量，兩個特徵描述子的之間的距離可以反應出其相似的程度，也就是這兩個特徵點是不是同一個。根據描述子的不同，可以選擇不同的距離度量。如果是浮點型別的描述子，可以使用其歐式距離；對於二進位制的描述子（BRIEF）可以使用其漢明距離（兩個不同二進位制之間的漢明距離指的是兩個二進位制串不同位的個數）。

有了計算描述子相似度的方法，那麼在特徵點的集合中如何尋找和其最相似的特徵點，這就是特徵點的匹配了。最簡單直觀的方法就是上面使用的：暴力匹配方法(Brute-Froce Matcher)，計算某一個特徵點描述子與其他所有特徵點描述子之間的距離，然後將得到的距離進行排序，取距離最近的一個作為匹配點。這種方法簡單粗暴，其結果也是顯而易見的，通過上面的匹配結果，也可以看出有大量的錯誤匹配，這就需要使用一些機制來過濾掉錯誤的匹配。

• 漢明距離小於最小距離的兩倍
  選擇已經匹配的點對的漢明距離小於最小距離的兩倍作為判斷依據，如果小於該值則認為是一個錯誤的匹配，過濾掉；大於該值則認為是一個正確的匹配。其實現程式碼也很簡單，如下：

    // 匹配對篩選
    double min_dist = 1000, max_dist = 0;
    // 找出所有匹配之間的最大值和最小值
    for (int i = 0; i < descriptors1.rows; i++)
    {
        double dist = matches[i].distance;
        if (dist < min_dist) min_dist = dist;
        if (dist > max_dist) max_dist = dist;
    }
    // 當描述子之間的匹配大於2倍的最小距離時，即認為該匹配是一個錯誤的匹配。
    // 但有時描述子之間的最小距離非常小，可以設定一個經驗值作為下限
    vector<DMatch> good_matches;
    for (int i = 0; i < descriptors1.rows; i++)
    {
        if (matches[i].distance <= max(2 * min_dist, 30.0))
            good_matches.push_back(matches[i]);
    }

結果如下：

對比只是用暴力匹配的方法，進行過濾後的匹配效果好了很多。

• 交叉匹配
  針對暴力匹配，可以使用交叉匹配的方法來過濾錯誤的匹配。交叉過濾的是想很簡單，再進行一次匹配，反過來使用被匹配到的點進行匹配，如果匹配到的仍然是第一次匹配的點的話，就認為這是一個正確的匹配。舉例來說就是，假如第一次特徵點A使用暴力匹配的方法，匹配到的特徵點是特徵點B；反過來，使用特徵點B進行匹配，如果匹配到的仍然是特徵點A，則就認為這是一個正確的匹配，否則就是一個錯誤的匹配。OpenCV中BFMatcher已經封裝了該方法，建立BFMatcher的例項時，第二個引數傳入true即可，BFMatcher bfMatcher(NORM_HAMMING,true)。

• KNN匹配
  K近鄰匹配，在匹配的時候選擇K個和特徵點最相似的點，如果這K個點之間的區別足夠大，則選擇最相似的那個點作為匹配點，通常選擇K = 2，也就是最近鄰匹配。對每個匹配返回兩個最近鄰的匹配，如果第一匹配和第二匹配距離比率足夠大（向量距離足夠遠），則認為這是一個正確的匹配，比率的閾值通常在2左右。
  OpenCV中的匹配器中封裝了該方法，上面的程式碼可以呼叫bfMatcher->knnMatch(descriptors1, descriptors2, knnMatches, 2);具體實現的程式碼如下：

    const float minRatio = 1.f / 1.5f;
    const int k = 2;

    vector<vector<DMatch>> knnMatches;
    matcher->knnMatch(leftPattern->descriptors, rightPattern->descriptors, knnMatches, k);

    for (size_t i = 0; i < knnMatches.size(); i++) {
        const DMatch& bestMatch = knnMatches[i][0];
        const DMatch& betterMatch = knnMatches[i][1];

        float  distanceRatio = bestMatch.distance / betterMatch.distance;
        if (distanceRatio < minRatio)
            matches.push_back(bestMatch);
    }const  float minRatio =  1.f  /  1.5f;
    const  int k =  2;

    vector<vector<DMatch>> knnMatches;
    matcher->knnMatch(leftPattern->descriptors, rightPattern->descriptors, knnMatches, 2);

    for (size_t i =  0; i < knnMatches.size(); i++) {
        const DMatch& bestMatch = knnMatches[i][0];
        const DMatch& betterMatch = knnMatches[i][1];
        float distanceRatio = bestMatch.distance  / betterMatch.distance;
        if (distanceRatio < minRatio)
            matches.push_back(bestMatch);
    }

將不滿足的最近鄰的匹配之間距離比率大於設定的閾值（1/1.5）匹配剔除。

• RANSAC
  另外還可採用隨機取樣一致性（RANSAC）來過濾掉錯誤的匹配，該方法利用匹配點計算兩個影象之間單應矩陣，然後利用重投影誤差來判定某一個匹配是不是正確的匹配。OpenCV中封裝了求解單應矩陣的方法findHomography,可以為該方法設定一個重投影誤差的閾值，可以得到一個向量mask來指定那些是符合該重投影誤差的匹配點對，以此來剔除錯誤的匹配，程式碼如下：

const int minNumbermatchesAllowed = 8;
    if (matches.size() < minNumbermatchesAllowed)
        return;

    //Prepare data for findHomography
    vector<Point2f> srcPoints(matches.size());
    vector<Point2f> dstPoints(matches.size());

    for (size_t i = 0; i < matches.size(); i++) {
        srcPoints[i] = rightPattern->keypoints[matches[i].trainIdx].pt;
        dstPoints[i] = leftPattern->keypoints[matches[i].queryIdx].pt;
    }

    //find homography matrix and get inliers mask
    vector<uchar> inliersMask(srcPoints.size());
    homography = findHomography(srcPoints, dstPoints, CV_FM_RANSAC, reprojectionThreshold, inliersMask);

    vector<DMatch> inliers;
    for (size_t i = 0; i < inliersMask.size(); i++){
        if (inliersMask[i])
            inliers.push_back(matches[i]);
    }
    matches.swap(inliers);const  int minNumbermatchesAllowed =  8;
    if (matches.size() < minNumbermatchesAllowed)
        return;

    //Prepare data for findHomography
    vector<Point2f>  srcPoints(matches.size());
    vector<Point2f>  dstPoints(matches.size());

    for (size_t i =  0; i < matches.size(); i++) {
        srcPoints[i] = rightPattern->keypoints[matches[i].trainIdx].pt;
        dstPoints[i] = leftPattern->keypoints[matches[i].queryIdx].pt;
    }

    //find homography matrix and get inliers mask
    vector<uchar>  inliersMask(srcPoints.size());
    homography =  findHomography(srcPoints, dstPoints, CV_FM_RANSAC, reprojectionThreshold, inliersMask);

    vector<DMatch> inliers;
    for (size_t i =  0; i < inliersMask.size(); i++){
        if (inliersMask[i])
            inliers.push_back(matches[i]);
    }
    matches.swap(inliers);

之前寫過一篇OpenCV的特徵點匹配及一些剔除錯誤匹配的文章，OpenCV2:特徵匹配及其優化，使用的是OpenCV2，在OpenCV3中更新了特徵點檢測和匹配的介面，不過大體還是差不多的。上一篇的文末附有練習程式碼的下載連結，不要直接開啟sln或者project檔案，有可能vs版本不一樣打不開，本文的測試程式碼還沒有整理，等有時間好好打理下github，練習的程式碼隨手都丟了，到想用的時候又找不到了。

後記

翻了下，上一篇部落格還是6月30號釋出的，而今已是12月底，半年6個月時間就這樣過去了。而我，好像沒有什麼成長啊，工資還是那麼多，除錯bug的技術倒是積累了很多，知道多執行緒程式除錯;多程式通訊;學會了用Windebug:分析dump檔案，在無程式碼環境中attach到執行檔案中分析問題或者拿著pdb檔案和原始碼在現場環境中進行除錯...;實實在在的感受到了C++的記憶體洩漏和空指標導致的各種奇葩問題；知道了使用未初始化的變數的不穩定性；知道了專案設計中擴充套件性的重要的...
寫之前覺得自己虛度了半年，總結下來，這半年下來時間還是成長了不少的，心裡的愧疚感降低了不少。不過以後還是要堅持寫部落格記錄下學習的過程...