ElasticFusion: Dense SLAM Without A Pose Graph 沒有姿態(tài)圖稠密SLAM
[TOC]
摘要
提出了一種實(shí)時(shí)密集視覺SLAM的新方法。我們的系統(tǒng)能夠捕捉全面稠密的全局一致的基于surfelbased的房間尺度環(huán)境的地圖,使用RGB-D相機(jī)以增量的在線方式探索,沒有任何姿態(tài)圖優(yōu)化或任何后處理步驟。這是通過使用密集的frame-to-model方法進(jìn)行攝像機(jī)跟蹤和基于窗口的表面融合以及通過非剛性表面變形的頻繁模型細(xì)化來實(shí)現(xiàn)的。我們的方法盡可能多地應(yīng)用局部model-to-model的表面閉環(huán)優(yōu)化,以保持接近地圖分布的模式,同時(shí)利用全局環(huán)路閉合從任意漂移中恢復(fù)并保持全局一致性。
- 構(gòu)建surfel-based的房間尺度的稠密地圖。
- 沒有任何姿態(tài)圖優(yōu)化或任何后處理步驟。(傳統(tǒng)的 SLAM 算法一般通過不斷優(yōu)化相機(jī)軌跡或者特征點(diǎn)的方式,來提高軌跡估計(jì)或者重建的精度,這篇文章采用不斷優(yōu)化重建的 map 的方式,提高重建和位姿估計(jì)的精度。)
這是通過使用密集的幀 - 模型攝像機(jī)跟蹤和基于窗口的基于表面的融合以及通過非剛性表面變形的頻繁模型細(xì)化來實(shí)現(xiàn)的。
II.方法概述
我們采用的架構(gòu)通常在實(shí)時(shí)密集的可視SLAM系統(tǒng)中找到,它們?cè)诟櫤偷貓D之間交替
- 基于融合surfel的環(huán)境模型估計(jì)。我們方法的這一組成部分受到Keller[9]等人基于surfel的融合系統(tǒng)的啟發(fā),在第III節(jié)中概述了一些值得注意的差異。
- 對(duì)模型最近觀測(cè)區(qū)域(模型活動(dòng)區(qū)域)的數(shù)據(jù)進(jìn)行跟蹤和融合。將一段時(shí)間內(nèi)沒有觀察到的地圖較老的部分分割到模型的非活動(dòng)區(qū)域(不用于跟蹤或數(shù)據(jù)融合)
- 每幀,嘗試將當(dāng)前估計(jì)的相機(jī)幀內(nèi)的活動(dòng)模型部分與同一幀內(nèi)的非活動(dòng)模型部分標(biāo)注(register)。如果標(biāo)注成功,則對(duì)舊的非活動(dòng)模型關(guān)閉了一個(gè)循環(huán),并且整個(gè)模型沒有嚴(yán)格變形,以反映該標(biāo)注。然后重新激活地圖中導(dǎo)致該循環(huán)關(guān)閉的非活動(dòng)部分,以允許在地圖的標(biāo)注區(qū)域之間進(jìn)行跟蹤和表面融合(包括surfel剔除).
- 對(duì)于全局閉環(huán)檢測(cè),將場(chǎng)景的預(yù)測(cè)視圖添加到隨機(jī)fern編碼數(shù)據(jù)庫[6]。每個(gè)幀,嘗試通過此數(shù)據(jù)庫找到匹配的預(yù)測(cè)視圖。如果檢測(cè)到匹配,則將視圖一起標(biāo)記(register)并檢查標(biāo)記是否與模型的幾何形狀全局一致。如果是這樣,請(qǐng)?jiān)诘貓D中使用非剛性變形反映此標(biāo)記,使surface進(jìn)入全局對(duì)齊。
圖2提供了方法概述的主要步驟的可視化。表面法線疊加在非活動(dòng)模型上,用灰度著色;(i)當(dāng)相機(jī)向左移動(dòng)時(shí),所有數(shù)據(jù)最初都在活動(dòng)模型中;(ii)隨著時(shí)間的推移,最近沒有看到的地圖區(qū)域被設(shè)置為非活動(dòng)區(qū)域。注意突出顯示的區(qū)域;(ii)相機(jī)重新檢查地圖的非活動(dòng)區(qū)域,關(guān)閉一個(gè)局部循環(huán)并一起記錄地表。先前突出顯示的非活動(dòng)區(qū)域隨后變?yōu)榛顒?dòng)區(qū)域;攝像機(jī)繼續(xù)向右探測(cè),更多的回路閉合;繼續(xù)勘探新的地區(qū);攝像機(jī)重新進(jìn)入非活動(dòng)區(qū)域,但漂移太遠(yuǎn),無法關(guān)閉局部回路;(vii)此處的失調(diào)很明顯,紅色箭頭表示從活動(dòng)點(diǎn)到不活動(dòng)點(diǎn)的等值點(diǎn);(vili)觸發(fā)一個(gè)全局循環(huán)閉包,使活動(dòng)和非活動(dòng)模型對(duì)齊;隨著更多的局部環(huán)路關(guān)閉和非活躍地區(qū)重新活躍,繼續(xù)向右探索;(x)最終完整地圖彩色表面法線顯示潛在的變形圖和采樣相機(jī)姿勢(shì)在全局循環(huán)關(guān)閉數(shù)據(jù)庫中。
算法流程
ElasticFusion 通過 RGB-D 圖像配準(zhǔn)計(jì)算位姿,根據(jù)輸入點(diǎn)云利用 ICP 算法和直接法求解位姿
計(jì)算相機(jī)位姿如果誤差大于設(shè)定閾值,表示跟蹤失敗,啟動(dòng)重定位算法;如果誤差小于設(shè)定閾值,則進(jìn)入下一部分。
利用 Random Ferns 進(jìn)行全局的回環(huán)檢測(cè)算法(后續(xù)博客單獨(dú)介紹),檢測(cè)是否存在全局的回環(huán),如果存在全局的回環(huán),假設(shè)當(dāng)前幀為
檢測(cè)到和第 ii 幀存在回環(huán);再利用第一部中的跟蹤算法計(jì)算當(dāng)前幀和第 ii 幀之間的位姿,計(jì)算得到位姿變換后,在圖像中均勻抽取一些點(diǎn),建立約束,優(yōu)化 node 參數(shù)(關(guān)于如何優(yōu)化 map 后續(xù)文章還會(huì)展開介紹)。
在第 3 部中如果不存在全局的回環(huán),則檢測(cè)是否存在局部的回環(huán)(局部回環(huán)檢測(cè)算法后續(xù)會(huì)展開介紹),如果存在局部的回環(huán),則同第 3 步,進(jìn)行位姿估計(jì),并且建立約束,優(yōu)化 node 參數(shù)。
計(jì)算得到相機(jī)位姿后,將當(dāng)前幀的點(diǎn)云和重建好的做融合,融合使用 openGL 的 shading language,如果在存在局部的或者全局的回環(huán),在使用 openGL 進(jìn)行點(diǎn)的融合時(shí)候,將優(yōu)化之后的節(jié)點(diǎn)變量,作用于全部的點(diǎn)。
融合到全局模型中后,再用 openGL 投影得到當(dāng)前視角下可以看到的點(diǎn),用來對(duì)下一幀圖像配準(zhǔn)。
III.融合預(yù)測(cè)的跟蹤
- 場(chǎng)景用一個(gè)無序的surfel列表
表示(同文獻(xiàn)[9]),其中每個(gè)表面
具有下面這些屬性:
位置
法線
顏色
權(quán)重(這里權(quán)重是什么???)
半徑給定點(diǎn)周圍的局部表面區(qū)域,同時(shí)最小化可見孔,計(jì)算如文獻(xiàn)[17]/半徑通過場(chǎng)景表面離相機(jī)光心的距離的求得,距離越大,面片的半徑越大
初始化時(shí)間戳
最后更新時(shí)間戳 - 表面處理初始化和深度圖融合和文獻(xiàn)[9]中一樣,使用地圖進(jìn)行位姿估計(jì)時(shí)有兩處不同
- 不僅通過幾何frame-to-model預(yù)測(cè)深度圖,還通過frame-to-model預(yù)測(cè)了顏色
- 我們定義了一個(gè)時(shí)間窗閾值
,它將M劃分為活動(dòng)和非活動(dòng)的surfel。(僅標(biāo)記為活動(dòng)模型表面的部分被用于相機(jī)姿勢(shì)估計(jì)和深度圖融合。當(dāng)自上次更新表面以來(即,具有與其相關(guān)聯(lián)的原始深度測(cè)量用于融合)的時(shí)間大于時(shí)
,M中的表面被宣布為非活動(dòng)狀態(tài)。
A.幾何位姿估計(jì)
當(dāng)前幀深度圖(原文這里用的depth map)和來自上一幀的預(yù)測(cè)active模型深度圖
之間的運(yùn)動(dòng)參數(shù)
,通過最小化3D back-projected(反投影) 頂點(diǎn)之間的點(diǎn)到平面的成本誤差函數(shù)得到:(2)
是時(shí)間t這一幀深度圖中第k個(gè)頂點(diǎn)在相機(jī)坐標(biāo)系下的空間位置,
和
是建立的地圖中相關(guān)聯(lián)的頂點(diǎn)和向量(第t-1步優(yōu)化完畢得到)。T是從先前相機(jī)姿勢(shì)到當(dāng)前相機(jī)姿勢(shì)的當(dāng)前估計(jì),并且
是將李代數(shù)se3映射到對(duì)應(yīng)李群SE3中,代表的應(yīng)該是前一幀在世界坐標(biāo)系的位置。頂點(diǎn)之間用投影數(shù)據(jù)關(guān)聯(lián)互相關(guān)聯(lián).
該公式表達(dá)的含義是用深度圖像估計(jì)相機(jī)姿態(tài)變換。而優(yōu)化的誤差函數(shù)則是非常正常地將新一幀中對(duì)應(yīng)的點(diǎn)(應(yīng)該是表達(dá)在空間坐標(biāo)系下),按照姿態(tài)變換投影到上一幀位置中去,然后計(jì)算它們之間在空間的距離。法向量的左右是計(jì)算距離(法向量方向的投影)
其中所謂上一幀的位置,實(shí)際上應(yīng)該叫做當(dāng)前model的位置。即和
都是與地圖相關(guān)的量,因而不帶下標(biāo)t。
B.光度位姿估計(jì)
在當(dāng)前的實(shí)時(shí)彩色圖像和最后一幀的預(yù)測(cè)活動(dòng)模型顏色
之間,目標(biāo)是找到最小化像素之間光度誤差(強(qiáng)度差異)成本的運(yùn)動(dòng)參數(shù)
:(3)
式中,T為從前一個(gè)相機(jī)姿態(tài)到當(dāng)前相機(jī)姿態(tài)轉(zhuǎn)換的當(dāng)前估計(jì)值。注意,方程2和3省略了3維向量和對(duì)應(yīng)的齊次4維向量之間的轉(zhuǎn)換(與T相乘時(shí)需要),以簡(jiǎn)化符號(hào)。
這個(gè)值是從估計(jì)的active model部分的,而不是單純的上一幀,所以這是個(gè)frame-model模型。
則是新來的一幀彩色圖像。T和李群計(jì)算,則將其從通過深度取到的深度圖坐標(biāo)系下的空間點(diǎn)轉(zhuǎn)化成,世界坐標(biāo)系下。
C.聯(lián)合優(yōu)化
最小化聯(lián)合成本函數(shù):
這里設(shè)置
IV.Deformation Graph變形圖
- 為了確保地圖中的局部和全局表面一致性,我們?cè)谝唤Msurfel M中顯示(反映reflect)出成功的表面閉環(huán),這是通過根據(jù)后面第V和VI節(jié)中描述的任何一種閉環(huán)方法提供的表面約束使所有surfel(包括有效和無效)非剛性變形來實(shí)現(xiàn)的。我們采用基于Sumner等人的嵌入變形技術(shù)的空間變形方法。 [23]。
- 變形圖由一組節(jié)點(diǎn)和變組成,每個(gè)節(jié)點(diǎn)
具有:
- 時(shí)間戳
,
- 位置
,
- 相鄰節(jié)點(diǎn)集合
,和相鄰節(jié)點(diǎn)之間由(有向)邊相連。
- 相鄰節(jié)點(diǎn)個(gè)數(shù)k,
(這里說他們的實(shí)驗(yàn)中使用K=4?這個(gè)都是一樣的?)
- 每個(gè)節(jié)點(diǎn)還以
矩陣
和
向量
的形式存儲(chǔ)放射變換,默認(rèn)情況下分別初始化為標(biāo)識(shí)和,當(dāng)使表面變形時(shí),每個(gè)節(jié)點(diǎn)的
和參數(shù)
將根據(jù)表面約束進(jìn)行優(yōu)化,見IV-C
- 時(shí)間戳
- 為了將變形圖應(yīng)用于曲面,每個(gè)surfel
在圖
中確定一組影響節(jié)點(diǎn),表面的變形位置由下式給出:
表面的變形法線由下式給出:
是一個(gè)標(biāo)量,表示影響節(jié)點(diǎn)
在surfel
上,當(dāng)n=k時(shí)總和為1。
這里是
中最近的K+1個(gè)節(jié)點(diǎn)的歐幾里得距離。
deformation graph 由一些 nodes 組成,node 是在重建好的點(diǎn)均勻抽樣得到,如上圖所示,紅色的表示抽取的 node,黑色的表示重建好的其它的點(diǎn),node 的數(shù)量和重建好點(diǎn)的數(shù)量成正相關(guān)。
- 建設(shè)每一幀都構(gòu)造了一個(gè)新的變形圖,因?yàn)樗?jì)算量小,比增量修改現(xiàn)有的變形圖更簡(jiǎn)單
Deformation Graph 優(yōu)化
建立連接
- Graph Nodes從重建好的Surfel中均勻抽樣初始化,如結(jié)構(gòu)圖中的紅色的點(diǎn)。
- Graph Nodes按照時(shí)間關(guān)系(而不是空間關(guān)系)建立連接(這里時(shí)間關(guān)系是指surfel上的時(shí)間戳
)
- 建設(shè)每一幀都構(gòu)造了一個(gè)新的變形圖,因?yàn)樗?jì)算量小,比增量修改現(xiàn)有的變形圖更簡(jiǎn)單。初始化一個(gè)新的變形圖G,按照時(shí)間關(guān)系(而不是空間關(guān)系)建立連接
利用時(shí)間順序建立連接的原因:按時(shí)間比按空間關(guān)系計(jì)算效率上更高,但更重要的是防止表面的時(shí)間上不相關(guān)的區(qū)域相互影響(即活動(dòng)和非活動(dòng)區(qū)域)
- Map 中其它點(diǎn)(灰色的點(diǎn))和 Nodes 先按照時(shí)間關(guān)系搜索最近的點(diǎn),再在最近點(diǎn)周圍按照空間關(guān)系找最近的 Nodes 相連接。
Deformation Graph 結(jié)構(gòu)和優(yōu)化:
紅色的點(diǎn)是抽取的 Nodes,每個(gè) Node 有待優(yōu)化參數(shù)
和
(待優(yōu)化的參數(shù)是仿射變換)
根據(jù) Local Loop Closure 和 Global Loop Closure 建立的約束, 優(yōu)化 Nodes 中參數(shù)優(yōu)化。
再將優(yōu)化的參數(shù)作用于全部的點(diǎn)(Map中全部的點(diǎn)):
位置坐標(biāo)更新:
法向量更新:
Node權(quán)值:
這里大概地思路是:在Map中均勻抽取一些Node(紅色的點(diǎn)),每個(gè)Node都有待優(yōu)化參數(shù),根據(jù)局部閉環(huán)和全局閉環(huán)建立約束,優(yōu)化參數(shù),再將優(yōu)化后的參數(shù)作用于Map中全部的點(diǎn),對(duì)Map中所有點(diǎn)進(jìn)行優(yōu)化。
局部閉環(huán)
- 跟蹤算法求解位姿,根據(jù)位姿將點(diǎn)云融合
- 按照時(shí)間將重建好的點(diǎn)劃分成 ACTIVE 和 INACTIVE
- 根據(jù)求解得到的位姿,從 ACTIVE 和 INACTIVE 分別投影得到兩幅點(diǎn)云
- 配準(zhǔn)兩幅點(diǎn)云,如果可以配準(zhǔn)上,則說明存在 Local Loop Closure
大概思路:ACTIVE點(diǎn)是最近被相機(jī)拍到的,INACTICE則是很久沒出現(xiàn)過的,如果兩部分能有部分配準(zhǔn)上,則說明回到原來到過的地方了。
建立約束:
:當(dāng)前幀的 pose。
:通過上一幀 pose 從模型投影得到的點(diǎn)。
:active 點(diǎn)坐標(biāo)。
: 將投影得到的相機(jī)系下的 active 點(diǎn) align 到世界系下的 inactive 點(diǎn)的 pose,pose 通過將世界系下的 inactive 點(diǎn)設(shè)為配準(zhǔn)的模型,將相機(jī)系下的 active 點(diǎn)設(shè)為待配準(zhǔn)的幀獲得。
為世界系下 inactive 點(diǎn)坐標(biāo)。
:當(dāng)前幀的時(shí)間
:對(duì)應(yīng)的 INACTIVE 點(diǎn)的時(shí)間(模型投影點(diǎn))
約束的含義是將 ACTIVE 點(diǎn)對(duì)齊到 INACTIVE 點(diǎn)。
全局閉環(huán)
為什么有了局部閉環(huán)還要全局閉環(huán)?
相機(jī)移動(dòng)距離長時(shí)軌跡 drifts 太大, 用 ACTIVE 和 INACTIVE 點(diǎn)投影得到的點(diǎn)云配準(zhǔn)不上。
回環(huán)檢測(cè)算法 Randomized Ferns
算法流程:
- 輸入新圖像,計(jì)算Block碼值
- 根據(jù)Block碼值計(jì)算和list表中幀之間的相似度
- 相似度判別是否加入關(guān)鍵字或者坐重定位
圖像編碼
- 每幅圖像有m個(gè)block編碼,每個(gè)block有n個(gè)fern
我理解為這里fern個(gè)數(shù)為圖形的通道數(shù),這里是rgbd四個(gè)通道,所以n=4
- fern計(jì)算如下:
其中
是閾值,I為輸入的圖像,
包含通道c和像素點(diǎn)坐標(biāo)x
- block計(jì)算:
為fern值
- 圖像碼值:
判斷幀間相似度
對(duì)于一幅新獲取的圖像,首先計(jì)算每個(gè) block 的編碼
,根據(jù)block的編碼,在上圖所示的列表中,索引到在 block
處具備相同編碼的圖像
,圖像
和圖像
的相似度
加1。
對(duì)于圖像所有的block做上述運(yùn)算,可以計(jì)算得到對(duì)于存儲(chǔ)的所有的關(guān)鍵幀圖像的相似度。
通過相似度判別當(dāng)前幀是否加入做關(guān)鍵幀(對(duì)于所有存儲(chǔ)的關(guān)鍵幀相似度小于一定閾值),或者存在回環(huán)(和某一幀圖像相似度大于一定閾值),還可以通過和相似度大的圖像配準(zhǔn),進(jìn)行重定位。
建立約束
:當(dāng)前幀 pose。
:通過上一幀 pose 從模型投影得到的點(diǎn)。
:檢測(cè)到的回環(huán)幀的 pose。
: 當(dāng)前幀時(shí)間戳。
:回環(huán)幀時(shí)間戳。
含義:將當(dāng)前幀點(diǎn)云對(duì)齊到 Fern 的點(diǎn)云
優(yōu)化
前面用全局和局部回函檢測(cè)建立的約束,通過下面目標(biāo)函數(shù)對(duì)節(jié)點(diǎn)的參數(shù) 和
進(jìn)行優(yōu)化,目標(biāo)函數(shù)由四部分組成:
評(píng)估
從軌跡估計(jì)、曲面重建精度和計(jì)算性能等方面對(duì)系統(tǒng)性能進(jìn)行了定性和定量評(píng)估。
A.軌跡跟蹤
與其他四種最先進(jìn)的基于RGB-D的SLAM系統(tǒng)DVO SLAM、RGB-D SLAM 、 MRSMap 和Kintinuous進(jìn)行了對(duì)比,使用絕對(duì)軌跡(ATE)均方根誤差度量(RMSE),結(jié)果表明,我們的軌跡估計(jì)性能與現(xiàn)有的最先進(jìn)的系統(tǒng)持平或更好
B.表面估計(jì)
在Handa等人的ICL-NUIM數(shù)據(jù)集上評(píng)估了表面重建結(jié)果。和a中列出的SLAM系統(tǒng)相比,
我們還展示了一些以手持方式捕獲的數(shù)據(jù)集的定性結(jié)果,展示了系統(tǒng)的通用性。每個(gè)數(shù)據(jù)集的統(tǒng)計(jì)數(shù)據(jù)列在表IV中。
C.計(jì)算性能
為了分析該系統(tǒng)的計(jì)算性能,我們提供了一個(gè)平均幀處理時(shí)間在整個(gè)酒店序列的圖表。測(cè)試平臺(tái)是一臺(tái)臺(tái)式機(jī),CPU為Intel Core i7-4930K,主頻為3.4GHz。32GB內(nèi)存和nVidia GeForce GTX 780 Ti GPU與3GB內(nèi)存。系統(tǒng)的執(zhí)行時(shí)間隨著map中surfels的數(shù)量而增加。總體平均每幀31ms擴(kuò)展到峰值平均45ms,意味著最壞情況處理頻率為22Hz。這是在廣泛接受的最小頻率范圍內(nèi)的融合密集SLAM算法.,并根據(jù)我們的定性結(jié)果顯示,足以進(jìn)行實(shí)時(shí)操作。