ORB-SLAM3 細讀單目初始化過程(上)

阿新 • • 發佈：2021-01-07

作者：喬不思

來源：微信公眾號|3D視覺工坊（系投稿）

3D視覺精品文章彙總：https://github.com/qxiaofan/awesome-3D-Vision-Papers/

點選上方“3D視覺工坊”，選擇“星標”

乾貨第一時間送達

學習ORB-SLAM3單目視覺SLAM中，發現有很多知識點需要展開和深入，同時又需要對系統有整體的認知，為了強化記憶，記錄該系列筆記，為自己圖方便，也希望對大家有所啟發。

因為知識有限，因此先記錄初始化過程中的重要節點，並非全部細節，如果需要看程式碼的話，建議直接去看作者的原始碼ORB_SLAM3（https://github.com/UZ-SLAMLab/ORB_SLAM3）。

這是我自己稍微做了點修改，可以跑資料集的版本，可以參考一下。https://github.com/shanpenghui/ORB_SLAM3_Fixed

TrackMonocular是ORBSLAM單目視覺SLAM的追蹤器介面，因此從這裡入手。其中GrabImageMonocular下⾯有2個主要的函式：Frame::Frame()和Tracking::Track()。我會按照下⾯的框架流程來分解單⽬初始化過程，以便對整個流程有⽐較清晰的認識。

1.Frame::Frame()

1)作用

主要完成工作是特徵點提取，涉及到的知識點其實很多，包括影象金字塔、特徵點均勻化、四叉樹演算法分發特徵點、特徵點方向計算等等

2)主要的三個函式 ExtractORB UndistortKeyPoints AssignFeaturesToGrid

Frame()中其實呼叫的是ORBextractor::operator()，是一個過載操作符函式，此係列筆記主要針對重點理論如何落實到程式碼上，不涉及程式設計技巧，因此不討論該函式的原理和實現，直接深入，探尋本質。

對這個單目影象進行提取特徵點 Frame::ExtractORB

用OpenCV的矯正函式、內參對提取到的特徵點進行矯正 Frame::UndistortKeyPoints

將特徵點分配到影象網格中 Frame::AssignFeaturesToGrid

3)Frame::ExtractORB

3-1)作用

主要完成工作是提取影象的ORB特徵點和計算描述子

3-2)主要的函式 ComputePyramid ComputeKeyPointsOctTree computeDescriptors

構建影象金字塔 ORBextractor::ComputePyramid

利用四叉樹演算法均分特徵點 ORBextractor::ComputeKeyPointsOctTree

計算某層金字塔影象上特徵點的描述子 static ORBextractor::computeDescriptors

3-3)構建影象金字塔 ComputePyramid

3-3-1影象金字塔是什麼東東？

首先，影象金字塔的概念是: 影象金字塔是影象中多尺度表達的一種，是一種以多解析度來解釋影象的有效但概念簡單的結構。如圖：

3-3-2程式碼怎麼實現影象金字塔？

上面討論了搭建影象金字塔，那怎麼搭建呢？ORBSLAM3中，作者呼叫OpenCV的resize函式實現影象縮放，構建每層金字塔的影象，在函式ORBextractor::ComputePyramid中。

resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR);

3-3-3尺度不變性是什麼東東？

我們搭建完金字塔了，但是有個問題，影象的進行了縮放之後，假如要用同一個相機去看，則需要根據縮放的程度來調整相機到影象的距離，來保持其觀測的一致性，這就是尺度不變性由來。

在ORB-SLAM3中，為了實現特徵尺度不變性採用了影象金字塔，金字塔的縮放因子為1.2。其思路就是對原始圖形（第0層）依次進行1/1.2縮放比例進行降取樣得到共計8張圖片（包括原始影象），然後分別對得到的影象進行特徵提取，並記錄特徵所在金字塔的第幾層，這樣得到一幀影象的特徵點，如圖1所示。

現在假設在第二層中有一特徵點F，為了避免縮放帶來特徵點F在縱向的移動，為簡化敘述，選擇的特徵點F位於影象中心，如圖2所示。根據相機成像“物近像大，物遠像小”的原理，如圖2所示為相機成像的示意圖。假設圖1中攝像機原始影象即金字塔第0層對應圖2中成像視野I0 ，則圖1中影象金字塔第2層影象可以相應對應於圖2中成像視野I2 。

