基于點(diǎn)云的語義單目SLAM

語義SLAM的最終目的一般有兩種：提高定位精度，或提高建圖質(zhì)量。定位精度很好理解，誤差低就完事兒了，一般策略有：添加新的誤差項(xiàng)/考慮動(dòng)態(tài)環(huán)境的影響等。而對(duì)于建圖質(zhì)量，每篇文章的側(cè)重點(diǎn)則不完全相同。有的是提高地圖的幾何準(zhǔn)確度（3D坐標(biāo)，法向量等），有的是提高語義標(biāo)號(hào)的準(zhǔn)確度，有的是去除動(dòng)態(tài)物體的影響（殘影），有的則是將物體作為地圖的基本元素獨(dú)立出來。

接下來是三篇語義SLAM連載文章，依次為基于點(diǎn)，面元和物體的語義SLAM，根據(jù)地圖中建圖元素的不同進(jìn)行分類。

注：有的文章可能只涉及語義建圖（Semantic Mapping），而沒有將語義用于提高定位精度，比如【論文閱讀21】Surfel-based Mapping: SemanticFusion。但是這里為了簡(jiǎn)單，統(tǒng)稱為語義SLAM。

• 在ORB-SLAM中添加直接法。

• 利用每幀中每個(gè)特征點(diǎn)的測(cè)量信息：深度測(cè)量（三角化結(jié)果），語義標(biāo)號(hào)（CNN輸出）和特征點(diǎn)匹配精度（公式(13)(14)），維護(hù)每個(gè)特征在地圖中的性質(zhì)：深度，語義標(biāo)號(hào)，和內(nèi)點(diǎn)率（屬于靜態(tài)物體的概率）。維護(hù)過程以概率方式而非簡(jiǎn)單的加權(quán)平均方式來完成。

N. Brasch, A. Bozic, J. Lallemand, F. Tombari. Semantic Monocular SLAM for Highly Dynamic Environments. IEEE/RSJ International Conference on Intelligent Robots and Systems, 393-400, 2018.

摘要

最近在單目SLAM中的提高已經(jīng)能夠得到在靜態(tài)環(huán)境中運(yùn)行的實(shí)時(shí)系統(tǒng)，但是在動(dòng)態(tài)場(chǎng)景變化和運(yùn)動(dòng)時(shí)會(huì)失敗，由于它們?nèi)鄙亠@式動(dòng)態(tài)外點(diǎn)處理。Brasch等人提出了一種語義 SLAM框架來解決高動(dòng)態(tài)環(huán)境，組合基于特征和直接法來達(dá)到挑戰(zhàn)環(huán)境下的魯棒性。提出的方法利用了場(chǎng)景的語義信息到一個(gè)顯式的概率模型中，其最大化跟蹤和建圖的概率來依賴那些與相機(jī)沒有相對(duì)運(yùn)動(dòng)的場(chǎng)景部分。與SOTA相比，我們的算法在動(dòng)態(tài)環(huán)境中展示出更穩(wěn)定的可靠位姿估計(jì)，在靜態(tài)序列中達(dá)到了相似表現(xiàn)，在Virtual KITTI和Synthia數(shù)據(jù)集上。

1. 介紹

在過去幾年，對(duì)于單目SLAM領(lǐng)域的大量研究允許達(dá)到了之前未見到的精度，魯棒性和速度，產(chǎn)生了大量在機(jī)器人和增強(qiáng)現(xiàn)實(shí)領(lǐng)域的新應(yīng)用。與基于雙目或者RGB-D的技術(shù)相比，單目SLAM算法[1]-[4]依賴更廉價(jià)的硬件，更容易標(biāo)定并且在深度范圍上沒有限制，使得它們對(duì)于同時(shí)專注室內(nèi)和室外場(chǎng)景的移動(dòng)應(yīng)用中特別地受歡迎。

單目SLAM方法可以被劃分為兩類。基于描述的方法[5][1]使用顯式關(guān)鍵點(diǎn)描述子來尋找不同圖片間的特征匹配并最小化它們之間的重投影誤差。不同地，直接法[6][2][4][3]最小化光度誤差基于從一張圖片到另一張圖片的像素密度的投影。描述和直接法有它們各自優(yōu)勢(shì)和缺陷，如[3]中分析的那樣。特別地，描述方法對(duì)于幾何噪聲耕路邦，即像素位置偏移，來源于不準(zhǔn)確的相機(jī)內(nèi)參標(biāo)定或者卷簾門效應(yīng)，而直接法更適合用于解決光度噪聲，來源于運(yùn)動(dòng)模糊。

