0. 簡(jiǎn)介

視覺里程計(jì)最近幾年越來越受到學(xué)術(shù)界以及工業(yè)界的認(rèn)可，以ORB和VINS為代表的視覺SLAM已經(jīng)可以滿足絕大多數(shù)場(chǎng)景，而OV2SLAM在其他VSLAM中脫穎而出，其實(shí)時(shí)性以及具體的回環(huán)性能在測(cè)試中都得到了認(rèn)可。

1.OV2SLAM與ORB-SLAM2的對(duì)比

下圖為OV2SLAM的示意圖

下圖為ORB-SLAM2示意圖

下面是兩個(gè)算法的對(duì)比

我們從上表中可以看到相比于ORB-SLAM2而言，OV2SLAM有以下四個(gè)主要的區(qū)別點(diǎn)

1.放棄了ORB算子，采用了LK光流來代替。這樣可以大大的節(jié)省前端特征點(diǎn)提取的時(shí)間

2.在輸出實(shí)時(shí)pose的時(shí)候，ORBSLAM2精度比OV2SLAM高，相比于ORBSLAM2使用TrackLocalMap來進(jìn)行motion-only的BA而言，OV2SLAM輸出的實(shí)時(shí)pose是通過PnP計(jì)算出來的。雖然OV2SLAM也有TrackLocalMap,但是他這一步僅進(jìn)行特征點(diǎn)的re-track, 沒有進(jìn)行位姿計(jì)算。

3.在回環(huán)檢測(cè)部分ORBSLAM2使用的是DBOW 離線訓(xùn)練字典，而OV2SLAM使用的是iBowLCD 在線構(gòu)建字典，這就導(dǎo)致OV2SLAM可以適應(yīng)更多的場(chǎng)景

4.最后也是最關(guān)鍵的就是全局位姿輸出，ORBSLAM2使用的是FullBA 優(yōu)化來優(yōu)化全部關(guān)鍵幀的位姿和3D點(diǎn)，而OV2SLAM使用的是looselyBA，這個(gè)優(yōu)化方式僅優(yōu)化當(dāng)前幀和回環(huán)幀關(guān)聯(lián)的信息。所以使用FullBA 可以影響到包括losselyBA以及其余關(guān)鍵幀的pose和3D點(diǎn)。

2 視覺前端

這部分內(nèi)容其實(shí)都大同小異，值得一提的是：

圖像預(yù)處理（image pre-processing ）關(guān)鍵點(diǎn)追蹤（keypoint tracking）離群值濾波（outlier filtering）位姿估計(jì)（pose estimation）觸發(fā)條件，創(chuàng)建關(guān)鍵幀（keyframe creation triggering）

下面我們來簡(jiǎn)述一下具體流程：

1.圖像預(yù)處理：通過CLAHE進(jìn)行對(duì)比度增強(qiáng)，這既增加了動(dòng)態(tài)范圍，又限制了曝光適應(yīng)引起的強(qiáng)度變化。

2.關(guān)鍵點(diǎn)追蹤：關(guān)鍵點(diǎn)跟蹤是通過引導(dǎo)的從粗到精的光流法來執(zhí)行的。使用具有9×9像素窗口和金字塔比例因子為2的反向組合Lucas-Kanade(LK)算法[12]的金字塔實(shí)現(xiàn)來單獨(dú)跟蹤關(guān)鍵點(diǎn)。

這里提到2D點(diǎn)和3D點(diǎn)這兩種，對(duì)于3D關(guān)鍵點(diǎn)(即已經(jīng)三角化的3D關(guān)鍵點(diǎn))，會(huì)先使用勻速運(yùn)動(dòng)模型估計(jì)當(dāng)前幀的初始位姿，然后將3D keyoints對(duì)應(yīng)的3D點(diǎn)投影到當(dāng)前幀，作為該3D keypoints在當(dāng)前幀的初始位置。

對(duì)于2D關(guān)鍵點(diǎn)-即那些沒有關(guān)于其真實(shí)3D位置的先驗(yàn)信息的關(guān)鍵點(diǎn)，會(huì)將其在當(dāng)前幀的初始位置設(shè)置為在上一幀中的位置

