視覺SLAM開源算法ORB-SLAM3原理與代碼解析

ORB-SLAM最早的版本在2014年的RSS上發(fā)布，在2016年作者又發(fā)布了ORB-SLAM2,接著在2020年發(fā)布了ORB-SLAM 3。ORB-SLAM1只能針對(duì)單目相機(jī)數(shù)據(jù)進(jìn)行處理；ORB-SLAM 2 增加了對(duì)于雙目和RGB-D相機(jī)的處理，在回環(huán)檢測(cè)模塊增加了Full Global BA的處理；ORB-SLAM 3則增加了對(duì)于IMU融合的支持，兼容魚眼相機(jī)模型，并且增加了Altas多地圖的支持；同時(shí)，回環(huán)檢測(cè)為了支持多地圖的模式，提供了一種叫Welding BA的優(yōu)化方式。ORB -SLAM的作者將上述提到的各版本項(xiàng)目都開源了，為學(xué)術(shù)研究還是工程落地都提供了很好的參考。

01

基礎(chǔ)概念

首先來介紹一下ORB SLAM中涉及的一些基礎(chǔ)概念。

幀&關(guān)鍵幀：視覺SLAM都是對(duì)一個(gè)圖像序列進(jìn)行處理，每一張圖像被稱為幀，而關(guān)鍵幀則是通過一定篩選機(jī)制得到的、具有一定代表性的圖像幀。

地圖點(diǎn)/路標(biāo)點(diǎn)：將圖像上被觀察到的特征點(diǎn)通過三角化等方式進(jìn)行深度恢復(fù)，我們就可以得到其對(duì)應(yīng)的在三維空間的位置，同時(shí)包含幀的觀測(cè)信息，這種點(diǎn)就被稱為地圖點(diǎn)或路標(biāo)點(diǎn)。

共視：當(dāng)一個(gè)地圖點(diǎn)被多幀觀察到時(shí)，我們就可以稱這幾幀有基于該地圖點(diǎn)的共視關(guān)系。

共視圖&本質(zhì)圖:我們可以把共視關(guān)系作用邊表示，關(guān)鍵幀用節(jié)點(diǎn)表示，就可以建立共視圖，而本質(zhì)圖在共視圖基礎(chǔ)上只保留具有較強(qiáng)共視關(guān)系的邊。

Altas（地圖集）：ORB-SLAM 3提供了多地圖的存儲(chǔ)和拼接功能，在跟蹤丟失后可以嘗試將現(xiàn)有地圖和歷史地圖進(jìn)行匹配、融合，并更新當(dāng)前的活躍地圖（Active Map）。

數(shù)據(jù)關(guān)聯(lián)：在語義SLAM中，第k幀檢測(cè)到物體Obj1、Obj2，第k+1幀檢測(cè)到物體Obj3、Obj4，確定Obj1和Obj3、Obj4中的哪一個(gè)是對(duì)真實(shí)世界中同一個(gè)物體的多次觀測(cè)，這是數(shù)據(jù)關(guān)聯(lián)的一個(gè)直觀例子。在間接法（特征法）SLAM中，表現(xiàn)為不同幀中的特征，哪些是對(duì)應(yīng)于同一個(gè)空間路標(biāo)點(diǎn)/地圖點(diǎn)的。在ORB3中考慮到的數(shù)據(jù)關(guān)聯(lián)包括短期內(nèi)滑動(dòng)窗口中關(guān)鍵幀觀測(cè)到的路標(biāo)點(diǎn)和圖像特征的數(shù)據(jù)關(guān)聯(lián)；中期的數(shù)據(jù)關(guān)聯(lián)是指圖像特征與局部地圖點(diǎn)的關(guān)聯(lián)；長期的數(shù)據(jù)關(guān)聯(lián)包括利用場(chǎng)景識(shí)別技術(shù)和詞袋模型，在回環(huán)檢測(cè)、重定位等過程中的數(shù)據(jù)關(guān)聯(lián)；而多地圖的數(shù)據(jù)關(guān)聯(lián)還可以實(shí)現(xiàn)地圖之間地圖點(diǎn)的匹配和融合。

ORB-SLAM 3的基本流程和此前的ORB版本沒有顯著的改變，只是也增加了部分新特性。基于詞袋模型的關(guān)鍵幀數(shù)據(jù)和之前差不多，每一個(gè)關(guān)鍵幀均會(huì)被存入數(shù)據(jù)庫用于回環(huán)檢測(cè)。地圖結(jié)構(gòu)上進(jìn)行了改進(jìn)，ORB-SLAM3使用Altas地圖集的結(jié)構(gòu)，地圖中包含一個(gè)Active Map和若干個(gè)Non-active Map，每個(gè)Map均包括地圖點(diǎn)，關(guān)鍵幀，共視圖，Spanning Tree等信息。跟蹤線程添加了IMU的積分，并且和以前一樣進(jìn)行關(guān)鍵幀的判斷和構(gòu)造；在LocalMapping線程中中執(zhí)行IMU的初始化，以及和之前相同的冗余關(guān)鍵幀和地圖點(diǎn)剔除、新地圖點(diǎn)的創(chuàng)建等工作；在回環(huán)檢測(cè)部分主要添加了多地圖的融合。