然而，當(dāng)前單目SLAM算法依賴周圍環(huán)境是靜態(tài)的假設(shè)，限制了它們?cè)诖蠖鄶?shù)真實(shí)世界場(chǎng)景中的應(yīng)用。為了解決動(dòng)態(tài)物體，它們或者在優(yōu)化期間利用M-估計(jì)器（Tukey[5]，Huber[2][1][3]），或者使用基于RANSAC的方法來檢測(cè)和濾除運(yùn)動(dòng)[7]。為了能正常工作，兩種方法都需要大多數(shù)點(diǎn)都相對(duì)相機(jī)運(yùn)動(dòng)是靜態(tài)的。相反地，當(dāng)運(yùn)動(dòng)物體占據(jù)相機(jī)視野的大部分，尤其是當(dāng)大多數(shù)視覺特征位于這些區(qū)域內(nèi)，當(dāng)前的單目SLAM方法將會(huì)失效。這是由其普遍的現(xiàn)象在大多數(shù)室外與駕駛相關(guān)的情境中：特別是動(dòng)態(tài)物體運(yùn)動(dòng)緩慢或者從靜止開始運(yùn)動(dòng)時(shí)（想象一輛車暫時(shí)停在交通燈前，如圖1所示），外點(diǎn)的檢測(cè)非常困難。沒有進(jìn)一步關(guān)于觀測(cè)區(qū)域的信息，特別是對(duì)于單目方法，一般不太可能區(qū)分圖片中的運(yùn)動(dòng)和靜止部分。由于最近基于CNNs的場(chǎng)景理解和語義分割方法的進(jìn)步，高層次推理可以被使用來減少圖片中靜止和動(dòng)態(tài)部分的歧義。這時(shí)非常有趣的考慮到新的卷積結(jié)構(gòu)的發(fā)展和能夠高效在移動(dòng)/嵌入式GPUs中運(yùn)行的模型的發(fā)展[8]。

一個(gè)來自CitySpaces數(shù)據(jù)集的例子，其中一輛汽車在一個(gè)交通燈前。大多數(shù)圖片區(qū)域?qū)儆跁簳r(shí)靜止的物體，將會(huì)緩慢移動(dòng)，因此導(dǎo)致基于運(yùn)動(dòng)的外點(diǎn)檢測(cè)方法的失效。在(c)中一個(gè)語義掩膜忽略所有在潛在動(dòng)態(tài)區(qū)域的關(guān)鍵點(diǎn)，并因此不能使用停下的車輛用于位姿估計(jì)。我們的方法(d)使用逐點(diǎn)外點(diǎn)估計(jì)使用深度方差并融合(1)的語義信息。紅色圓圈可視化估計(jì)的外點(diǎn)測(cè)量，綠色圓圈是用于位姿估計(jì)的內(nèi)點(diǎn)。

通過依賴場(chǎng)景中的語義信息，可能檢測(cè)出潛在的動(dòng)態(tài)物體而不需要顯式地跟蹤它們。能夠分割出場(chǎng)景的靜態(tài)部分例如大樓或者車道線，我們可以指引特征提取并匹配該部分。此外，沒有依賴逐幀語義信息，我們提出一種概率模型，考慮所有幀的語義信息來估計(jì)地圖點(diǎn)的語義。除了語義信息，我們還是用時(shí)序運(yùn)動(dòng)信息來推理地圖點(diǎn)是運(yùn)動(dòng)的還是靜止的。我們更新地圖點(diǎn)的概率參數(shù)當(dāng)有新的觀測(cè)時(shí)。為了得到一個(gè)實(shí)時(shí)SLAM系統(tǒng)，我們?cè)O(shè)計(jì)了一種高效的具有低常數(shù)內(nèi)存消耗的在線概率更新。在我們的評(píng)估中，我們展示了更穩(wěn)定的結(jié)果在高動(dòng)態(tài)環(huán)境中在仿真和真實(shí)數(shù)據(jù)中，同時(shí)展示了類似SOTA的表現(xiàn)在靜態(tài)場(chǎng)景中。

2. 相關(guān)工作

動(dòng)態(tài)物體被大多數(shù)SLAM算法看作是外點(diǎn)。我們提出使用語義信息來選擇一組位于靜態(tài)場(chǎng)景部分的積極特征用于更魯棒的位姿估計(jì)，與現(xiàn)存語義SLAM方法專注于稠密3D重建不同。語義先驗(yàn)由RGB圖片上訓(xùn)練的深度模型生成。