3.離群值濾波：采用基于核線約束的RANSAC濾波，使用3D關(guān)鍵點(diǎn)估計(jì)基礎(chǔ)矩陣（Essential Matrix）然后用它來過濾不一致的2D關(guān)鍵點(diǎn)。

4.位姿估計(jì)：使用穩(wěn)健的Huber成本函數(shù)最小化3D關(guān)鍵點(diǎn)重投影誤差來執(zhí)行姿態(tài)估計(jì)，也就是我們上文提到的PnP計(jì)算位姿并更新運(yùn)動(dòng)模型

5.創(chuàng)建關(guān)鍵幀：跟蹤的3D關(guān)鍵點(diǎn)的數(shù)量w.r.t.。最后一個(gè)關(guān)鍵幀低于閾值(跟蹤的關(guān)鍵點(diǎn)少于85%)，或者如果檢測(cè)到明顯的視差(平均15個(gè)像素的未旋轉(zhuǎn)關(guān)鍵點(diǎn)運(yùn)動(dòng))，則會(huì)創(chuàng)建新的關(guān)鍵幀。

值得一提的是OV2SLAM將每張圖像分為nbwcells*nbhcells個(gè)網(wǎng)格，是基于網(wǎng)格的特征提取。每一幀的更新也是只保留了一個(gè)指向當(dāng)前幀的指針pcurframe，用于實(shí)時(shí)記錄當(dāng)前幀跟蹤到的2D、3D特征點(diǎn)以及位姿信息。

接受到新圖像時(shí)只更新時(shí)間戳和幀號(hào)，在光流跟蹤以及計(jì)算位姿后分別更新當(dāng)前幀的特征點(diǎn)、位姿信息并更新運(yùn)動(dòng)模型。

3. 建圖線程

主要處理兩個(gè)任務(wù)，負(fù)責(zé)三角化以及局部地圖的跟蹤，在雙目的配置情況下會(huì)增加立體匹配的功能。

3.1 三角化

mapping中的三角化是整個(gè)系統(tǒng)中唯一生成3D點(diǎn)的地方，包括初始化成功后的生成初始地圖。三角化的主要功能是在當(dāng)前關(guān)鍵幀與共視關(guān)鍵幀之間產(chǎn)生新的地圖點(diǎn)，使得跟蹤更穩(wěn)。

具體過程為：對(duì)于當(dāng)前關(guān)鍵幀中的每一個(gè)2D特征點(diǎn)，將當(dāng)前2D點(diǎn)與第一次觀測(cè)到該2D點(diǎn)的關(guān)鍵幀中的對(duì)應(yīng)2D點(diǎn)進(jìn)行三角化。相比ORB-SLAM中將當(dāng)前關(guān)鍵幀與其共視程度最高的20幀至少有15個(gè)共視點(diǎn)的相鄰關(guān)鍵幀通過詞袋進(jìn)行特征匹配，再將成功匹配的點(diǎn)對(duì)進(jìn)行三角化的策略，二者的效果有待進(jìn)一步實(shí)驗(yàn)對(duì)比。

下圖為ORB-SLAM的LocalMapping中的三角化搜索范圍。這個(gè)操作和ORB-SLAM2類似

3.2 暫時(shí)三角化

通常用于單目初始化3D地圖，本文發(fā)現(xiàn)在雙目情況下可以用于在當(dāng)前關(guān)鍵幀之前，已經(jīng)正確追蹤到，但找不到雙目匹配的關(guān)鍵點(diǎn)。這時(shí)所有成功三角化的地圖點(diǎn)立刻應(yīng)用于前端進(jìn)行定位，然后這些3D位置通過BA進(jìn)行重新優(yōu)化。

3.3 局部地圖追蹤

類似于ORB-SLAM2局部地圖，局部地圖包括當(dāng)前關(guān)鍵幀或共視圖關(guān)鍵幀觀察到的3D地圖點(diǎn)。

局部地圖追蹤的目標(biāo)是找出屬于局部地圖的3D地圖點(diǎn)是否可以與當(dāng)前幀的關(guān)鍵點(diǎn)相匹配。這種重追蹤的操作可以看做是基本的回環(huán)檢測(cè)。