有了以上鋪墊現在，再來說說，尺度不變性。簡單來說，因為影象金字塔對影象進行了縮放，假如要把該層的影象特徵點移到其他層上，就要對應的放大影象，同時相機與影象的距離也要對應著進行縮放，保證其尺度不變性。

3-3-4程式碼哪裡用到了尺度不變性？

3-3-4-1MapPoint::PredictScale

ORBSLAM3中，作者呼叫MapPoint::PredictScale函式，根據地圖點到光心的距離，來預測一個類似特徵金字塔的尺度。

因為在進行投影匹配的時候會給定特徵點的搜尋範圍，由於考慮到處於不同尺度(也就是距離相機遠近,位於影象金字塔中不同圖層)的特徵點受到相機旋轉的影響不同，因此會希望距離相機近的點的搜尋範圍更大一點,距離相機更遠的點的搜尋範圍更小一點,所以要在這裡,根據點到關鍵幀/幀的距離來估計它在當前的關鍵幀/幀中,會大概處於哪個尺度。

可以參考下圖示意：

ORB_SLAM3 MapPoint.cc 函式 MapPoint::PredictScale Line 536 539

ratio = mfMaxDistance/currentDist;

int nScale = ceil(log(ratio)/pKF->mfLogScaleFactor);

3-3-4-2MapPoint::UpdateNormalAndDepth

ORBSLAM3中，作者呼叫MapPoint::UpdateNormalAndDepth函式，來更新平均觀測方向以及觀測距離範圍。由於一個MapPoint會被許多相機觀測到，因此在插入關鍵幀後，需要更新相應變數，建立新的關鍵幀的時候會呼叫該函式。上面變數和程式碼中的對應關係是：

在ORB_SLAM3 MapPoint.cc 函式 MapPoint::UpdateNormalAndDepth Line 490-491

// 觀測相機位置到該點的距離上限
mfMaxDistance = dist*levelScaleFactor;  
// 觀測相機位置到該點的距離下限
mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];

至此，構建影象金字塔 ComputePyramid記錄完畢，再來回顧一下，說到底，搭建影象金字塔就是為了在不同尺度上來描述影象，從而達到充分解釋影象的目的。

3-4)四叉樹演算法 ComputeKeyPointsOctTree

其實，程式碼中，核心演算法在ORBextractor::DistributeOctTree中實現的。先講原理吧。

3-4-1四叉樹是什麼東東？

裝逼地說，啊不，專業地說，四叉樹或四元樹也被稱為Q樹（Q-Tree）。四叉樹廣泛應用於影象處理、空間資料索引、2D中的快速碰撞檢測、儲存稀疏資料等，而八叉樹（Octree）主要應用於3D圖形處理。這裡可能會有歧義，程式碼中明明是Octree，不是八叉樹嗎？為什麼這裡講的是四叉樹原理呢？其實ORBSLAM裡面是用四叉樹來均分特徵點，後來有人用八叉樹來構建和管理地圖，可能因為考慮到3D原因，作者在這裡才把函式定義成OctTree，但實際用到的是四叉樹原理。

QuadTree四叉樹顧名思義就是樹狀的資料結構，其每個節點有四個孩子節點，可將二維平面遞迴分割子區域。QuadTree常用於空間資料庫索引，3D的椎體可見區域裁剪，甚至圖片分析處理，我們今天介紹的是QuadTree最常被遊戲領域使用到的碰撞檢測。採用QuadTree演算法將大大減少需要測試碰撞的次數，從而提高遊戲重新整理效能。

不得不感慨，作者怎麼懂那麼多？大神就是大神，各種學科專業交叉融合，膜拜。GO ON。

四叉樹很簡單，就是把一塊2d的區域，等分成4份,如下圖: 我們把4塊區域從右上象限開始編號，逆時針。

四叉樹起始於單節點。物件會被新增到四叉樹的單節點上。