A. 動(dòng)態(tài)SLAM

在過去，不同的策略已經(jīng)被提出來解決視覺SLAM中的動(dòng)態(tài)外點(diǎn)。在[4]中，只有深度在一些觀測(cè)后收斂到具有較小方差的積極特征被用于跟蹤。大量[5]的改進(jìn)已經(jīng)被提出來顯式處理動(dòng)態(tài)物體。在[7]中，一個(gè)替代的RANSAC構(gòu)架被使用，其中采樣被調(diào)整來分割采樣點(diǎn)。[9]使用光流在所有特征點(diǎn)的流朝向中尋找聚類，并使用聚類將動(dòng)態(tài)物體從靜態(tài)背景中分割出去。RGB-D相機(jī)或者雙目相機(jī)的使用產(chǎn)生了高度可靠和稠密的深度地圖，在這些情形中空閑空間推理可以被用于檢測(cè)動(dòng)態(tài)物體。動(dòng)態(tài)物體被檢測(cè)如果它們移動(dòng)到之前空閑的區(qū)域，并在位姿估計(jì)中被標(biāo)記為外點(diǎn)[10]。當(dāng)只有稀疏和具有噪聲的深度信息可用時(shí)，空閑推理是不可能的。為了處理單目系統(tǒng)構(gòu)架中的動(dòng)態(tài)場(chǎng)景，最近的工作專注于多身體SfM構(gòu)架。這里場(chǎng)景被分割為多個(gè)運(yùn)動(dòng)剛體和靜態(tài)世界。物體實(shí)例被首先通過運(yùn)動(dòng)分割檢測(cè)出來，然后對(duì)每個(gè)聚類，一個(gè)逐幀的變換被計(jì)算并且BA被用于有緣最終的軌跡[11]。這里，輸出的質(zhì)量依賴運(yùn)動(dòng)分割。如果運(yùn)動(dòng)很小，分割很差，緩慢運(yùn)動(dòng)的物體不會(huì)被檢測(cè)出來。而且執(zhí)行時(shí)間遠(yuǎn)非實(shí)時(shí)。

B. 語義SLAM

大多數(shù)現(xiàn)存方法，組合經(jīng)典的SLAM和場(chǎng)景的語義分割，使用SLAM系統(tǒng)的位姿圖來構(gòu)建一個(gè)在圖片序列上時(shí)序或者空間一致的分割。時(shí)序或者空間一致性可以被構(gòu)建為一個(gè)圖片上的CRF[12]，一個(gè)稠密體素柵格[13]或者一個(gè)網(wǎng)格[14]。稠密CRFs的使用使得大多數(shù)現(xiàn)存方法不適合大規(guī)模動(dòng)態(tài)場(chǎng)景的實(shí)時(shí)應(yīng)用，由于它們的低幀率[12]。其他方法使用在線更新用于語義融合[15]，允許它們實(shí)時(shí)運(yùn)行。大多數(shù)上述方法不反饋語義信息到位姿估計(jì)流程。在[12]中，語義信息被用于在3D模型融合中加權(quán)測(cè)量。[16]移除了點(diǎn)如果語義類別對(duì)于多次觀測(cè)是不同的。為了獲得稠密的3D模型，雙目相機(jī)也被使用在[12]和[17]中。

3. 概率語義SLAM

提出的SLAM系統(tǒng)建立在ORB-SLAM框架上[1]，其由跟蹤，建圖和回環(huán)三個(gè)模塊組成。圖2給出了框架的綜述。我們對(duì)動(dòng)態(tài)和靜態(tài)地圖點(diǎn)提出了一個(gè)顯式模型，并只跟蹤靜態(tài)點(diǎn)。

圖2.  我們動(dòng)態(tài)SLAM框架綜述。ORB-SLAM方法被擴(kuò)展通過在地圖模塊中使用直接特征提取和匹配，因此它們可以在跟蹤模塊中被使用用于位姿估計(jì)。我們還整合了一個(gè)概率外點(diǎn)模型來更新每個(gè)地圖點(diǎn)的狀態(tài)。只有積極地圖點(diǎn)被使用在位姿估計(jì)中。增加或者更新的模塊被顯示為紅色。