ORB-SLAM 3框架

總結(jié)一下，ORB-SLAM3的貢獻(xiàn)包括：

1、提供了一個(gè)單雙目VI-SLAM的系統(tǒng)；

2、改善召回率的場(chǎng)景識(shí)別技術(shù)；

3、多地圖機(jī)制；

4、抽象的相機(jī)表示。

02

抽象相機(jī)模型介紹

為什么ORB-SLAM3需要一個(gè)抽象的相機(jī)模型呢？

相比于傳統(tǒng)相機(jī)，魚眼相機(jī)超過180度的廣視角可以獲取更多的信息，但是因?yàn)樗环厢樋啄Ｐ蛿?shù)學(xué)建模的假設(shè)，導(dǎo)致uniform reprojection error的假設(shè)失效；如果對(duì)于圖像直接進(jìn)行裁剪，將會(huì)導(dǎo)致外圍圖像丟失，反而喪失了魚眼相機(jī)大視角的優(yōu)勢(shì)。在ORB-SLAM3中，相機(jī)成像模型提供投影、反投影和相關(guān)的雅克比計(jì)算等函數(shù)，并且將此前系統(tǒng)中的EPNP更換為MAP-PNP, 從而實(shí)現(xiàn)了相機(jī)成像模型與SLAM部分的解耦，還可以擴(kuò)展，能將相同的SLAM pipeline用于大部分類型的相機(jī)。

此外，針對(duì)雙目相機(jī)模型，ORB-SLAM3也提供了一定的改善。ORB-SLAM2假設(shè)我們針對(duì)雙目相機(jī)預(yù)先進(jìn)行了極線矯正，但是很多時(shí)候由于無法保證左右目相機(jī)光軸的絕對(duì)平行，極線矯正的效果也往往不好；而有些時(shí)候，我們需要使用兩個(gè)參數(shù)不同的相機(jī)進(jìn)行觀測(cè)，而ORB-SLAM2無法兼容這類雙目相機(jī)，如類似RGB-D相機(jī)中焦距、分辨率相差巨大的彩色相機(jī)+近紅外相機(jī)，如果將彩色圖像和近紅外圖像組成雙目圖像，ORB-SLAM2無法綜合利用這對(duì)圖像估計(jì)相機(jī)位姿。在ORB-SLAM3中將左右兩目相機(jī)視作為具有固定位姿變換的兩臺(tái)單目相機(jī)使用，并且也不再限制兩臺(tái)相機(jī)必須具有足夠面積的、重疊的共視區(qū)域，解決了這個(gè)問題。

03

VISLAM實(shí)現(xiàn)和IMU初始化

ORB-SLAM3中VI-SLAM在ORB-SLAM-VI上進(jìn)行了改進(jìn)，包括：提供快速，準(zhǔn)確的IMU初始化；支持單雙目VI-SLAM；支持針孔/魚眼相機(jī)模型。在視覺和IMU融合方面，ORB-SLAM3在位姿求解時(shí)所建立優(yōu)化問題的殘差項(xiàng)，包括所有關(guān)鍵幀和上一幀IMU估計(jì)的殘差項(xiàng)，以及所有路標(biāo)點(diǎn)觀測(cè)的視覺誤差項(xiàng)。其中針對(duì)視覺路標(biāo)點(diǎn)的觀測(cè)，為了避免錯(cuò)誤匹配造成的極端值的影響，嵌套了魯棒核函數(shù)。

IMU初始化的目的是為了得到Body系速度、重力方向和IMU偏置。ORB-SLAM3中初始化流程的設(shè)計(jì)建立在作者的幾點(diǎn)思考上：

1、ORB-SLAM純單目已經(jīng)可以初始化得到精確的地圖，尺度信息可以通過IMU得到；雙目圖像輸入下則尺度客觀，可以不考慮尺度信息的問題；

2、如果將尺度單獨(dú)作為優(yōu)化變量進(jìn)行表示和優(yōu)化，效果比在BA中的隱式表達(dá)收斂更快；

3、IMU初始化過程中必須考慮傳感器的不確定性，否則會(huì)產(chǎn)生難以預(yù)測(cè)的巨大誤差。

