ORB-SLAM3 細讀單目初始化過程(上)
來源：公眾號|計算機視覺工坊（系投稿）
作者：喬不思
「3D視覺工坊」技術(shù)交流群已經(jīng)成立，目前大約有12000人，方向主要涉及3D視覺、CV&深度學(xué)習(xí)、SLAM、三維重建、點云后處理、自動駕駛、CV入門、三維測量、VR/AR、3D人臉識別、醫(yī)療影像、缺陷檢測、行人重識別、目標跟蹤、視覺產(chǎn)品落地、視覺競賽、車牌識別、硬件選型、學(xué)術(shù)交流、求職交流、ORB-SLAM系列源碼交流、深度估計等。工坊致力于干貨輸出，不做搬運工，為計算機視覺領(lǐng)域貢獻自己的力量！歡迎大家一起交流成長~
添加小助手微信：CV_LAB，備注學(xué)校/公司+姓名+研究方向即可加入工坊一起學(xué)習(xí)進步。
QQ群「3D視覺研習(xí)社」，群號：574432628
學(xué)習(xí)ORB-SLAM3單目視覺SLAM中，發(fā)現(xiàn)有很多知識點需要展開和深入，同時又需要對系統(tǒng)有整體的認知，為了強化記憶，記錄該系列筆記，為自己圖方便，也希望對大家有所啟發(fā)。
因為知識有限，因此先記錄初始化過程中的重要節(jié)點，并非全部細節(jié)，如果需要看代碼的話，建議直接去看作者的源代碼ORB_SLAM3（https://github.com/UZ-SLAMLab/ORB_SLAM3）。
這是我自己稍微做了點修改，可以跑數(shù)據(jù)集的版本，可以參考一下。https://github.com/shanpenghui/ORB_SLAM3_Fixed
TrackMonocular是ORBSLAM單目視覺SLAM的追蹤器接口，因此從這里入手。其中GrabImageMonocular下?有2個主要的函數(shù)：Frame::Frame()和Tracking::Track()。我會按照下?的框架流程來分解單?初始化過程，以便對整個流程有?較清晰的認識。
1.Frame::Frame()
1)作用
主要完成工作是特征點提取，涉及到的知識點其實很多，包括圖像金字塔、特征點均勻化、四叉樹算法分發(fā)特征點、特征點方向計算等等
2)主要的三個函數(shù) ExtractORB UndistortKeyPoints AssignFeaturesToGrid
Frame()中其實調(diào)用的是ORBextractor::operator()，是一個重載操作符函數(shù)，此系列筆記主要針對重點理論如何落實到代碼上，不涉及編程技巧，因此不討論該函數(shù)的原理和實現(xiàn)，直接深入，探尋本質(zhì)。
對這個單目圖像進行提取特征點 Frame::ExtractORB
用OpenCV的矯正函數(shù)、內(nèi)參對提取到的特征點進行矯正 Frame::UndistortKeyPoints
將特征點分配到圖像網(wǎng)格中 Frame::AssignFeaturesToGrid
3)Frame::ExtractORB
3-1)作用
主要完成工作是提取圖像的ORB特征點和計算描述子
3-2)主要的函數(shù) ComputePyramid ComputeKeyPointsOctTree computeDescriptors
構(gòu)建圖像金字塔 ORBextractor::ComputePyramid
利用四叉樹算法均分特征點 ORBextractor::ComputeKeyPointsOctTree
計算某層金字塔圖像上特征點的描述子 static ORBextractor::computeDescriptors
3-3)構(gòu)建圖像金字塔 ComputePyramid
3-3-1圖像金字塔是什么東東？
首先，圖像金字塔的概念是: 圖像金字塔是圖像中多尺度表達的一種，是一種以多分辨率來解釋圖像的有效但概念簡單的結(jié)構(gòu)。如圖：
3-3-2代碼怎么實現(xiàn)圖像金字塔？
上面討論了搭建圖像金字塔，那怎么搭建呢？ORBSLAM3中，作者調(diào)用OpenCV的resize函數(shù)實現(xiàn)圖像縮放，構(gòu)建每層金字塔的圖像，在函數(shù)ORBextractor::ComputePyramid中。
resize(mvImagePyramid[level-1],?mvImagePyramid[level],?sz,?0,?0,?INTER_LINEAR);
3-3-3尺度不變性是什么東東？
我們搭建完金字塔了，但是有個問題，圖像的進行了縮放之后，假如要用同一個相機去看，則需要根據(jù)縮放的程度來調(diào)整相機到圖像的距離，來保持其觀測的一致性，這就是尺度不變性由來。
在ORB-SLAM3中，為了實現(xiàn)特征尺度不變性采用了圖像金字塔，金字塔的縮放因子為1.2。其思路就是對原始圖形（第0層）依次進行1/1.2縮放比例進行降采樣得到共計8張圖片（包括原始圖像），然后分別對得到的圖像進行特征提取，并記錄特征所在金字塔的第幾層，這樣得到一幀圖像的特征點，如圖1所示。
現(xiàn)在假設(shè)在第二層中有一特征點F，為了避免縮放帶來特征點F在縱向的移動，為簡化敘述，選擇的特征點F位于圖像中心，如圖2所示。根據(jù)相機成像“物近像大，物遠像小”的原理，如圖2所示為相機成像的示意圖。假設(shè)圖1中攝像機原始圖像即金字塔第0層對應(yīng)圖2中成像視野I0 ，則圖1中圖像金字塔第2層圖像可以相應(yīng)對應(yīng)于圖2中成像視野I2 。
有了以上鋪墊現(xiàn)在，再來說說，尺度不變性。簡單來說，因為圖像金字塔對圖像進行了縮放，假如要把該層的圖像特征點移到其他層上，就要對應(yīng)的放大圖像，同時相機與圖像的距離也要對應(yīng)著進行縮放，保證其尺度不變性。
3-3-4代碼哪里用到了尺度不變性？
3-3-4-1MapPoint::PredictScale
ORBSLAM3中，作者調(diào)用MapPoint::PredictScale函數(shù)，根據(jù)地圖點到光心的距離，來預(yù)測一個類似特征金字塔的尺度。
因為在進行投影匹配的時候會給定特征點的搜索范圍，由于考慮到處于不同尺度(也就是距離相機遠近,位于圖像金字塔中不同圖層)的特征點受到相機旋轉(zhuǎn)的影響不同，因此會希望距離相機近的點的搜索范圍更大一點,距離相機更遠的點的搜索范圍更小一點,所以要在這里,根據(jù)點到關(guān)鍵幀/幀的距離來估計它在當前的關(guān)鍵幀/幀中,會大概處于哪個尺度。
可以參考下圖示意：
ORB_SLAM3 MapPoint.cc 函數(shù) MapPoint::PredictScale Line 536 539
ratio?=?mfMaxDistance/currentDist;int?nScale?=?ceil(log(ratio)/pKF->mfLogScaleFactor);
3-3-4-2MapPoint::UpdateNormalAndDepth
ORBSLAM3中，作者調(diào)用MapPoint::UpdateNormalAndDepth函數(shù)，來更新平均觀測方向以及觀測距離范圍。由于一個MapPoint會被許多相機觀測到，因此在插入關(guān)鍵幀后，需要更新相應(yīng)變量，創(chuàng)建新的關(guān)鍵幀的時候會調(diào)用該函數(shù)。上面變量和代碼中的對應(yīng)關(guān)系是：
在ORB_SLAM3 MapPoint.cc 函數(shù) MapPoint::UpdateNormalAndDepth Line 490-491
//?觀測相機位置到該點的距離上限mfMaxDistance?=?dist*levelScaleFactor;??//?觀測相機位置到該點的距離下限mfMinDistance?=?mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];????
至此，構(gòu)建圖像金字塔 ComputePyramid記錄完畢，再來回顧一下，說到底，搭建圖像金字塔就是為了在不同尺度上來描述圖像，從而達到充分解釋圖像的目的。
3-4)四叉樹算法 ComputeKeyPointsOctTree
其實，代碼中，核心算法在ORBextractor::DistributeOctTree中實現(xiàn)的。先講原理吧。
3-4-1四叉樹是什么東東？
裝逼地說，啊不，專業(yè)地說，四叉樹或四元樹也被稱為Q樹（Q-Tree）。四叉樹廣泛應(yīng)用于圖像處理、空間數(shù)據(jù)索引、2D中的快速碰撞檢測、存儲稀疏數(shù)據(jù)等，而八叉樹（Octree）主要應(yīng)用于3D圖形處理。這里可能會有歧義，代碼中明明是Octree，不是八叉樹嗎？為什么這里講的是四叉樹原理呢？其實ORBSLAM里面是用四叉樹來均分特征點，后來有人用八叉樹來構(gòu)建和管理地圖，可能因為考慮到3D原因，作者在這里才把函數(shù)定義成OctTree，但實際用到的是四叉樹原理。
QuadTree四叉樹顧名思義就是樹狀的數(shù)據(jù)結(jié)構(gòu)，其每個節(jié)點有四個孩子節(jié)點，可將二維平面遞歸分割子區(qū)域。QuadTree常用于空間數(shù)據(jù)庫索引，3D的椎體可見區(qū)域裁剪，甚至圖片分析處理，我們今天介紹的是QuadTree最常被游戲領(lǐng)域使用到的碰撞檢測。采用QuadTree算法將大大減少需要測試碰撞的次數(shù)，從而提高游戲刷新性能。
不得不感慨，作者怎么懂那么多？大神就是大神，各種學(xué)科專業(yè)交叉融合，膜拜。GO ON。
四叉樹很簡單，就是把一塊2d的區(qū)域，等分成4份,如下圖: 我們把4塊區(qū)域從右上象限開始編號， 逆時針。
四叉樹起始于單節(jié)點。對象會被添加到四叉樹的單節(jié)點上。
當更多的對象被添加到四叉樹里時，它們最終會被分為四個子節(jié)點。（我是這么理解的：下面的圖片不是分為四個區(qū)域嗎，每個區(qū)域就是一個孩子或子節(jié)點）然后每個物體根據(jù)他在2D空間的位置而被放入這些子節(jié)點中的一個里。任何不能正好在一個節(jié)點區(qū)域內(nèi)的物體會被放在父節(jié)點。（這點我不是很理解，就這幅圖來說，那根節(jié)點的子節(jié)點豈不是有五個節(jié)點了。）
如果有更多的對象被添加進來，那么每個子節(jié)點要繼續(xù)劃分（成四個節(jié)點）。
大概也可以是這樣：
好了。概念普及完了。那在ORB-SLAM3中，它到底想干嘛呢？
3-4-2四叉樹用來干嘛？
ORB-SLAM中使用四叉樹來快速篩選特征點，篩選的目的是非極大值抑制，取局部特征點鄰域中FAST角點相應(yīng)值最大的點，而如何搜索到這些扎堆的特征點，則采用的是四叉樹的分快思想，遞歸找到成群的點，并從中找到相應(yīng)值最大的點。
3-4-3代碼怎么實現(xiàn)的？
在ORBextractor.cc 函數(shù) ORBextractor::DistributeOctTree
第一部分：
輸入圖像未分的關(guān)鍵點 對應(yīng)ORBextractor::DistributeOctTree函數(shù)中的形參vToDistributeKeysORBextractor.cc#L537
根據(jù)圖像區(qū)域構(gòu)造初始的根結(jié)點，每個根結(jié)點包含圖像的一個區(qū)域，每個根結(jié)點同樣包括4個子結(jié)點定義一個提取器 ExtractorNode ni;ORBextractor.cc#L552設(shè)置提取器節(jié)點的圖像邊界 ni.UL ni.UR ni.BL ni.BRORBextractor.cc#L552-L556 將剛才生成的提取節(jié)點添加到列表中l(wèi)Nodes.push_back(ni);ORBextractor.cc#L559 存儲這個初始的提取器節(jié)點句柄vpIniNodes[i] = &lNodes.back();ORBextractor.cc#L560
將未分的所有關(guān)鍵點分配給2中構(gòu)造的根結(jié)點，這樣每個根節(jié)點都包含了其所負責區(qū)域內(nèi)的所有關(guān)鍵點?按特征點的橫軸位置，分配給屬于那個圖像區(qū)域的提取器節(jié)點vpIniNodes[kp.pt.x/hX]->vKeys.push_back(vToDistributeKeys[i]);ORBextractor.cc#L567
根結(jié)點構(gòu)成一個根結(jié)點list，代碼中是lNodes用來更新與存儲所有的根結(jié)點 遍歷lNodes，標記不可再分的節(jié)點，用的標記變量是lit->bNoMoreORBextractor.cc#L576
第二部分
當列表中還有可分的結(jié)點區(qū)域的時候：while(!bFinish)ORBextractor.cc#L592
開始遍歷列表中所有的提取器節(jié)點，并進行分解或者保留：while(lit!=lNodes.end())ORBextractor.cc#L604
判斷當前根結(jié)點是否可分，可分的意思是，它包含的關(guān)鍵點能夠繼續(xù)分配到其所屬的四個子結(jié)點所在區(qū)域中（左上，右上，左下，右下），代碼中是判斷標志位if(lit->bNoMore)ORBextractor.cc#L606意思是如果當前的提取器節(jié)點具有超過一個的特征點，那么就要進行繼續(xù)細分
如果可分，將分出來的子結(jié)點作為新的根結(jié)點放入INodes的前部，e.g. lNodes.front().lit = lNodes.begin();ORBextractor.cc#L626，就是在四個if(n*.vKeys.size()>0)條件中執(zhí)行。然后將原先的根結(jié)點從列表中刪除，e.g.lit=lNodes.erase(lit);ORBextractor.cc#L660。由于新加入的結(jié)點是從列表頭加入的，不會影響這次的循環(huán)，該次循環(huán)只會處理當前級別的根結(jié)點。
當所有結(jié)點不可分，e.g(int)lNodes.size()==prevSizeORBextractor.cc#L667，或者結(jié)點已經(jīng)超過需要的點(int)lNodes.size()>=NORBextractor.cc#L667時，跳出循環(huán)bFinish = true;ORBextractor.cc#L669。
3-5)計算特征點描述子 computeDescriptors
3-5-1描述子是什么東東？
圖像的特征點可以簡單的理解為圖像中比較顯著顯著的點，如輪廓點，較暗區(qū)域中的亮點，較亮區(qū)域中的暗點等。
ORB采用的是哪種描述子呢？是用FAST（features from accelerated segment test）算法來檢測特征點。這個定義基于特征點周圍的圖像灰度值，檢測候選特征點周圍一圈的像素值，如果候選點周圍領(lǐng)域內(nèi)有足夠多的像素點與該候選點的灰度值差別夠大，則認為該候選點為一個特征點。
3-5-2計算特征描述子
利用上述步驟得到特征點后，我們需要以某種方式描述這些特征點的屬性。
這些屬性的輸出我們稱之為該特征點的描述子（Feature DescritorS）。
ORB采用BRIEF算法來計算一個特征點的描述子。BRIEF算法的核心思想是在關(guān)鍵點P的周圍以一定模式選取N個點對，把這N個點對的比較結(jié)果組合起來作為描述子。
如上圖所示，計算特征描述子的步驟分四步：
3-5-3如何保證描述子旋轉(zhuǎn)不變性?
在現(xiàn)實生活中，我們從不同的距離，不同的方向、角度，不同的光照條件下觀察一個物體時，物體的大小，形狀，明暗都會有所不同。但我們的大腦依然可以判斷它是同一件物體。理想的特征描述子應(yīng)該具備這些性質(zhì)。即，在大小、方向、明暗不同的圖像中，同一特征點應(yīng)具有足夠相似的描述子，稱之為描述子的可復(fù)現(xiàn)性。當以某種理想的方式分別計算描述子時，應(yīng)該得出同樣的結(jié)果。即描述子應(yīng)該對光照（亮度）不敏感，具備尺度一致性（大小 ），旋轉(zhuǎn)一致性（角度）等。
前面為了解決尺度一致性問題，采用了圖像金字塔來改善這方面的性能。而現(xiàn)在，主要解決BRIEF描述子不具備旋轉(zhuǎn)不變性的問題。
那我們?nèi)绾蝸斫鉀Q該問題呢？
在當前關(guān)鍵點P周圍以一定模式選取N個點對，組合這N個點對的T操作的結(jié)果就為最終的描述子。當我們選取點對的時候，是以當前關(guān)鍵點為原點，以水平方向為X軸，以垂直方向為Y軸建立坐標系。當圖片發(fā)生旋轉(zhuǎn)時，坐標系不變，同樣的取點模式取出來的點卻不一樣，計算得到的描述子也不一樣，這是不符合我們要求的。因此我們需要重新建立坐標系，使新的坐標系可以跟隨圖片的旋轉(zhuǎn)而旋轉(zhuǎn)。這樣我們以相同的取點模式取出來的點將具有一致性。
打個比方，我有一個印章，上面刻著一些直線。用這個印章在一張圖片上蓋一個章子，圖片上分處直線2頭的點將被取出來。印章不變動的情況下，轉(zhuǎn)動下圖片，再蓋一個章子，但這次取出來的點對就和之前的不一樣。為了使2次取出來的點一樣，我需要將章子也旋轉(zhuǎn)同一個角度再蓋章。（取點模式可以認為是章子上直線的分布情況）
ORB在計算BRIEF描述子時建立的坐標系是以關(guān)鍵點為圓心，以關(guān)鍵點P和取點區(qū)域的質(zhì)心Q的連線為X軸建立2維坐標系。P為關(guān)鍵點。圓內(nèi)為取點區(qū)域，每個小格子代表一個像素?，F(xiàn)在我們把這塊圓心區(qū)域看做一塊木板，木板上每個點的質(zhì)量等于其對應(yīng)的像素值。根據(jù)積分學(xué)的知識我們可以求出這個密度不均勻木板的質(zhì)心Q。
我們知道圓心是固定的而且隨著物體的旋轉(zhuǎn)而旋轉(zhuǎn)。當我們以PQ作為坐標軸時，在不同的旋轉(zhuǎn)角度下，我們以同一取點模式取出來的點是一致的。這就解決了旋轉(zhuǎn)一致性的問題。
3-5-4如何計算上面提到的質(zhì)心？灰度質(zhì)心法
說到解決該問題，這就不得不提到關(guān)鍵函數(shù)static IC_Angle ORBextractor.cc#L75 這個計算的方法是大家耳熟能詳?shù)幕叶荣|(zhì)心法：以幾何中心和灰度質(zhì)心的連線作為該特征點方向。具體是什么原理呢？
其實原理網(wǎng)上很多文章講解的很多了，我就直接貼上公式了。
3-5-5代碼如何實現(xiàn)？
首先取出關(guān)鍵點P。const uchar* centerORBextractor.cc#L79
因為實現(xiàn)中利用了一個技巧，就是同時計算圓對稱上下兩條線的和，這樣可以加速計算過程。所以計算中間的一條線上的點的和進行單獨處理。m_10 += u * center[u];ORBextractor.cc#L82
要在一個圖像塊區(qū)域HALF_PATCH_SIZE中循環(huán)計算得到圖像塊的矩，這里結(jié)合四叉樹算法，要明白在ORBSLAM中一個圖像塊區(qū)域的大小是30，而這里說過，用了一個技巧是同時計算兩條線，因此分一半，就是15，所以HALF_PATCH_SIZE=15
一條直線上的像素坐標開頭和結(jié)尾分別是-d和d，所以for (int u = -d; u <= d; ++u)ORBextractor.cc#L92
位于直線關(guān)鍵點P上方的像素點坐標是val_plus = center[u + v*step]ORBextractor.cc#L94
位于直線關(guān)鍵點P下方的像素點坐標是val_minus = center[u - v*step]ORBextractor.cc#L94
因為$m_{10}$只和X有關(guān)，像素坐標中對應(yīng)著u，所以$m_{10}$ = X坐標*像素值 = u * (val_plus+val_minus)
因為$m_{01}$只和Y有關(guān)，像素坐標中對應(yīng)著v，所以$m_{01}$ = Y坐標*像素值 = v * v_sum = v * (循環(huán)和(val_plus - val_minus))ORBextractor.cc#L95
3-5-6高斯模糊是什么？有什么用？怎么實現(xiàn)？
所謂”模糊”，可以理解成每一個像素都取周邊像素的平均值。圖中，2是中間點，周邊點都是1?！爸虚g點”取”周圍點”的平均值，就會變成1。在數(shù)值上，這是一種”平滑化”。在圖形上，就相當于產(chǎn)生”模糊”效果，”中間點”失去細節(jié)。
顯然，計算平均值時，取值范圍越大，”模糊效果”越強烈。
注意：提取特征點的時候，使用的是清晰的原圖像。這里計算描述子的時候，為了避免圖像噪聲的影響，使用了高斯模糊。
從數(shù)學(xué)的角度來看，高斯模糊的處理過程就是圖像與其正態(tài)分布做卷積。
正態(tài)分布
我們可以計算當前像素一定范圍內(nèi)的像素的權(quán)重，越靠近當前像素權(quán)重越大，形成一個符合正態(tài)分布的權(quán)重矩陣。
卷積
利用卷積算法，我們可以將當前像素的顏色與周圍像素的顏色按比例進行融合，得到一個相對均勻的顏色。
卷積核
卷積核一般為矩陣，我們可以將它想象成卷積過程中使用的模板，模板中包含了當前像素周圍每個像素顏色的權(quán)重。
有了這些基礎(chǔ)，我們再來看ORBSLAM到底怎么實現(xiàn)這個高斯模糊的？在代碼中，使用的是OpenCV的GaussianBlur函數(shù)。
對每層金字塔的圖像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怎么實現(xiàn)描述子的計算？
在《3-4-4-2計算特征描述子》說過，BRIEF算法的核心思想是在關(guān)鍵點P的周圍以一定模式選取N個點對，把這N個點對的比較結(jié)果組合起來作為描述子。
其描述子desc[i]為一個字節(jié)val8位，每一位是來自于兩個像素點灰度的直接比較：ORBextractor.cc#L124
????????t0?=?GET_VALUE(0);?t1?=?GET_VALUE(1);
????????val?=?t0?<?t1;???????//描述子本字節(jié)的bit0????????t0?=?GET_VALUE(2);?t1?=?GET_VALUE(3);
????????val?|=?(t0?<?t1)?<<?1;?????//描述子本字節(jié)的bit1????????t0?=?GET_VALUE(4);?t1?=?GET_VALUE(5);
????????val?|=?(t0?<?t1)?<<?2;?????//描述子本字節(jié)的bit2????????t0?=?GET_VALUE(6);?t1?=?GET_VALUE(7);
????????val?|=?(t0?<?t1)?<<?3;?????//描述子本字節(jié)的bit3????????t0?=?GET_VALUE(8);?t1?=?GET_VALUE(9);
????????val?|=?(t0?<?t1)?<<?4;?????//描述子本字節(jié)的bit4????????t0?=?GET_VALUE(10);?t1?=?GET_VALUE(11);
????????val?|=?(t0?<?t1)?<<?5;?????//描述子本字節(jié)的bit5????????t0?=?GET_VALUE(12);?t1?=?GET_VALUE(13);
????????val?|=?(t0?<?t1)?<<?6;?????//描述子本字節(jié)的bit6????????t0?=?GET_VALUE(14);?t1?=?GET_VALUE(15);
????????val?|=?(t0?<?t1)?<<?7;?????//描述子本字節(jié)的bit7
每比較出8bit結(jié)果，需要16個隨機點（參考《3-4-4-1描述子是什么東東？》）。ORBextractor.cc#L121
pattern?+=?16
其中，定義描述子是32個字節(jié)長，所以O(shè)RBSLAM的描述子一共是32*8=256位組成。
在《3-4-4-3如何保證描述子旋轉(zhuǎn)不變性?》中說過，ORBSLAM的描述子是帶旋轉(zhuǎn)不變性的，有些人評價說這可能也是ORB-SLAM的最大貢獻（知識有限，無法做評價，只是引入，無關(guān)對錯），這么重要的地方具體體現(xiàn)在代碼的哪里呢？作者定義了一共局部宏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如何保證描述子旋轉(zhuǎn)不變性?》，其實就是把灰度質(zhì)心法找到的質(zhì)心Q和特征點P就連成的直線PQ和坐標軸對齊，轉(zhuǎn)個角度，就是二維坐標系的旋轉(zhuǎn)公式：
3-6)總結(jié)
Frame::ExtractORB 主要完成工作是提取圖像的ORB特征點和計算描述子，其主要的函數(shù)分別是ComputePyramid、ComputeKeyPointsOctTree和computeDescriptors。
ComputePyramid函數(shù)主要完成了構(gòu)建圖像金字塔功能。ComputeKeyPointsOctTree函數(shù)使用四叉樹法對一個圖像金字塔圖層中的特征點進行平均和分發(fā)。computeDescriptors函數(shù)用來計算某層金字塔圖像上特征點的描述子。
至此，完成了圖像特征點的提取，并且將提取的關(guān)鍵點和描述子存放在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模型，感興趣的話可以下載數(shù)據(jù)集跑跑效果，具體方法可參考文章：
EVO Evaluation of SLAM 4 --- ORB-SLAM3 編譯和利用數(shù)據(jù)集運行（https://blog.csdn.net/shanpenghui/article/details/109354918）
其中，矯正就是用的Pinhole模型，就是針孔相機模型，在代碼中有體現(xiàn)Frame.cc#L751
cv::undistortPoints(mat,mat,?static_cast<Pinhole*>(mpCamera)->toK(),mDistCoef,cv::Mat(),mK);
我們針對針孔相機模型來討論一下。因為知識淺薄，所以想從基礎(chǔ)討論起，大神們可直接略過。
4-1為什么要矯正？
圖像成像模型
說到相機成像，就不得不說到初中物理，透視投影。
我們可以將透鏡的成像簡單地抽象成下圖所示：
畸變校正
理想的針孔成像模型確定的坐標變換關(guān)系均為線性的，而實際上，現(xiàn)實中使用的相機由于鏡頭中鏡片因為光線的通過產(chǎn)生的不規(guī)則的折射，鏡頭畸變（lens distortion）總是存在的，即根據(jù)理想針孔成像模型計算出來的像點坐標與實際坐標存在偏差?；兊囊胧沟贸上衲Ｐ椭械膸缀巫儞Q關(guān)系變?yōu)榉蔷€性，增加了模型的復(fù)雜度，但更接近真實情形?；儗?dǎo)致的成像失真可分為徑向失真和切向失真兩類：
徑向畸變（Radial Distortion）
簡單來說，由透鏡形狀（相機鏡頭徑向曲率的不規(guī)則變化）引起的畸變稱為徑向畸變，是導(dǎo)致相機成像變形的主要因素。徑向畸變主要分為桶形畸變和枕型畸變。在針孔模型中，一條直線投影到像素平面上還是一條直線。但在實際中，相機的透鏡往往使得真實環(huán)境中的一條直線在圖片中變成了曲線。越靠近圖像的邊緣現(xiàn)象越明顯。由于透鏡往往是中心對稱的，這使得不規(guī)則畸變通常徑向?qū)ΨQ。（成像中心處的徑向畸變最小，距離中心越遠，產(chǎn)生的變形越大，畸變也越明顯 ）
正向畸變（枕型畸變）：從圖像中心開始，徑向曲率逐漸增加。
負向畸變（桶形畸變）：邊緣的徑向曲率小于中心的徑向曲率。（魚眼相機）
實際攝像機的透鏡總是在成像儀的邊緣產(chǎn)生顯著的畸變，這種現(xiàn)象來源于“筒形”或“魚眼”的影響。如下圖，光線在原理透鏡中心的地方比靠近中心的地方更加彎曲。對于常用的普通透鏡來說，這種現(xiàn)象更加嚴重。筒形畸變在便宜的網(wǎng)絡(luò)攝像機中非常厲害，但在高端攝像機中不明顯，因為這些透鏡系統(tǒng)做了很多消除徑向畸變的工作。
切向畸變（Tangential Distortion）
切向畸變是由于相機鏡頭在制造安裝過程中并非完全平行于成像平面造成的。不同于徑向畸變在圖像中心徑向方向上發(fā)生偏移變形，切向畸變主要表現(xiàn)為圖像點相對理想成像點產(chǎn)生切向偏移。
4-2怎么矯正？
徑向畸變模型：r 為像平面坐標系中點(x, y)與圖像中心(x0, y0)的像素距離。
切向畸變模型可以描述為：$p_1$和$p_2$，鏡頭的切向畸變系數(shù)。
所以要想矯正圖像，最終需要得到的5個畸變參數(shù)：
我們來理一理矯正和不矯正坐標之間的關(guān)系。
4-3代碼怎么實現(xiàn)？
ORBSLAM中，用內(nèi)參對特征點去畸變。
首先判斷是否需要去畸變。
if(mDistCoef.at<float>(0)==0.0)
利用OpenCV的函數(shù)進行矯正
cv::undistortPoints(mat,mat,?static_cast<Pinhole*>(mpCamera)->toK(),mDistCoef,cv::Mat(),mK);
具體實現(xiàn)就不展開了，感興趣可以找OpenCV相關(guān)資料。
5)Frame::AssignFeaturesToGrid
將圖片分割為64*48大小的柵格，并將關(guān)鍵點按照位置分配到相應(yīng)柵格中，從而降低匹配時的復(fù)雜度，實現(xiàn)加速計算。舉個例子：
當我們需要在一條圖片上搜索特征點的時候，是按照grid搜索還是按照pixel搜索好？毫無疑問，先粗（grid）再細（pixel）搜索效率比較高。
這也是Frame::GetFeaturesInArea函數(shù)里面用的方法，變量mGrid聯(lián)系了 AssignFeaturesToGrid 的結(jié)果和其他函數(shù)：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這個結(jié)果在代碼中，后面的流程里，有幾個函數(shù)都要用：
SearchForInitialization?函數(shù)?單目初始化中用于參考幀和當前幀的特征點匹配
SearchByProjection?函數(shù)?通過投影地圖點到當前幀，對Local?MapPoint進行跟蹤
具體怎么分配呢？
先分配空間
mGrid[i][j].reserve(nReserve);
如果找到特征點所在網(wǎng)格坐標，將這個特征點的索引添加到對應(yīng)網(wǎng)格的數(shù)組mGrid中
if(PosInGrid(kp,nGridPosX,nGridPosY))
???mGrid[nGridPosX][nGridPosY].push_back(i);
6)總結(jié)
總而言之，F(xiàn)rame起到的是前端的作用，主要的作用完成對圖像特征點進行提取以及描述子的計算：
1. 通過構(gòu)建圖像金字塔，多尺度表達圖像，提高抗噪性；
2. 根據(jù)尺度不變性，計算相機與各圖層圖像的距離，準備之后的計算；
3. 利用四叉樹的快分思想，快速篩選特征點，避免特征點扎堆；
4. 利用灰度質(zhì)心法解決BRIEF描述子不具備旋轉(zhuǎn)不變性的問題，增強了描述子的魯棒性；
5. 利用相機模型，對提取的特征點進行畸變校正；
6. 最終通過大網(wǎng)格形式快速分配特征點，加速了運行速度。
以上僅是個人見解，如有紕漏望各位指出，謝謝。
參考：
1.數(shù)字圖像處理(21): 圖像金字塔(高斯金字塔 與 拉普拉斯金字塔)
2.ORB_SLAM2中特征提取之圖像金字塔尺度不變性理解
3.ORB-SLAM（一）：關(guān)于ORB-SLAM中四叉樹的使用
4.ORB-SLAM中的ORB特征（提?。?/p>
5.ORB-SLAM中四叉樹管理角點
6.高斯模糊（高斯濾波）的原理與算法
7.相機的那些事兒 (二)成像模型
8.【二】[詳細]針孔相機模型、相機鏡頭畸變模型、相機標定與OpenCV實現(xiàn)
9.全局視覺定位
備注：作者也是我們「3D視覺從入門到精通」特邀嘉賓：一個超干貨的3D視覺學(xué)習(xí)社區(qū)
本文僅做學(xué)術(shù)分享，如有侵權(quán)，請聯(lián)系刪文。