我們從前兩幀開始，并使用ORB特征進(jìn)行基于基本矩陣估計(jì)的初始化[1]，接著進(jìn)行全局BA：聯(lián)合優(yōu)化相機(jī)位姿和地圖點(diǎn)。為了補(bǔ)償位姿估計(jì)誤差，我們使用Lucas-Kanade光流[8]和極線約束的組合替代極線搜索（和[3]中一樣）來估計(jì)直接特征的初始深度。

對(duì)于每個(gè)新幀，ORB特征被提取，并且對(duì)應(yīng)被發(fā)現(xiàn)通過描述子匹配。位姿估計(jì)被初始化基于常運(yùn)動(dòng)速度模型。我們優(yōu)化新幀的位姿基于描述和直接的方法，通過多分辨率，多步驟非線性優(yōu)化。依賴每層金字塔中描述和直接特征的數(shù)量，我們使用多輪，其中我們?cè)黾踊蛞瞥卣鼽c(diǎn)基于它們的殘差。我們只在關(guān)鍵幀中提取新的直接特征。如果在當(dāng)前幀中沒有足夠的對(duì)應(yīng)被找到，一個(gè)新的關(guān)鍵幀被創(chuàng)建。

有了新幀的估計(jì)位姿，我們可以三角化描述和直接特征來得到一個(gè)深度估計(jì)。地圖點(diǎn)和相機(jī)位姿被聯(lián)合精修通過局部BA在一個(gè)固定窗口大小的關(guān)鍵幀中。

A. 位姿估計(jì)和建圖

描述特征類似ORB被用在SfM方法中，提供快速和可靠的匹配，減少了假陽性對(duì)應(yīng)。地圖點(diǎn)一般被參數(shù)化為3D點(diǎn)并且優(yōu)化被構(gòu)建為最小化重投影誤差（見等式2）。進(jìn)一步地，描述的特征可以被用于識(shí)別回環(huán)并且在一個(gè)已知地圖上進(jìn)行重定位，其可以被用于類似自動(dòng)駕駛中的定位和AR應(yīng)用中的添加標(biāo)簽。

另一方面，直接特征避免關(guān)鍵點(diǎn)和描述子的昂貴提取代價(jià)，但是一個(gè)好的初始化位姿是需要的為了使得優(yōu)化收斂到全局最小值。這導(dǎo)致需要高幀率或者相對(duì)緩慢的相機(jī)運(yùn)動(dòng)。基于圖片塊的匹配準(zhǔn)確率不像描述子比較那樣可靠，假陽性或局部最小值可能被檢測(cè)到。直接特征也對(duì)強(qiáng)，突然的或者局部光照變化敏感。估計(jì)一個(gè)仿射曝光模型[3]可以降低這些影響。然而，直接特征也可以被提取在低紋理或者強(qiáng)運(yùn)動(dòng)模糊的情形中。

由于這些互補(bǔ)性質(zhì)，我們打算可能的話使用描述特征。在沒有足夠特征被發(fā)現(xiàn)時(shí)，我們替代地使用直接特征。

不像用于描述特征的3D地圖點(diǎn)方法，我們決定使用和直接特征一致的逆深度構(gòu)架。因此，我們可以使用相同的概率模型對(duì)于兩種特征，其簡(jiǎn)化了聯(lián)合優(yōu)化的權(quán)重。

我們決定使用ORB特征作為描述特征，由于它們的快速提取（FAST-角點(diǎn)+方向）和魯棒的描述子（BRIEF）。我們跟隨[1]的應(yīng)用使用柵格提取策略來得到整張圖片的均勻分布和一個(gè)多分辨率金字塔。對(duì)于直接特征，我們基于[3]中的工作使用分辨率金字塔和一個(gè)基于柵格的特征選擇策略。

在位姿估計(jì)和局部BA中，重投影誤差  和  的加權(quán)和被最小化。

 (1)

其中  平衡了重投影誤差和光度誤差。因?yàn)槲覀冊(cè)谥瘘c(diǎn)級(jí)別推理，這兩種參數(shù)的動(dòng)態(tài)適應(yīng)是不需要的。

我們定義  為  中的位姿變換，將一個(gè)表示在視覺坐標(biāo)系中的點(diǎn)  變換到第  幀的坐標(biāo)系統(tǒng)中。同時(shí)，  是相機(jī)內(nèi)參矩陣，  表示從歸一化坐標(biāo)到笛卡爾坐標(biāo)的變換，  是關(guān)鍵點(diǎn)的估計(jì)深度。

重投影誤差被給出通過觀測(cè)關(guān)鍵點(diǎn)  和匹配的關(guān)鍵點(diǎn)  從第  幀到第  幀的重投影的像素距離。