當更多的物件被新增到四叉樹裡時，它們最終會被分為四個子節點。（我是這麼理解的：下面的圖片不是分為四個區域嗎，每個區域就是一個孩子或子節點）然後每個物體根據他在2D空間的位置而被放入這些子節點中的一個裡。任何不能正好在一個節點區域內的物體會被放在父節點。（這點我不是很理解，就這幅圖來說，那根節點的子節點豈不是有五個節點了。）

如果有更多的物件被新增進來，那麼每個子節點要繼續劃分（成四個節點）。

大概也可以是這樣：

好了。概念普及完了。那在ORB-SLAM3中，它到底想幹嘛呢？

3-4-2四叉樹用來幹嘛？

ORB-SLAM中使用四叉樹來快速篩選特徵點，篩選的目的是非極大值抑制，取區域性特徵點鄰域中FAST角點相應值最大的點，而如何搜尋到這些扎堆的特徵點，則採用的是四叉樹的分快思想，遞迴找到成群的點，並從中找到相應值最大的點。

3-4-3程式碼怎麼實現的？

在ORBextractor.cc 函式 ORBextractor::DistributeOctTree

第一部分：

輸入影象未分的關鍵點對應ORBextractor::DistributeOctTree函式中的形參vToDistributeKeysORBextractor.cc#L537
根據影象區域構造初始的根結點，每個根結點包含影象的一個區域，每個根結點同樣包括4個子結點定義一個提取器 ExtractorNode ni;ORBextractor.cc#L552設定提取器節點的影象邊界 ni.UL ni.UR ni.BL ni.BRORBextractor.cc#L552-L556 將剛才生成的提取節點新增到列表中lNodes.push_back(ni);ORBextractor.cc#L559 儲存這個初始的提取器節點控制代碼vpIniNodes[i] = &lNodes.back();ORBextractor.cc#L560
將未分的所有關鍵點分配給2中構造的根結點，這樣每個根節點都包含了其所負責區域內的所有關鍵點按特徵點的橫軸位置，分配給屬於那個影象區域的提取器節點vpIniNodes[kp.pt.x/hX]->vKeys.push_back(vToDistributeKeys[i]);ORBextractor.cc#L567
根結點構成一個根結點list，程式碼中是lNodes用來更新與儲存所有的根結點遍歷lNodes，標記不可再分的節點，用的標記變數是lit->bNoMoreORBextractor.cc#L576

第二部分

當列表中還有可分的結點區域的時候：while(!bFinish)ORBextractor.cc#L592
開始遍歷列表中所有的提取器節點，並進行分解或者保留：while(lit!=lNodes.end())ORBextractor.cc#L604
判斷當前根結點是否可分，可分的意思是，它包含的關鍵點能夠繼續分配到其所屬的四個子結點所在區域中（左上，右上，左下，右下），程式碼中是判斷標誌位if(lit->bNoMore)ORBextractor.cc#L606意思是如果當前的提取器節點具有超過一個的特徵點，那麼就要進行繼續細分
如果可分，將分出來的子結點作為新的根結點放入INodes的前部，e.g. lNodes.front().lit = lNodes.begin();ORBextractor.cc#L626，就是在四個if(n*.vKeys.size()>0)條件中執行。然後將原先的根結點從列表中刪除，e.g.lit=lNodes.erase(lit);ORBextractor.cc#L660。由於新加入的結點是從列表頭加入的，不會影響這次的迴圈，該次迴圈只會處理當前級別的根結點。
當所有結點不可分，e.g(int)lNodes.size()==prevSizeORBextractor.cc#L667，或者結點已經超過需要的點(int)lNodes.size()>=NORBextractor.cc#L667時，跳出迴圈bFinish = true;ORBextractor.cc#L669。

3-5)計算特徵點描述子 computeDescriptors

3-5-1描述子是什麼東東？

影象的特徵點可以簡單的理解為影象中比較顯著顯著的點，如輪廓點，較暗區域中的亮點，較亮區域中的暗點等。

ORB採用的是哪種描述子呢？是用FAST（features from accelerated segment test）演算法來檢測特徵點。這個定義基於特徵點周圍的影象灰度值，檢測候選特徵點周圍一圈的畫素值，如果候選點周圍領域內有足夠多的畫素點與該候選點的灰度值差別夠大，則認為該候選點為一個特徵點。