任意這樣的3D地圖點(diǎn)，在當(dāng)前關(guān)鍵幀的投影與一個(gè)關(guān)鍵點(diǎn)的距離少于兩個(gè)像素點(diǎn)時(shí)被定義為候選匹配。計(jì)算3D地圖點(diǎn)和其他幾個(gè)候選點(diǎn)的距離，接受最近距離的候選。

4. 狀態(tài)估計(jì)線程

使用局部BA來精煉已選擇的關(guān)鍵幀位姿和3D地圖點(diǎn)位置，此外過濾冗余的關(guān)鍵幀來限制外來局部BA的運(yùn)行時(shí)間。

4.1 局部地圖的優(yōu)化

這里的優(yōu)化與ORB-SLAM相近，優(yōu)化當(dāng)前關(guān)鍵幀和與其至少有25個(gè)共視點(diǎn)的相鄰關(guān)鍵幀的位姿以及這些關(guān)鍵幀對(duì)應(yīng)的地圖點(diǎn)的3D坐標(biāo)，對(duì)于那些不在這些關(guān)鍵幀范圍內(nèi)，但是可以觀測(cè)到這些地圖點(diǎn)的關(guān)鍵幀，也將觀測(cè)添加到BA中但是不對(duì)這些關(guān)鍵幀的位姿進(jìn)行優(yōu)化。

4.2 關(guān)鍵幀過濾

刪去關(guān)鍵幀的標(biāo)準(zhǔn)為：該關(guān)鍵幀觀測(cè)到的95%3D點(diǎn)已經(jīng)被至少其他4個(gè)關(guān)鍵幀觀測(cè)到

5. 在線詞袋回環(huán)檢測(cè)器

回環(huán)檢測(cè)主要負(fù)責(zé)檢測(cè)回環(huán)和重定位，即校正當(dāng)前姿勢(shì)估計(jì)和當(dāng)前幀與已檢測(cè)到LC的已傳遞關(guān)鍵幀之間的估計(jì)軌跡。這是一個(gè)重要的創(chuàng)新點(diǎn)。主要流程主要分為關(guān)鍵幀預(yù)處理、使用iBoW-LCD算法檢測(cè)候選關(guān)鍵幀、驗(yàn)證候選關(guān)鍵幀、位姿圖優(yōu)化以及l(fā)ooseBA。

5.1 關(guān)鍵幀預(yù)處理以及選取候選關(guān)鍵幀

作者在論文中提到，出于定位的考慮，OV2SLAM并不會(huì)跟蹤太多的特征點(diǎn)（大概只有200多）。在這里為了更新詞袋樹，對(duì)每幅圖像額外提取300個(gè)FAST特征并計(jì)算其描述子，然后將此關(guān)鍵幀傳給iBoW-LCD用于更新詞袋樹，當(dāng)關(guān)鍵幀數(shù)量大于100幀時(shí)，在詞袋樹中查找當(dāng)前關(guān)鍵幀的閉環(huán)候選關(guān)鍵幀。

5.2 驗(yàn)證候選關(guān)鍵幀

發(fā)現(xiàn)一個(gè)好的LC候選，首先確定不是一個(gè)假陽(yáng)性。對(duì)當(dāng)前關(guān)鍵幀Ki和候選關(guān)鍵幀Klc，首先應(yīng)用一個(gè)k最近鄰暴力匹配算法在這兩個(gè)關(guān)鍵幀的描述子中。

而不明確的匹配首先由經(jīng)典的ratio test進(jìn)行過濾。然后使用RANSAC方法進(jìn)行計(jì)算基礎(chǔ)矩陣Essential Matrix 以便只保持滿足極線幾何（對(duì)極幾何）的匹配。

使用這種內(nèi)在維持方法，我們使用P3P-RANSAC方法用回環(huán)候選關(guān)鍵幀Klc觀測(cè)到的剩余3D地圖點(diǎn)對(duì)當(dāng)前關(guān)鍵幀Ki進(jìn)行一個(gè)假設(shè)位姿計(jì)算。