(2)

以相同方式，光度誤差是圖片  中的像素  和它在圖片  中重投影之間的像素密度差。

(3)

我們跟隨[3]并對(duì)光照變化使用仿射變換模型。

(4)

其中  是快門時(shí)間，  和  是對(duì)于每幀估計(jì)的仿射變換參數(shù)。

一個(gè)具有魯棒Huber模的加權(quán)高斯牛頓方法被應(yīng)用以解決非線性最小二乘問題。類似[2]我們使用協(xié)方差縮放，每一項(xiàng)被加權(quán)使用逆協(xié)方差來反映每個(gè)測(cè)量的不確定性。

協(xié)方差傳播被執(zhí)行在每個(gè)新的測(cè)量后通過等式(5)的更新方法。

(5)

B. 概率外點(diǎn)拒絕

SLAM的一個(gè)核心想法是精修3D世界的地圖，通過更新每個(gè)地圖點(diǎn)的3D位置當(dāng)它被再次觀測(cè)到時(shí)。由于一些測(cè)量比其他測(cè)量更可靠，可能通過依賴一種概率方法將測(cè)量的方差作為權(quán)重進(jìn)行平均比簡(jiǎn)單的平均要更好。

在一個(gè)動(dòng)態(tài)環(huán)境中，只有地圖點(diǎn)的位置是不夠的。如果我們執(zhí)行BA使用場(chǎng)景中的所有點(diǎn)，包括動(dòng)態(tài)點(diǎn)，這將會(huì)損害優(yōu)化估計(jì)，因?yàn)锽A假設(shè)點(diǎn)位置的時(shí)序一致性。因此，我們也想要知道哪些點(diǎn)是足夠可靠的用于BA。

我們對(duì)每個(gè)地圖點(diǎn)估計(jì)內(nèi)點(diǎn)率  ，描述該點(diǎn)的可靠未定的概率。內(nèi)點(diǎn)率能夠以多種方式被建模，例如一些方法保存成功和不成功三角化的次數(shù)[1]。在[19],[4]中，一種概率模型被使用來建模深度，并將內(nèi)點(diǎn)率建模為隱變量。在上述兩種情形中，內(nèi)點(diǎn)率都被更新通過觀測(cè)地圖點(diǎn)隨時(shí)間的位置，并基于估計(jì)的相機(jī)位姿決定他們是否為動(dòng)態(tài)點(diǎn)。

確定地圖點(diǎn)的內(nèi)點(diǎn)率的運(yùn)動(dòng)估計(jì)可能在單目SLAM中是奇異的。對(duì)于緩慢運(yùn)動(dòng)的物體或者如果一個(gè)大動(dòng)態(tài)物體占據(jù)了相機(jī)的絕大部分視野，并且它本身被看做是靜態(tài)視覺的一部分。我們將語義信息包含到內(nèi)點(diǎn)率的估計(jì)中來提供另一種獨(dú)立的信息來源，來推測(cè)地圖點(diǎn)是動(dòng)態(tài)點(diǎn)的可能性。因此，除了深度  和內(nèi)點(diǎn)率 ，我們還對(duì)每個(gè)地圖點(diǎn)估計(jì)一個(gè)語義類別  。

當(dāng)一個(gè)地圖點(diǎn)被觀測(cè)到時(shí)，我們使用三角化計(jì)算它當(dāng)前的深度估計(jì)  ，和該深度估計(jì)的方差  。新測(cè)量的方差來自三角化，假設(shè)關(guān)鍵點(diǎn)在圖片中的位置是已知的，并且具有像素級(jí)精度[19]。我們也估計(jì)匹配精度  ，后面會(huì)詳細(xì)描述，并對(duì)關(guān)鍵點(diǎn)從神經(jīng)網(wǎng)絡(luò)中檢索語義類別概率  。這里  是神經(jīng)網(wǎng)絡(luò)的輸出，并且可以被理解為一個(gè)關(guān)鍵點(diǎn)屬于語義類別  的概率，給定當(dāng)前圖片幀  。

我們定義深度測(cè)量似然概率在等式6中。它基于[19]，但是我們擴(kuò)展它通過使用匹配精度。為了簡(jiǎn)化符號(hào)，我們使用  。

 (6)