3-5-2計算特徵描述子

利用上述步驟得到特徵點後，我們需要以某種方式描述這些特徵點的屬性。

這些屬性的輸出我們稱之為該特徵點的描述子（Feature DescritorS）。

ORB採用BRIEF演算法來計算一個特徵點的描述子。BRIEF演算法的核心思想是在關鍵點P的周圍以一定模式選取N個點對，把這N個點對的比較結果組合起來作為描述子。

如上圖所示，計算特徵描述子的步驟分四步：

3-5-3如何保證描述子旋轉不變性?

在現實生活中，我們從不同的距離，不同的方向、角度，不同的光照條件下觀察一個物體時，物體的大小，形狀，明暗都會有所不同。但我們的大腦依然可以判斷它是同一件物體。理想的特徵描述子應該具備這些性質。即，在大小、方向、明暗不同的影象中，同一特徵點應具有足夠相似的描述子，稱之為描述子的可復現性。當以某種理想的方式分別計算描述子時，應該得出同樣的結果。即描述子應該對光照（亮度）不敏感，具備尺度一致性（大小），旋轉一致性（角度）等。

前面為了解決尺度一致性問題，採用了影象金字塔來改善這方面的效能。而現在，主要解決BRIEF描述子不具備旋轉不變性的問題。

那我們如何來解決該問題呢？

在當前關鍵點P周圍以一定模式選取N個點對，組合這N個點對的T操作的結果就為最終的描述子。當我們選取點對的時候，是以當前關鍵點為原點，以水平方向為X軸，以垂直方向為Y軸建立座標系。當圖片發生旋轉時，座標系不變，同樣的取點模式取出來的點卻不一樣，計算得到的描述子也不一樣，這是不符合我們要求的。因此我們需要重新建立座標系，使新的座標系可以跟隨圖片的旋轉而旋轉。這樣我們以相同的取點模式取出來的點將具有一致性。

打個比方，我有一個印章，上面刻著一些直線。用這個印章在一張圖片上蓋一個章子，圖片上分處直線2頭的點將被取出來。印章不變動的情況下，轉動下圖片，再蓋一個章子，但這次取出來的點對就和之前的不一樣。為了使2次取出來的點一樣，我需要將章子也旋轉同一個角度再蓋章。（取點模式可以認為是章子上直線的分佈情況）

ORB在計算BRIEF描述子時建立的座標系是以關鍵點為圓心，以關鍵點P和取點區域的質心Q的連線為X軸建立2維座標系。P為關鍵點。圓內為取點區域，每個小格子代表一個畫素。現在我們把這塊圓心區域看做一塊木板，木板上每個點的質量等於其對應的畫素值。根據積分學的知識我們可以求出這個密度不均勻木板的質心Q。

我們知道圓心是固定的而且隨著物體的旋轉而旋轉。當我們以PQ作為座標軸時，在不同的旋轉角度下，我們以同一取點模式取出來的點是一致的。這就解決了旋轉一致性的問題。

3-5-4如何計算上面提到的質心？灰度質心法

說到解決該問題，這就不得不提到關鍵函式static IC_Angle ORBextractor.cc#L75 這個計算的方法是大家耳熟能詳的灰度質心法：以幾何中心和灰度質心的連線作為該特徵點方向。具體是什麼原理呢？

其實原理網上很多文章講解的很多了，我就直接貼上公式了。

3-5-5程式碼如何實現？

首先取出關鍵點P。const uchar* centerORBextractor.cc#L79
因為實現中利用了一個技巧，就是同時計算圓對稱上下兩條線的和，這樣可以加速計算過程。所以計算中間的一條線上的點的和進行單獨處理。m_10 += u * center[u];ORBextractor.cc#L82
要在一個影象塊區域HALF_PATCH_SIZE中迴圈計算得到影象塊的矩，這裡結合四叉樹演算法，要明白在ORBSLAM中一個影象塊區域的大小是30，而這裡說過，用了一個技巧是同時計算兩條線，因此分一半，就是15，所以HALF_PATCH_SIZE=15
一條直線上的畫素座標開頭和結尾分別是-d和d，所以for (int u = -d; u <= d; ++u)ORBextractor.cc#L92
位於直線關鍵點P上方的畫素點座標是val_plus = center[u + v*step]ORBextractor.cc#L94
位於直線關鍵點P下方的畫素點座標是val_minus = center[u - v*step]ORBextractor.cc#L94
因為$m_{10}$只和X有關，畫素座標中對應著u，所以$m_{10}$ = X座標*畫素值 = u * (val_plus+val_minus)
因為$m_{01}$只和Y有關，畫素座標中對應著v，所以$m_{01}$ = Y座標*畫素值 = v * v_sum = v * (迴圈和(val_plus - val_minus))ORBextractor.cc#L95