若計(jì)算出一個(gè)可靠位姿，由大量?jī)?nèi)參器（inliers）確定，我們得到候選關(guān)鍵幀Klc的局部地圖，然后搜索在當(dāng)前關(guān)鍵幀Ki的額外的匹配，將3D地圖點(diǎn)的投影使用P3P的方法計(jì)算位姿。

然后，使用等式(1)基于到目前為止發(fā)現(xiàn)的所有匹配來精煉假設(shè)位姿，并且基于由Huber魯棒成本函數(shù)檢測(cè)到的離群值來執(zhí)行最后的過濾步驟。

5.3 位姿圖優(yōu)化

最終如果保留30個(gè)內(nèi)點(diǎn)，我們驗(yàn)證回環(huán)檢測(cè)，并執(zhí)行位姿圖優(yōu)化(PGO)來矯正完整軌跡。

目的是將誤差均攤到檢測(cè)到的關(guān)鍵幀與當(dāng)前關(guān)鍵幀之間的所有關(guān)鍵幀之間。

5.4 looseBA

與ORB-SLAM中的fullBA相比，OV2SLAM只對(duì)受閉環(huán)檢測(cè)影響的關(guān)鍵幀以及地圖點(diǎn)進(jìn)行優(yōu)化，這樣減輕了BA的負(fù)擔(dān)，但是仍然要花費(fèi)數(shù)秒的時(shí)間

6. OV2SLAM總結(jié)

OV2SLAM整體精度比ORBSLAM2略微下降，但是速度明顯提升。通過PnP、looselyBA 等方法能夠快速的構(gòu)建出我們想要的地圖。對(duì)于OV2SLAM而言相較于ORBSLAM2增加了異常點(diǎn)剔除、估算位姿準(zhǔn)確性等操作。

當(dāng)然對(duì)于ORB-SLAM3來說除了會(huì)判斷遠(yuǎn)點(diǎn)和近點(diǎn)，遠(yuǎn)點(diǎn)只用于優(yōu)化旋轉(zhuǎn)，尺度信息只能依靠近點(diǎn)以外，還是用了極大后驗(yàn)概率（MAP）估計(jì)應(yīng)用于利用幾何BA（光束平差法）最小化特征重投影誤差中。并將歷史數(shù)據(jù)一起加入約束中一起計(jì)算BA。

除此以外，ORB-SLAM3中還提出新的位置識(shí)別算法，現(xiàn)在當(dāng)前候選關(guān)鍵幀進(jìn)行幾何一致性檢測(cè)，然后檢測(cè)候選關(guān)鍵幀與三個(gè)共視關(guān)鍵幀的局部一致性，以計(jì)算成本為代價(jià)換來高召回率。

除此以外相較于這兩篇文章，ORB-SLAM3還提出了 ORB-SLAM Atlas，該地圖集提供了ORB-SLAM3的多地圖SLAM特性，ORB-SLAM地圖集中存有一系列非活動(dòng)的地圖，并且這些地圖可以進(jìn)行位置識(shí)別、相機(jī)重定位、回環(huán)檢測(cè)以及精準(zhǔn)的無(wú)縫地圖拼接。

7. OV2SLAM 安裝

安裝 OpenGV

git clone https://github.com/laurentkneip/opengv
cd opengv
mkdir build
cd build/
cmake ..
sudo make -j4 install

安裝OV2SLAM

xxxxxxxxxx
cd ~/catkin_ws/src/
git clone https://github.com/ov2slam/ov2slam.git
cd ~/catkin_ws/src/ov2slam
chmod +x build_thirdparty.sh
./build_thirdparty.sh
cd ~/catkin_ws/src
catkin_make --pkg ov2slam

運(yùn)行 OV2SLAM

xxxxxxxxxx
rosrun ov2slam ov2slam_node /home/crp/catkin_ws/src/ov2slam/parameters_files/accurate/euroc/euroc_mono.yaml
cd ~/catkin_ws/src/ov2slam
rviz -d ov2slam_visualization.rviz
rosbag play MH_04_difficult.bag

審核編輯：劉清