ElasticFusion: Dense SLAM Without A Pose Graph 沒有姿態(tài)圖稠密SLAM
[TOC]
摘要
提出了一種實時密集視覺SLAM的新方法昆庇。我們的系統(tǒng)能夠捕捉全面稠密的全局一致的基于surfelbased的房間尺度環(huán)境的地圖韭寸,使用RGB-D相機以增量的在線方式探索忆谓,沒有任何姿態(tài)圖優(yōu)化或任何后處理步驟。這是通過使用密集的frame-to-model方法進行攝像機跟蹤和基于窗口的表面融合以及通過非剛性表面變形的頻繁模型細化來實現(xiàn)的好渠。我們的方法盡可能多地應(yīng)用局部model-to-model的表面閉環(huán)優(yōu)化,以保持接近地圖分布的模式彤恶,同時利用全局環(huán)路閉合從任意漂移中恢復并保持全局一致性袋倔。
- 構(gòu)建surfel-based的房間尺度的稠密地圖。
- 沒有任何姿態(tài)圖優(yōu)化或任何后處理步驟诅岩。(傳統(tǒng)的 SLAM 算法一般通過不斷優(yōu)化相機軌跡或者特征點的方式讳苦,來提高軌跡估計或者重建的精度,這篇文章采用不斷優(yōu)化重建的 map 的方式吩谦,提高重建和位姿估計的精度鸳谜。)
這是通過使用密集的幀 - 模型攝像機跟蹤和基于窗口的基于表面的融合以及通過非剛性表面變形的頻繁模型細化來實現(xiàn)的。
II.方法概述
我們采用的架構(gòu)通常在實時密集的可視SLAM系統(tǒng)中找到式廷,它們在跟蹤和地圖之間交替
- 基于融合surfel的環(huán)境模型估計咐扭。我們方法的這一組成部分受到Keller[9]等人基于surfel的融合系統(tǒng)的啟發(fā),在第III節(jié)中概述了一些值得注意的差異滑废。
- 對模型最近觀測區(qū)域(模型活動區(qū)域)的數(shù)據(jù)進行跟蹤和融合蝗肪。將一段時間內(nèi)沒有觀察到的地圖較老的部分分割到模型的非活動區(qū)域(不用于跟蹤或數(shù)據(jù)融合)
- 每幀,嘗試將當前估計的相機幀內(nèi)的活動模型部分與同一幀內(nèi)的非活動模型部分標注(register)蠕趁。如果標注成功薛闪,則對舊的非活動模型關(guān)閉了一個循環(huán),并且整個模型沒有嚴格變形俺陋,以反映該標注豁延。然后重新激活地圖中導致該循環(huán)關(guān)閉的非活動部分,以允許在地圖的標注區(qū)域之間進行跟蹤和表面融合(包括surfel剔除).
- 對于全局閉環(huán)檢測腊状,將場景的預測視圖添加到隨機fern編碼數(shù)據(jù)庫[6]术浪。每個幀,嘗試通過此數(shù)據(jù)庫找到匹配的預測視圖寿酌。如果檢測到匹配胰苏,則將視圖一起標記(register)并檢查標記是否與模型的幾何形狀全局一致。如果是這樣醇疼,請在地圖中使用非剛性變形反映此標記硕并,使surface進入全局對齊。
圖2提供了方法概述的主要步驟的可視化秧荆。表面法線疊加在非活動模型上倔毙,用灰度著色;(i)當相機向左移動時,所有數(shù)據(jù)最初都在活動模型中;(ii)隨著時間的推移乙濒,最近沒有看到的地圖區(qū)域被設(shè)置為非活動區(qū)域陕赃。注意突出顯示的區(qū)域;(ii)相機重新檢查地圖的非活動區(qū)域卵蛉,關(guān)閉一個局部循環(huán)并一起記錄地表。先前突出顯示的非活動區(qū)域隨后變?yōu)榛顒訁^(qū)域;攝像機繼續(xù)向右探測么库,更多的回路閉合;繼續(xù)勘探新的地區(qū);攝像機重新進入非活動區(qū)域傻丝,但漂移太遠,無法關(guān)閉局部回路;(vii)此處的失調(diào)很明顯诉儒,紅色箭頭表示從活動點到不活動點的等值點;(vili)觸發(fā)一個全局循環(huán)閉包葡缰,使活動和非活動模型對齊;隨著更多的局部環(huán)路關(guān)閉和非活躍地區(qū)重新活躍,繼續(xù)向右探索;(x)最終完整地圖彩色表面法線顯示潛在的變形圖和采樣相機姿勢在全局循環(huán)關(guān)閉數(shù)據(jù)庫中忱反。
算法流程
ElasticFusion 通過 RGB-D 圖像配準計算位姿泛释,根據(jù)輸入點云利用 ICP 算法和直接法求解位姿
計算相機位姿如果誤差大于設(shè)定閾值,表示跟蹤失敗温算,啟動重定位算法怜校;如果誤差小于設(shè)定閾值,則進入下一部分注竿。
利用 Random Ferns 進行全局的回環(huán)檢測算法(后續(xù)博客單獨介紹)茄茁,檢測是否存在全局的回環(huán),如果存在全局的回環(huán)蔓搞,假設(shè)當前幀為 檢測到和第 ii 幀存在回環(huán);再利用第一部中的跟蹤算法計算當前幀和第 ii 幀之間的位姿随橘,計算得到位姿變換后喂分,在圖像中均勻抽取一些點,建立約束机蔗,優(yōu)化 node 參數(shù)(關(guān)于如何優(yōu)化 map 后續(xù)文章還會展開介紹)蒲祈。
在第 3 部中如果不存在全局的回環(huán),則檢測是否存在局部的回環(huán)(局部回環(huán)檢測算法后續(xù)會展開介紹)萝嘁,如果存在局部的回環(huán)梆掸,則同第 3 步,進行位姿估計牙言,并且建立約束酸钦,優(yōu)化 node 參數(shù)。
計算得到相機位姿后咱枉,將當前幀的點云和重建好的做融合卑硫,融合使用 openGL 的 shading language,如果在存在局部的或者全局的回環(huán)蚕断,在使用 openGL 進行點的融合時候欢伏,將優(yōu)化之后的節(jié)點變量,作用于全部的點亿乳。
融合到全局模型中后硝拧,再用 openGL 投影得到當前視角下可以看到的點,用來對下一幀圖像配準。
III.融合預測的跟蹤
- 場景用一個無序的surfel列表表示(同文獻[9]),其中每個表面具有下面這些屬性:
位置
法線
顏色
權(quán)重 (這里權(quán)重是什么障陶?滋恬??)
半徑 給定點周圍的局部表面區(qū)域咸这,同時最小化可見孔夷恍,計算如文獻[17]/半徑通過場景表面離相機光心的距離的求得,距離越大媳维,面片的半徑越大
初始化時間戳
最后更新時間戳 - 表面處理初始化和深度圖融合和文獻[9]中一樣酿雪,使用地圖進行位姿估計時有兩處不同
- 不僅通過幾何frame-to-model預測深度圖,還通過frame-to-model預測了顏色
- 我們定義了一個時間窗閾值侄刽,它將M劃分為活動和非活動的surfel指黎。(僅標記為活動模型表面的部分被用于相機姿勢估計和深度圖融合。當自上次更新表面以來(即州丹,具有與其相關(guān)聯(lián)的原始深度測量用于融合)的時間大于時醋安,M中的表面被宣布為非活動狀態(tài)。
A.幾何位姿估計
當前幀深度圖(原文這里用的depth map)和來自上一幀的預測active模型深度圖之間的運動參數(shù)墓毒,通過最小化3D back-projected(反投影) 頂點之間的點到平面的成本誤差函數(shù)得到:(2)
是時間t這一幀深度圖中第k個頂點在相機坐標系下的空間位置吓揪,和是建立的地圖中相關(guān)聯(lián)的頂點和向量(第t-1步優(yōu)化完畢得到)。T是從先前相機姿勢到當前相機姿勢的當前估計所计,并且是將李代數(shù)se3映射到對應(yīng)李群SE3中柠辞,代表的應(yīng)該是前一幀在世界坐標系的位置。頂點之間用投影數(shù)據(jù)關(guān)聯(lián)互相關(guān)聯(lián).
該公式表達的含義是用深度圖像估計相機姿態(tài)變換主胧。而優(yōu)化的誤差函數(shù)則是非常正常地將新一幀中對應(yīng)的點(應(yīng)該是表達在空間坐標系下)叭首,按照姿態(tài)變換投影到上一幀位置中去,然后計算它們之間在空間的距離踪栋。法向量的左右是計算距離(法向量方向的投影)
其中所謂上一幀的位置焙格,實際上應(yīng)該叫做當前model的位置。即和都是與地圖相關(guān)的量夷都,因而不帶下標t眷唉。
B.光度位姿估計
在當前的實時彩色圖像和最后一幀的預測活動模型顏色之間,目標是找到最小化像素之間光度誤差(強度差異)成本的運動參數(shù):(3)
式中囤官,T為從前一個相機姿態(tài)到當前相機姿態(tài)轉(zhuǎn)換的當前估計值厢破。注意,方程2和3省略了3維向量和對應(yīng)的齊次4維向量之間的轉(zhuǎn)換(與T相乘時需要)治拿,以簡化符號摩泪。
這個值是從估計的active model部分的,而不是單純的上一幀劫谅,所以這是個frame-model模型见坑。則是新來的一幀彩色圖像嚷掠。T和李群計算,則將其從通過深度取到的深度圖坐標系下的空間點轉(zhuǎn)化成荞驴,世界坐標系下不皆。
C.聯(lián)合優(yōu)化
最小化聯(lián)合成本函數(shù):
這里設(shè)置
IV.Deformation Graph變形圖
- 為了確保地圖中的局部和全局表面一致性,我們在一組surfel M中顯示(反映reflect)出成功的表面閉環(huán)熊楼,這是通過根據(jù)后面第V和VI節(jié)中描述的任何一種閉環(huán)方法提供的表面約束使所有surfel(包括有效和無效)非剛性變形來實現(xiàn)的霹娄。我們采用基于Sumner等人的嵌入變形技術(shù)的空間變形方法。 [23]鲫骗。
- 變形圖由一組節(jié)點和變組成犬耻,每個節(jié)點具有:
- 時間戳,
- 位置,
- 相鄰節(jié)點集合,和相鄰節(jié)點之間由(有向)邊相連。
- 相鄰節(jié)點個數(shù)k执泰,(這里說他們的實驗中使用K=4?這個都是一樣的枕磁?)
- 每個節(jié)點還以矩陣和向量的形式存儲放射變換,默認情況下分別初始化為標識和术吝,當使表面變形時计济,每個節(jié)點的和參數(shù)將根據(jù)表面約束進行優(yōu)化,見IV-C
- 為了將變形圖應(yīng)用于曲面排苍,每個surfel在圖中確定一組影響節(jié)點沦寂,表面的變形位置由下式給出:
表面的變形法線由下式給出:
是一個標量,表示影響節(jié)點在surfel 上淘衙,當n=k時總和為1传藏。
這里是中最近的K+1個節(jié)點的歐幾里得距離。
deformation graph 由一些 nodes 組成幔翰,node 是在重建好的點均勻抽樣得到漩氨,如上圖所示西壮,紅色的表示抽取的 node遗增,黑色的表示重建好的其它的點,node 的數(shù)量和重建好點的數(shù)量成正相關(guān)款青。
- 建設(shè)每一幀都構(gòu)造了一個新的變形圖做修,因為它計算量小,比增量修改現(xiàn)有的變形圖更簡單
Deformation Graph 優(yōu)化
建立連接
- Graph Nodes從重建好的Surfel中均勻抽樣初始化抡草,如結(jié)構(gòu)圖中的紅色的點饰及。
- Graph Nodes按照時間關(guān)系(而不是空間關(guān)系)建立連接(這里時間關(guān)系是指surfel上的時間戳)
- 建設(shè)每一幀都構(gòu)造了一個新的變形圖,因為它計算量小康震,比增量修改現(xiàn)有的變形圖更簡單燎含。初始化一個新的變形圖G,按照時間關(guān)系(而不是空間關(guān)系)建立連接
利用時間順序建立連接的原因:按時間比按空間關(guān)系計算效率上更高腿短,但更重要的是防止表面的時間上不相關(guān)的區(qū)域相互影響(即活動和非活動區(qū)域)
- Map 中其它點(灰色的點)和 Nodes 先按照時間關(guān)系搜索最近的點屏箍,再在最近點周圍按照空間關(guān)系找最近的 Nodes 相連接绘梦。
Deformation Graph 結(jié)構(gòu)和優(yōu)化:
紅色的點是抽取的 Nodes,每個 Node 有待優(yōu)化參數(shù) 和 (待優(yōu)化的參數(shù)是仿射變換)
根據(jù) Local Loop Closure 和 Global Loop Closure 建立的約束赴魁, 優(yōu)化 Nodes 中參數(shù)優(yōu)化卸奉。
再將優(yōu)化的參數(shù)作用于全部的點(Map中全部的點):
位置坐標更新:
法向量更新:
Node權(quán)值:
這里大概地思路是:在Map中均勻抽取一些Node(紅色的點),每個Node都有待優(yōu)化參數(shù)颖御,根據(jù)局部閉環(huán)和全局閉環(huán)建立約束榄棵,優(yōu)化參數(shù),再將優(yōu)化后的參數(shù)作用于Map中全部的點潘拱,對Map中所有點進行優(yōu)化疹鳄。
局部閉環(huán)
- 跟蹤算法求解位姿,根據(jù)位姿將點云融合
- 按照時間將重建好的點劃分成 ACTIVE 和 INACTIVE
- 根據(jù)求解得到的位姿泽铛,從 ACTIVE 和 INACTIVE 分別投影得到兩幅點云
- 配準兩幅點云尚辑,如果可以配準上,則說明存在 Local Loop Closure
大概思路:ACTIVE點是最近被相機拍到的盔腔,INACTICE則是很久沒出現(xiàn)過的杠茬,如果兩部分能有部分配準上,則說明回到原來到過的地方了弛随。
建立約束:
:當前幀的 pose瓢喉。
:通過上一幀 pose 從模型投影得到的點。
:active 點坐標舀透。
: 將投影得到的相機系下的 active 點 align 到世界系下的 inactive 點的 pose栓票,pose 通過將世界系下的 inactive 點設(shè)為配準的模型,將相機系下的 active 點設(shè)為待配準的幀獲得愕够。 為世界系下 inactive 點坐標走贪。
:當前幀的時間
:對應(yīng)的 INACTIVE 點的時間(模型投影點)
約束的含義是將 ACTIVE 點對齊到 INACTIVE 點。
全局閉環(huán)
為什么有了局部閉環(huán)還要全局閉環(huán)?
相機移動距離長時軌跡 drifts 太大惑芭, 用 ACTIVE 和 INACTIVE 點投影得到的點云配準不上坠狡。
回環(huán)檢測算法 Randomized Ferns
算法流程:
- 輸入新圖像,計算Block碼值
- 根據(jù)Block碼值計算和list表中幀之間的相似度
- 相似度判別是否加入關(guān)鍵字或者坐重定位
圖像編碼
- 每幅圖像有m個block編碼遂跟,每個block有n個fern
我理解為這里fern個數(shù)為圖形的通道數(shù)逃沿,這里是rgbd四個通道,所以n=4
- fern計算如下:
其中是閾值幻锁,I為輸入的圖像凯亮,包含通道c和像素點坐標x
- block計算:
為fern值
- 圖像碼值:
判斷幀間相似度
對于一幅新獲取的圖像,首先計算每個 block 的編碼 哄尔,根據(jù)block的編碼假消,在上圖所示的列表中,索引到在 block 處具備相同編碼的圖像 岭接,圖像 和圖像 的相似度 加1富拗。
對于圖像所有的block做上述運算堂鲤,可以計算得到對于存儲的所有的關(guān)鍵幀圖像的相似度。
通過相似度判別當前幀是否加入做關(guān)鍵幀(對于所有存儲的關(guān)鍵幀相似度小于一定閾值)媒峡,或者存在回環(huán)(和某一幀圖像相似度大于一定閾值)瘟栖,還可以通過和相似度大的圖像配準,進行重定位谅阿。
建立約束
:當前幀 pose半哟。
:通過上一幀 pose 從模型投影得到的點。
:檢測到的回環(huán)幀的 pose签餐。
: 當前幀時間戳寓涨。
:回環(huán)幀時間戳。
含義:將當前幀點云對齊到 Fern 的點云
優(yōu)化
前面用全局和局部回函檢測建立的約束氯檐,通過下面目標函數(shù)對節(jié)點的參數(shù) 和 進行優(yōu)化戒良,目標函數(shù)由四部分組成:
評估
從軌跡估計、曲面重建精度和計算性能等方面對系統(tǒng)性能進行了定性和定量評估冠摄。
A.軌跡跟蹤
與其他四種最先進的基于RGB-D的SLAM系統(tǒng)DVO SLAM糯崎、RGB-D SLAM 、 MRSMap 和Kintinuous進行了對比河泳,使用絕對軌跡(ATE)均方根誤差度量(RMSE)沃呢,結(jié)果表明,我們的軌跡估計性能與現(xiàn)有的最先進的系統(tǒng)持平或更好
B.表面估計
在Handa等人的ICL-NUIM數(shù)據(jù)集上評估了表面重建結(jié)果拆挥。和a中列出的SLAM系統(tǒng)相比薄霜,
我們還展示了一些以手持方式捕獲的數(shù)據(jù)集的定性結(jié)果,展示了系統(tǒng)的通用性纸兔。每個數(shù)據(jù)集的統(tǒng)計數(shù)據(jù)列在表IV中惰瓜。
C.計算性能
為了分析該系統(tǒng)的計算性能,我們提供了一個平均幀處理時間在整個酒店序列的圖表汉矿。測試平臺是一臺臺式機崎坊,CPU為Intel Core i7-4930K,主頻為3.4GHz负甸。32GB內(nèi)存和nVidia GeForce GTX 780 Ti GPU與3GB內(nèi)存流强。系統(tǒng)的執(zhí)行時間隨著map中surfels的數(shù)量而增加痹届∩氪總體平均每幀31ms擴展到峰值平均45ms,意味著最壞情況處理頻率為22Hz队腐。這是在廣泛接受的最小頻率范圍內(nèi)的融合密集SLAM算法.蚕捉,并根據(jù)我們的定性結(jié)果顯示,足以進行實時操作柴淘。