3-5-6高斯模糊是什麼？有什麼用？怎麼實現？

所謂”模糊”，可以理解成每一個畫素都取周邊畫素的平均值。圖中，2是中間點，周邊點都是1。“中間點”取”周圍點”的平均值，就會變成1。在數值上，這是一種”平滑化”。在圖形上，就相當於產生”模糊”效果，”中間點”失去細節。

顯然，計算平均值時，取值範圍越大，”模糊效果”越強烈。

注意：提取特徵點的時候，使用的是清晰的原影象。這裡計算描述子的時候，為了避免影象噪聲的影響，使用了高斯模糊。

從數學的角度來看，高斯模糊的處理過程就是影象與其正態分佈做卷積。

正態分佈

我們可以計算當前畫素一定範圍內的畫素的權重，越靠近當前畫素權重越大，形成一個符合正態分佈的權重矩陣。

卷積

利用卷積演算法，我們可以將當前畫素的顏色與周圍畫素的顏色按比例進行融合，得到一個相對均勻的顏色。

卷積核

卷積核一般為矩陣，我們可以將它想象成卷積過程中使用的模板，模板中包含了當前畫素周圍每個畫素顏色的權重。

有了這些基礎，我們再來看ORBSLAM到底怎麼實現這個高斯模糊的？在程式碼中，使用的是OpenCV的GaussianBlur函式。

對每層金字塔的影象for (int level = 0; level < nlevels; ++level)ORBextractor.cc#L1105，ORBSLAM都進行高斯模糊：ORBextractor.cc#L1115

GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);

3-5-7怎麼實現描述子的計算？

在《3-4-4-2計算特徵描述子》說過，BRIEF演算法的核心思想是在關鍵點P的周圍以一定模式選取N個點對，把這N個點對的比較結果組合起來作為描述子。

其描述子desc[i]為一個位元組val8位，每一位是來自於兩個畫素點灰度的直接比較：ORBextractor.cc#L124

        t0 = GET_VALUE(0); t1 = GET_VALUE(1);
        val = t0 < t1;       //描述子本位元組的bit0
        t0 = GET_VALUE(2); t1 = GET_VALUE(3);
        val |= (t0 < t1) << 1;     //描述子本位元組的bit1
        t0 = GET_VALUE(4); t1 = GET_VALUE(5);
        val |= (t0 < t1) << 2;     //描述子本位元組的bit2
        t0 = GET_VALUE(6); t1 = GET_VALUE(7);
        val |= (t0 < t1) << 3;     //描述子本位元組的bit3
        t0 = GET_VALUE(8); t1 = GET_VALUE(9);
        val |= (t0 < t1) << 4;     //描述子本位元組的bit4
        t0 = GET_VALUE(10); t1 = GET_VALUE(11);
        val |= (t0 < t1) << 5;     //描述子本位元組的bit5
        t0 = GET_VALUE(12); t1 = GET_VALUE(13);
        val |= (t0 < t1) << 6;     //描述子本位元組的bit6
        t0 = GET_VALUE(14); t1 = GET_VALUE(15);
        val |= (t0 < t1) << 7;     //描述子本位元組的bit7

每比較出8bit結果，需要16個隨機點（參考《3-4-4-1描述子是什麼東東？》）。ORBextractor.cc#L121

pattern += 16

其中，定義描述子是32個位元組長，所以ORBSLAM的描述子一共是32*8=256位組成。