接下來的討論IMU初始化問題時(shí)，均指單目輸入時(shí)的初始化。ORB3中IMU初始化的步驟包含三步，第一步是純視覺最大后驗(yàn)估計(jì)（MAP），第二步是純慣性MAP，第三步是視覺+慣性MAP。針對(duì)純視覺MAP，我們提取初始化后2s內(nèi)10幀圖像進(jìn)行純視覺BA，從而得到?jīng)]有尺度信息的相機(jī)位姿和路標(biāo)點(diǎn)位置。接下來我們進(jìn)行只有IMU參與的初始化，最終得到的優(yōu)化結(jié)果是：幀位姿、速度和地圖點(diǎn)，并都具有正確的尺度；Body系Z軸將被旋轉(zhuǎn)到和重力方向一致；IMU的偏置被更新。第三步是視覺IMU聯(lián)合后驗(yàn)估計(jì)，ORB-SLAM3只需要2秒就可以完成尺度的初始化，誤差在5%左右，此外，ORB-SLAM3還將進(jìn)行只包含尺度因子和重力方向的優(yōu)化，10秒一次，用于避免傳感器運(yùn)動(dòng)緩慢時(shí)IMU激勵(lì)不夠的情況。

ORB-SLAM3中的跟蹤和建圖和ORB-SLAM-VI類似，在短期跟丟后，在滿足一定條件時(shí)會(huì)嘗試?yán)肐MU積分得到的位姿信息進(jìn)行重定位；當(dāng)丟失持續(xù)一定時(shí)間后，將會(huì)重新初始化，創(chuàng)建新的Active map。

04

改進(jìn)的回環(huán)檢測(cè)與多地圖融合

這一部分的內(nèi)容很大程度上和ORB-SLAM2是相同的，我們首先來回顧一下基本概念。

準(zhǔn)確率（PrecisionRate)：檢測(cè)到的回環(huán)中正確的比率。

召回率（RecallRate）：檢測(cè)到的回環(huán)占總真實(shí)回環(huán)數(shù)的比率。

在ORB-SLAM1/2中，僅通過DBoW詞袋數(shù)據(jù)庫就可實(shí)現(xiàn)50%~80%的準(zhǔn)確率和召回率。在回環(huán)時(shí)，通過增加幾何一致性和時(shí)間一致性檢驗(yàn)，犧牲召回率來增加準(zhǔn)確率。ORB-SLAM3改進(jìn)了回環(huán)檢測(cè)的速度，提高了召回率，并且增加了多地圖的部分。

尋找閉環(huán)幀的過程可以分為六步：

1.針對(duì)每一個(gè)新關(guān)鍵幀，在數(shù)據(jù)庫中查詢到三個(gè)最相似的關(guān)鍵幀；

2.嘗試對(duì)新關(guān)鍵幀及其共視關(guān)鍵幀，和候選關(guān)鍵幀及其共視關(guān)鍵幀進(jìn)行數(shù)據(jù)關(guān)聯(lián)；

3.利用匹配的特征點(diǎn)和地圖點(diǎn)求解位姿轉(zhuǎn)換；

4.利用位姿變換的初始估計(jì)，進(jìn)行點(diǎn)云重投影尋找新的匹配，并且進(jìn)行位姿的優(yōu)化求精；

5.對(duì)時(shí)間一致性的檢驗(yàn)，此前的步驟相對(duì)復(fù)雜，在ORB-SLAM3中局部地圖里面已有關(guān)鍵幀的共視信息進(jìn)行判斷；

6.利用重力方向?qū)τ诨丨h(huán)結(jié)果進(jìn)行檢查。

回環(huán)檢測(cè)后是進(jìn)行回環(huán)還是地圖合并，取決于當(dāng)前關(guān)鍵幀檢測(cè)到的回環(huán)關(guān)鍵幀是在當(dāng)前的active map還是在其他map。當(dāng)對(duì)non-active map和active map進(jìn)行融合時(shí)，共視圖和本質(zhì)圖同步更新,active map中的信息被追加到歷史地圖中，匹配到的non-active map變成新的map。

05

總結(jié)

總體來說，ORB-SLAM3的流程和ORB-SLAM1/2非常相似，對(duì)于ORB-SLAM系列熟悉的同學(xué)應(yīng)該很容易上手；相機(jī)模型的抽象處理，使得SLAM位姿求解過程和相機(jī)成像模型解耦，理論上支持絕大多數(shù)成像模型的相機(jī)；通過對(duì)于IMU的支持，ORB-SLAM系列加入了VI-SLAM的大家庭，也表明多傳感器融合的SLAM是目前一大發(fā)展趨勢(shì)；多地圖的機(jī)制有利于跟丟后保留盡可能多的信息用于后續(xù)補(bǔ)救，也為后續(xù)實(shí)現(xiàn)多機(jī)器協(xié)同的SLAM提供了工作基礎(chǔ)。