這個(gè)定義后的直觀如下：如果當(dāng)前關(guān)鍵點(diǎn)被正確匹配，并且地圖點(diǎn)是靜止的，那么匹配精度  和內(nèi)點(diǎn)率  都接近  。因此，深度測(cè)量  服從高斯分布  。另一方面，如果當(dāng)前匹配是錯(cuò)誤的，或者點(diǎn)是動(dòng)態(tài)的，那么當(dāng)前深度測(cè)量被認(rèn)為是均勻分布  ，并且對(duì)于平均深度  的估計(jì)不提供任何有用信息。

類似深度的情形，我們建模地圖點(diǎn)的語義為神經(jīng)網(wǎng)絡(luò)輸出  和一個(gè)錯(cuò)誤匹配關(guān)鍵點(diǎn)的均勻分布的混合：

 (7)

這允許一個(gè)高效的在線更新和地圖點(diǎn)的動(dòng)態(tài)與靜態(tài)狀態(tài)之間的平滑轉(zhuǎn)變。

最終，我們需要定義語義類別上內(nèi)點(diǎn)率的獨(dú)立性。已經(jīng)被證實(shí)如果我們建模獨(dú)立性為Beta分布，那么可以推出一個(gè)高效的在線參數(shù)更新，如公式(8)所示。

 (8)

這里，參數(shù)  是一個(gè)熱編碼的語義類別，  是固定的常數(shù)，對(duì)每個(gè)語義類別進(jìn)行設(shè)置。它們表示了某個(gè)類別是靜態(tài)或者動(dòng)態(tài)的似然（例如：一輛車具有較小的  和較大的  ，因?yàn)樗锌赡苁莿?dòng)態(tài)的）。常數(shù)  可以被縮放使得與深度測(cè)量相比，在語義測(cè)量上放置更大或者更小的權(quán)重，例如，更大的 可以在內(nèi)點(diǎn)率的估計(jì)中給語義先驗(yàn)相比運(yùn)動(dòng)先驗(yàn)更大的權(quán)重。

深度，內(nèi)點(diǎn)率和語義類別的聯(lián)合模型的獨(dú)立性圖如圖3所示。測(cè)量的深度  依賴真實(shí)深度 ，匹配準(zhǔn)確率  和內(nèi)點(diǎn)率  ，其依賴語義  。

圖3.  聯(lián)合概率模型的圖。展示了深度測(cè)量d_i，測(cè)量精度\alpha_i和內(nèi)點(diǎn)率\phi的關(guān)系。后者依賴當(dāng)前幀CNN預(yù)測(cè)的語義類別c。

近似推理導(dǎo)致組合三項(xiàng)的后驗(yàn)概率。第一項(xiàng)包括高斯分布的深度概率，第二項(xiàng)是基于深度測(cè)量的Beta分布形式的內(nèi)點(diǎn)率，第三項(xiàng)是在語義類別上建模內(nèi)點(diǎn)率的獨(dú)立性的Beta分布，

 (9)

這里  是所有深度測(cè)量，  是所有語義信息觀測(cè)，  是CNN的  類概率密度輸出。

可以看出所有深度測(cè)量都被總結(jié)為均值深度  和深度方差  。類似地，內(nèi)點(diǎn)率遵循參數(shù)為  和  的beta分布。進(jìn)行代數(shù)變換后，一個(gè)高效的在線參數(shù)更新可以被推導(dǎo)，其對(duì)于地圖點(diǎn)進(jìn)行概率模型的快速更新。

對(duì)于沒有語義信息的幀，上一項(xiàng)沒有被使用。語義beta分布的參數(shù)  和  被表示通過如下關(guān)系：

 (10)

 (11)

類別后驗(yàn)概率  是所有語義測(cè)量的融合，見等式(12)。與現(xiàn)有融合方法相比[13]，等式(7)導(dǎo)致一個(gè)加權(quán)的語義融合，依賴每個(gè)測(cè)量的匹配精度  。

 (12)

為了估計(jì)描述特征的匹配精度，Hamming距離被用來比較二值描述子。

 (13)

對(duì)于直接法，我們使用兩個(gè)歸一化圖片塊的光度差。

(14)

在我們的應(yīng)用中，我們使用逆深度[4][20]，為了建模無窮遠(yuǎn)點(diǎn)。[20]也展示了逆深度更有可能服從高斯分布。根據(jù)內(nèi)點(diǎn)率，我們決定是否使用（積極點(diǎn)）或者不使用（消極點(diǎn)）地圖點(diǎn)用于位姿估計(jì)。當(dāng)前內(nèi)點(diǎn)率可以被計(jì)算使用(

  (15)