在《3-4-4-3如何保證描述子旋轉不變性?》中說過，ORBSLAM的描述子是帶旋轉不變性的，有些人評價說這可能也是ORB-SLAM的最大貢獻（知識有限，無法做評價，只是引入，無關對錯），這麼重要的地方具體體現在程式碼的哪裡呢？作者定義了一共區域性巨集ORBextractor.cc#L116

#define GET_VALUE(idx) center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + cvRound(pattern[idx].x*a - pattern[idx].y*b)]

其中，a = (float)cos(angle)和b = (float)sin(angle)

背後的原理呢？可能大家這麼多知識點看下來都懵逼了，我自己一次性梳理起來也很凌亂的。那就回顧一下《3-5-3如何保證描述子旋轉不變性?》，其實就是把灰度質心法找到的質心Q和特徵點P就連成的直線PQ和座標軸對齊，轉個角度，就是二維座標系的旋轉公式：

3-6)總結

Frame::ExtractORB 主要完成工作是提取影象的ORB特徵點和計算描述子，其主要的函式分別是ComputePyramid、ComputeKeyPointsOctTree和computeDescriptors。

ComputePyramid函式主要完成了構建影象金字塔功能。ComputeKeyPointsOctTree函式使用四叉樹法對一個影象金字塔圖層中的特徵點進行平均和分發。computeDescriptors函式用來計算某層金字塔影象上特徵點的描述子。

至此，完成了影象特徵點的提取，並且將提取的關鍵點和描述子存放在mvKeys和mDescriptors中。

4)Frame::UndistortKeyPoints

因為ORB-SLAM3中新增了虛擬相機的模型，論文中提及：

Our goal is to abstract the camera model from the whole SLAM pipeline by extracting all properties and functions related to the camera model (projection and unprojection functions, Jacobian, etc) to separate modules. This allows our system to use any camera model by providing the corresponding camera module.In ORB-SLAM3 library, apart from the pinhole model, we provide the Kannala-Brandt fisheye model.

其實跑TUM_VI的時候，就是用的KannalaBrandt8模型，感興趣的話可以下載資料集跑跑效果，具體方法可參考文章：

EVO Evaluation of SLAM 4 --- ORB-SLAM3 編譯和利用資料集執行（https://blog.csdn.net/shanpenghui/article/details/109354918）

其中，矯正就是用的Pinhole模型，就是針孔相機模型，在程式碼中有體現Frame.cc#L751

cv::undistortPoints(mat,mat, static_cast<Pinhole*>(mpCamera)->toK(),mDistCoef,cv::Mat(),mK);

我們針對針孔相機模型來討論一下。因為知識淺薄，所以想從基礎討論起，大神們可直接略過。

4-1為什麼要矯正？

影象成像模型

說到相機成像，就不得不說到初中物理，透視投影。

我們可以將透鏡的成像簡單地抽象成下圖所示：

畸變校正

理想的針孔成像模型確定的座標變換關係均為線性的，而實際上，現實中使用的相機由於鏡頭中鏡片因為光線的通過產生的不規則的折射，鏡頭畸變（lens distortion）總是存在的，即根據理想針孔成像模型計算出來的像點座標與實際座標存在偏差。畸變的引入使得成像模型中的幾何變換關係變為非線性，增加了模型的複雜度，但更接近真實情形。畸變導致的成像失真可分為徑向失真和切向失真兩類：

徑向畸變（Radial Distortion）

簡單來說，由透鏡形狀（相機鏡頭徑向曲率的不規則變化）引起的畸變稱為徑向畸變，是導致相機成像變形的主要因素。徑向畸變主要分為桶形畸變和枕型畸變。在針孔模型中，一條直線投影到畫素平面上還是一條直線。但在實際中，相機的透鏡往往使得真實環境中的一條直線在圖片中變成了曲線。越靠近影象的邊緣現象越明顯。由於透鏡往往是中心對稱的，這使得不規則畸變通常徑向對稱。（成像中心處的徑向畸變最小，距離中心越遠，產生的變形越大，畸變也越明顯）

正向畸變（枕型畸變）：從影象中心開始，徑向曲率逐漸增加。
負向畸變（桶形畸變）：邊緣的徑向曲率小於中心的徑向曲率。（魚眼相機）

實際攝像機的透鏡總是在成像儀的邊緣產生顯著的畸變，這種現象來源於“筒形”或“魚眼”的影響。如下圖，光線在原理透鏡中心的地方比靠近中心的地方更加彎曲。對於常用的普通透鏡來說，這種現象更加嚴重。筒形畸變在便宜的網路攝像機中非常厲害，但在高階攝像機中不明顯，因為這些透鏡系統做了很多消除徑向畸變的工作。

切向畸變（Tangential Distortion）

切向畸變是由於相機鏡頭在製造安裝過程中並非完全平行於成像平面造成的。不同於徑向畸變在影象中心徑向方向上發生偏移變形，切向畸變主要表現為影象點相對理想成像點產生切向偏移。

4-2怎麼矯正？

徑向畸變模型：r 為像平面座標系中點(x, y)與影象中心(x0, y0)的畫素距離。

切向畸變模型可以描述為：$p_1$和$p_2$，鏡頭的切向畸變係數。

所以要想矯正影象，最終需要得到的5個畸變引數：

我們來理一理矯正和不矯正座標之間的關係。

4-3程式碼怎麼實現？

ORBSLAM中，用內參對特徵點去畸變。

首先判斷是否需要去畸變。

if(mDistCoef.at<float>(0)==0.0)

利用OpenCV的函式進行矯正

cv::undistortPoints(mat,mat, static_cast<Pinhole*>(mpCamera)->toK(),mDistCoef,cv::Mat(),mK);

具體實現就不展開了，感興趣可以找OpenCV相關資料。

5)Frame::AssignFeaturesToGrid

將圖片分割為64*48大小的柵格，並將關鍵點按照位置分配到相應柵格中，從而降低匹配時的複雜度，實現加速計算。舉個例子：

當我們需要在一條圖片上搜索特徵點的時候，是按照grid搜尋還是按照pixel搜尋好？毫無疑問，先粗（grid）再細（pixel）搜尋效率比較高。

這也是Frame::GetFeaturesInArea函式裡面用的方法，變數mGrid聯絡了 AssignFeaturesToGrid 的結果和其他函式：Frame.cc#L676

for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
    {
        for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
        {
            const vector<size_t> vCell = (!bRight) ? mGrid[ix][iy] : mGridRight[ix][iy];

而mGrid這個結果在程式碼中，後面的流程裡，有幾個函式都要用：

SearchForInitialization 函式 單目初始化中用於參考幀和當前幀的特徵點匹配
SearchByProjection 函式 通過投影地圖點到當前幀，對Local MapPoint進行跟蹤

具體怎麼分配呢？

先分配空間

mGrid[i][j].reserve(nReserve);

如果找到特徵點所在網格座標，將這個特徵點的索引新增到對應網格的陣列mGrid中

if(PosInGrid(kp,nGridPosX,nGridPosY))
   mGrid[nGridPosX][nGridPosY].push_back(i);

6)總結

總而言之，Frame起到的是前端的作用，主要的作用完成對影象特徵點進行提取以及描述子的計算：

1. 通過構建影象金字塔，多尺度表達影象，提高抗噪性；

2. 根據尺度不變性，計算相機與各圖層影象的距離，準備之後的計算；

3. 利用四叉樹的快分思想，快速篩選特徵點，避免特徵點扎堆；

4. 利用灰度質心法解決BRIEF描述子不具備旋轉不變性的問題，增強了描述子的魯棒性；

5. 利用相機模型，對提取的特徵點進行畸變校正；

6. 最終通過大網格形式快速分配特徵點，加速了執行速度。

以上僅是個人見解，如有紕漏望各位指出，謝謝。

參考：

1.數字影象處理(21): 影象金字塔(高斯金字塔與拉普拉斯金字塔)

2.ORB_SLAM2中特徵提取之影象金字塔尺度不變性理解

3.ORB-SLAM（一）：關於ORB-SLAM中四叉樹的使用

4.ORB-SLAM中的ORB特徵（提取）

5.ORB-SLAM中四叉樹管理角點

6.高斯模糊（高斯濾波）的原理與演算法

7.相機的那些事兒 (二)成像模型

8.【二】[詳細]針孔相機模型、相機鏡頭畸變模型、相機標定與OpenCV實現

9.全域性視覺定位