一種視覺SLAM單目半稠密建圖方法的實現(xiàn)

尚任

摘?要：SLAM即同時定位與地圖構(gòu)建，一直是機(jī)器人和計算機(jī)視覺的研究熱點。尤其是視覺SLAM技術(shù)，21世紀(jì)以來在理論和實踐上均取得了明顯的突破，已逐步邁向市場應(yīng)用。建圖作為SLAM的兩大目標(biāo)之一，可以滿足更多的應(yīng)用需求。本文在給定相機(jī)軌跡的情況下，提出一種視覺SLAM單目半稠密建圖方法，利用極線搜索和塊匹配技術(shù)，加入圖像變換和逆深度高斯深度濾波器處理，以期避免單目稠密建圖嚴(yán)重依賴紋理、計算量大的缺點，提高單目半稠密建圖的準(zhǔn)確性和魯棒性。經(jīng)測試顯示，改進(jìn)的單目半稠密建圖方法在檢測梯度變化明顯像素點上更加準(zhǔn)確，深度估計的平均誤差和平方誤差分別減少了9%和47%，是一種可行有效的視覺SLAM單目半稠密建圖解決方案。

關(guān)鍵詞： 視覺SLAM;建圖;單目;極限搜索;塊匹配;逆深度

文章編號： 2095-2163（2021）01-0089-05 中圖分類號：TP391 文獻(xiàn)標(biāo)志碼：A

【Abstract】Simultaneous Localization and Mapping（SLAM） is the hot spot of Robots and Computer Vision. Visual SLAM had made a breakthrough since 21st century and applied to the market. Mapping is one of the targets of SLAM. Given location and poses of the monocular camera， the paper proposes a Visual SLAM semi-dense mapping method. Using searching on the epipolar line and block-matching， image transformation and inverse depth filter have been used in order to avoid depending on textures and large amounts of calculation. In this way， the accuracy and robustness of semi-dense mapping has been improved. From the testing result， the improved visual SLAM semi-dense mapping method on monocular camera has a better performance on gradient detecting of pixels. The average error and square error on depth estimation decrease about 9% and 47%. It is a feasible and effective solution to rebuild semi-dense mapping on monocular camera.

【Key words】visual SLAM; mapping; monocular; extreme search; block match; inverse depth

0 引?言

同時定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM），于1986年提出[1]，是指搭載特定傳感器的主體，在沒有環(huán)境先驗知識的情況下，在運動中估計自身軌跡，并同時建立環(huán)境的模型[2]。在視覺SLAM中，特定傳感器以視覺傳感器（即相機(jī)）為主體，研究者則需要根據(jù)連續(xù)拍攝的多幅圖像，推斷相機(jī)的運動以及周圍的環(huán)境。

定位與建圖是SLAM的兩大目標(biāo)。在視覺SLAM中，建圖與定位同樣重要，建圖還具有很多其他的應(yīng)用需求。對于單目相機(jī)的地圖構(gòu)建，國內(nèi)外已涌現(xiàn)了不少的研究成果，提供了很多優(yōu)秀的開源SLAM方案。PTAM[3]的問世是視覺SLAM發(fā)展過程中的重要事件，首次提出并實現(xiàn)了跟蹤與建圖過程的并行化。ORB-SLAM[4]是繼PTAM之后業(yè)內(nèi)著名的視覺SLAM系統(tǒng)，代表著主流特征點SLAM的一個高峰。ORB-SLAM能夠支持單目、雙目、RGB-D三種模式，有著良好的泛用性，但只能用于構(gòu)建稀疏地圖，在建圖方面顯得過于重量級。LSD-SLAM[5-6]標(biāo)志著單目直接法在SLAM中的成功應(yīng)用，其核心貢獻(xiàn)是將直接法應(yīng)用到了半稠密單目地圖重建中。還有很多單目SLAM開源系統(tǒng)（如SVO[7]、DSO[8]等等）。此外，羅鴻城[9]、謝瑒[10]、張劍華等人[11]、Vogiztzis等人[12]均對單目建圖進(jìn)行了探索。

本文旨在提出一種單目半稠密建圖的方法，研究中在圖像間仿射變換預(yù)處理后，利用塊匹配的方法進(jìn)行極線搜索[13]，并將像素點的逆深度進(jìn)行高斯融合，采用高斯分布的逆深度濾波器的方式估計像素點的深度，具有良好的準(zhǔn)確性和魯棒性。

1 SLAM系統(tǒng)介紹

研究時通常使用便攜式的傳感器來完成SLAM，在未知環(huán)境中進(jìn)行實時建模[14]。視覺SLAM主要是指如何用相機(jī)解決定位和建圖問題。按照工作方式的不同，相機(jī)可以分為單目（Monocular）、雙目（Stereo）和深度（RGB-D）三大類。此外，視覺SLAM中還可使用全景相機(jī)[15]、Event相機(jī)[16]等不同種類，但迄至目前還未成為主流，僅會將其應(yīng)用在部分場景中。

在典型的視覺SLAM框架中，主要分為前端視覺里程計（Visual Odometry）、后端優(yōu)化（Optimization）、回環(huán)檢測（Loop Closure Detection）和建圖（Mapping）四個模塊。對此擬做闡釋分述如下。

（1）視覺里程計。根據(jù)相鄰圖像的信息估計出粗略的相機(jī)運動以及局部地圖，處理的結(jié)果將作為后端優(yōu)化的初始值。視覺里程計的算法主要分為兩大類：特征點法和直接法。基于特征點法的前端，直到現(xiàn)在也被公認(rèn)為視覺里程計的主流方法。其中，特征點法穩(wěn)定性強(qiáng)，對光照和動態(tài)物體不敏感，是目前比較成熟的解決方案。但特征點法中，關(guān)鍵點的提取與描述子的計算非常耗時，忽略了除特征點以外的所有信息，而且在特征缺失區(qū)域效果并不理想。直接法根據(jù)像素的亮度信息估計相機(jī)的運動，克服了特征點法的不足，但由于灰度不變假設(shè)，對光照、環(huán)境等外界條件敏感，穩(wěn)定性相對較差。

（2）后端優(yōu)化。負(fù)責(zé)優(yōu)化整個問題，將視覺里程計估計的相機(jī)位姿以及回環(huán)檢測的信息進(jìn)行優(yōu)化，返回優(yōu)化后的結(jié)果，得到全局一致的軌跡和地圖。后端優(yōu)化部分主要有2種處理方法：批量（Batch）處理的非線性優(yōu)化方法和漸進(jìn)（Incremental）處理的濾波器方法。目前，視覺SLAM的主流是非線性優(yōu)化方法。以非線性優(yōu)化為主的后端考慮當(dāng)前狀態(tài)與之前所有狀態(tài)的關(guān)系，在同等計算量的情況下，能夠取得更好的精度和魯棒性[17]，但往往計算量過大，不適用于計算能力有限的平臺。以擴(kuò)展卡爾曼濾波為代表的濾波器方法，僅關(guān)注當(dāng)前時刻和前一時刻的情況，雖然精度和魯棒性上不如非線性優(yōu)化，但是在計算資源受限，或待估計量較簡單的場合，濾波器方法仍不失為一種有效的方式。

（3）回環(huán)檢測。判斷傳感器是否經(jīng)過先前位置，將檢測到的回環(huán)信息提供給后端進(jìn)行處理。目前，回環(huán)檢測用得最多的方法是基于外觀的方法，即僅根據(jù)2幅圖像的相似性確定回環(huán)檢測關(guān)系。較為經(jīng)典的是詞袋模型，該模型是一個非監(jiān)督學(xué)習(xí)的過程。但是深度學(xué)習(xí)方法由于在學(xué)習(xí)與無監(jiān)督聚類上的優(yōu)異表現(xiàn)，其在性能方面則有望勝過目前的主流方法。

（4）建圖模塊。根據(jù)估計的軌跡，建立與需求對應(yīng)的地圖。建圖是SLAM的兩大目標(biāo)之一。根據(jù)使用像素的數(shù)量，構(gòu)建的地圖分為3種：稀疏地圖、稠密地圖和半稠密地圖?？偟貋碚f，稀疏地圖只建模感興趣的部分，稠密地圖將建模所有看到過的部分，半稠密地圖所建模的像素數(shù)量介于稀疏地圖和稠密地圖之間。地圖的用途可以歸納為5點[14]，分別是：定位、導(dǎo)航、避障、重建和交互。其中，定位需要用到稀疏地圖，而導(dǎo)航、避障、重建則要用到稠密地圖。

2 半稠密直接法

對于單目建圖，特征點法和直接法均可以重建稀疏地圖，而半稠密地圖和稠密地圖由于沒有描述子，只能采用直接法重建。構(gòu)建稀疏地圖只能滿足定位需求，應(yīng)用有限，構(gòu)建稠密地圖雖然可以滿足地圖導(dǎo)航、避障和重建的應(yīng)用需求，但對物體紋理有強(qiáng)烈的依賴性。故本文采用單目半稠密建圖的折中方式。建圖需要估計或者獲取空間點的位置和深度。根據(jù)空間點深度的來源，可以把直接法分成3類：

（1）稀疏直接法，即空間點來自于稀疏關(guān)鍵點，不必計算描述子，速度最快，但只能計算稀疏的重構(gòu)。

（2）半稠密直接法，即空間點來自部分像素，考慮到像素梯度為零的像素點對計算運動增量沒有任何貢獻(xiàn)，只使用帶有梯度的像素點，能夠重構(gòu)一個半稠密結(jié)構(gòu)。

（3）稠密直接法，即空間點來自于所有像素，需要計算所有像素，多數(shù)不能在現(xiàn)有CPU上實時計算，需要GPU加速。而且由于使用了梯度不明顯的像素點，重構(gòu)稠密地圖的效果難以保證[14]。

本文采用半稠密直接法作為單目半稠密重建的重要環(huán)節(jié)，既保證了單目相機(jī)建圖實時性的要求，又保證了建圖結(jié)果的準(zhǔn)確性。

3 圖像匹配

本文沿用了直接法的思想，利用極限搜索和塊匹配技術(shù)確定一幅圖像中某像素出現(xiàn)在其他圖像里的位置。當(dāng)能夠確定某個像素在各個圖像中的位置，就可以像特征點法那樣，利用三角測量法確定對應(yīng)的深度。研究內(nèi)容詳見如下。

3.1 對極幾何與極線搜索

單目SLAM僅已知二維像素坐標(biāo)。對于稀疏地圖的構(gòu)建，可以利用特征匹配得到若干對配對好的匹配點，從而通過這些二維像素點的對應(yīng)關(guān)系，恢復(fù)出在兩幀之間相機(jī)的運動。該問題可用對極幾何解決。

而對于半稠密地圖和稠密地圖的構(gòu)建，需要對每個或大部分像素點做匹配，這種情況下，沒有描述子的存在，第一幅圖像上的像素點只能在極線上搜索，以找到其在第二幅圖像上比較相似的點。即沿著第二幅圖像中的極線，逐個比較每個像素與第一幅圖像像素點的相似程度。這種通過比較像素亮度來確定圖像間匹配，從而估計相機(jī)運動的做法，與視覺里程計中的直接法思想相同。

3.2 塊匹配

為避免直接法中單個像素沒有區(qū)分度且灰度值不變假設(shè)過強(qiáng)的弊端，一種有效的方法是以圖像塊為計算單位，而不是單個像素點。研究時，可在第一幅圖像待匹配像素點p1周圍取一個大小為n×n的小塊，假設(shè)在不同圖像間整個小塊灰度值不變，比較相同大小像素塊的亮度相對于比較單個像素的亮度要穩(wěn)定可靠得多。對于單目相機(jī)的半稠密地圖和稠密地圖的構(gòu)建，可在極線上取很多大小相等的像素塊進(jìn)行比較，使極線搜索的結(jié)果有更好的準(zhǔn)確性和魯棒性。

計算像素塊之間差異的方法有若干種，其中將每個像素塊均值去掉的處理方法更為可靠。常見的計算方法有SAD、SSD、NCC等[18]，去均值后，稱為去均值的SAD、去均值的SSD、去均值的NCC等等。研究中，不妨將p1周圍的去均值像素塊記為Ai，i=1，…，n，把極線上的n個去均值像素塊記為Bi，i=1，…，n。去均值的NCC（即去均值的歸一化互相關(guān)）計算公式為：

本文采用去均值的NCC來計算極線上像素塊的相似性度量，在極限搜索中將得到一個沿著極線的NCC分布。由于圖像的非凸性質(zhì)，在搜索距離較長的情況下，NCC分布通常會得到一個非凸函數(shù)，即這個分布存在很多峰值[12]。這種情況下，可使用概率分布來描述像素塊的深度值，而不是用某個單一數(shù)值來描述深度。

3.3 圖像變換

在塊匹配中，研究假設(shè)像素塊在相機(jī)運動時保持不變。這個假設(shè)在相機(jī)平移時能夠保持成立，但在相機(jī)發(fā)生明顯旋轉(zhuǎn)時就有可能不成立。特別地，當(dāng)相機(jī)繞著光心旋轉(zhuǎn)較大角度，會出現(xiàn)相關(guān)性直接變成負(fù)數(shù)的情況，即使都是同一個像素塊。在這種情況下，在塊匹配前，做一次圖像變換，把2幀圖像間的運動考慮進(jìn)來，是一種常見有效的預(yù)處理方式。

仿射變換的矩陣形式如下：

現(xiàn)以第一幅圖像為參考幀，推導(dǎo)參考幀與當(dāng)前幀之間的仿射變換。根據(jù)相機(jī)模型，第一幀圖像上的一個像素PR與真實的三維點世界坐標(biāo)PW有以下關(guān)系：

其中，dR表示像素PR距離相機(jī)成像平面的深度。

類似地，對于當(dāng)前幀，PC為真實三維點世界坐標(biāo)PW的投影為：

2幀圖像之間的像素關(guān)系為：

當(dāng)PR與dR已知時，可以計算出PC的投影位置。再給PR的2個分量各加一個增量du、dv，就可以求得PC的增量duc、dvc。這樣就可以算出局部范圍內(nèi)參考幀和當(dāng)前幀一個線性的坐標(biāo)變換，構(gòu)成仿射變換。經(jīng)過仿射變換后的塊匹配，以期得到對旋轉(zhuǎn)更好的魯棒性。

4 深度估計

4.1 三角測量

研究已通過極線搜索和仿射變換后的塊匹配技術(shù)得到了半稠密或稠密深度估計中某個像素在各個圖中的位置，這樣就可以利用三角測量（Triangulation）方法確定其深度。三角測量最早由高斯提出，是指通過不同位置對同一路標(biāo)點進(jìn)行觀察，從觀察到的位置推斷路標(biāo)點的距離。在視覺SLAM中，主要用三角化來估計像素點的距離（即深度）。由于噪聲的存在，通常求得深度的最小二乘解。

4.2 高斯分布的逆深度濾波器

對像素點的深度估計，有濾波器方法或非線性優(yōu)化兩種求解思路。由于前端已經(jīng)占用一定的計算量，建圖方面通常采用計算量相對較少的濾波器方法。

對深度的分布假設(shè)有若干種方法，可假設(shè)深度值服從高斯分布，也可假設(shè)深度值服從均勻-高斯混合分布[7，12]。

但是深度的高斯分布并不準(zhǔn)確，因為近處的點不會小于相機(jī)焦距，而且在一些室外場景中，會存在距離很遠(yuǎn)的點，這個分布并不和高斯分布一樣是對稱形狀。在仿真中，研究發(fā)現(xiàn)假設(shè)深度的倒數(shù)（即逆深度）為高斯分布是比較有效的。逆深度在實際應(yīng)用中也具有更好的數(shù)值穩(wěn)定性，從而成為一種通用的技巧。本文采用的就是高斯分布的逆深度濾波器方法。這里，假設(shè)某個像素點的逆深度1d服從高斯分布，對應(yīng)數(shù)學(xué)公式可寫為：

新觀測的信息更新原來像素點的深度分布，融合后的逆深度分布仍然是高斯分布。采用逆深度的方差更新方式描述深度的不確定性。在實際工程中，當(dāng)深度不確定性小于一定閾值時，即可認(rèn)為深度數(shù)據(jù)已收斂。

5 建圖

本次研究主要著重于改進(jìn)單目半稠密建圖的方法，故選用給定相機(jī)軌跡的數(shù)據(jù)進(jìn)行討論，根據(jù)一段視頻系列估計某幅圖像的深度。研究時選用了REMODE[13，19]數(shù)據(jù)集和EuRoC數(shù)據(jù)集。

5.1 數(shù)據(jù)集介紹

REMODE數(shù)據(jù)集提供了一架無人機(jī)采集的單目俯視圖像，共200張，同時提供了每張圖像的真實位姿。EuRoC數(shù)據(jù)集是雙目和IMU數(shù)據(jù)集，包含2個場景：蘇黎世聯(lián)邦理工學(xué)院的一個廠房和一個普通房間。本文僅用雙目中的一個相機(jī)的廠房場景數(shù)據(jù)和對應(yīng)的圖像真實位姿數(shù)據(jù)，共200張。

5.2 建圖測試結(jié)果分析

建圖測試結(jié)果驗證分為對照組和改進(jìn)組。其中，對照組采用未優(yōu)化的單目半稠密建圖方法（極限搜索+塊匹配+高斯分布的深度濾波器），改進(jìn)組采用改進(jìn)的單目半稠密建圖方法（極限搜索+圖像變換+塊匹配+高斯分布的逆深度濾波器）。

對照組方法和改進(jìn)組方法在REMODE和EuRoC數(shù)據(jù)集中表現(xiàn)如圖1所示， 數(shù)據(jù)集測試結(jié)果分析見表1。

可以看出，改進(jìn)的方法有更好的準(zhǔn)確性和魯棒性。具體來說，在檢測梯度變化明顯的像素點上，對于相對簡單的REMODE數(shù)據(jù)集室內(nèi)普通房間場景，對照組和改進(jìn)組兩種方法效果大致相同，深度的平均誤差和平方誤差相差無幾，但在EuRoC數(shù)據(jù)集復(fù)雜工廠室內(nèi)場景中，改進(jìn)的方法顯然要更加準(zhǔn)確，深度圖效果更為突出，平均誤差減少了9%，平方誤差減少了47%。在用時上，由于改進(jìn)的半稠密建圖方法對圖像中每個像素做了仿射變換，使改進(jìn)組方法用時多于對照組方法。

6 結(jié)束語

在視覺SLAM中，相比于RGB-D建圖，單目建圖的計算量大，結(jié)果也并不可靠。但是單目視覺SLAM的應(yīng)用能夠適用于復(fù)雜室外環(huán)境，而且性價比高，是頗具學(xué)術(shù)價值的研究方向。在今后的研究工作中，將致力于提出完整的單目SLAM系統(tǒng)，以期得到更加準(zhǔn)確有效的定位及建圖解決方案。

參考文獻(xiàn)

[1]BARFOOT T. State estimation for robotics-A matrix lie group approach[M]. Cambridge： Cambridge University Press， 2016.

[2]DAVISON A J， REID I D， MOLTON N D， et al. MonoSLAM： Real-time single camera SLAM[J]. IEEE transactions on pattern analysis and machine intelligence， 2007， 29（6）：1052-1067.

[3]KLEIN G， MURRAY D. Parallel tracking and mapping for small AR workspaces[C]// ISMAR 2007 6th IEEE and ACM International Symposium on mixed and Augmented Reality. Nara， Japan：IEEE， 2007： 225-234.

[4]MUR-ARTAL R， MONTIEL J， TARDOS J D. Orb-slam： A versatile and accurate monocular slam system[J]. arXiv preprint arXiv： 1502.00956， 2015.

[5]ENGEL J， SCHOEPS T， CREMERS D. Lsd-slam： Large-scale direct monocular SLAM[M]//FLEET D， PAJDLA T， SCHIELE B， et al. Computer Vision-ECCV 2014. Lecture Notes in Computer Science. Cham：Springer， 2014：834-849.

[6]ENGEL J， STURM J， CREMERS D. Semi-dense visual odometry for a monocular camera[C]//Proceedings of the IEEE International Conference on Computer Vision. Washington， DC， USA：IEEE， 2013：1449-1456.

[7]FORSTER C， PIZZOLI M， SCARAMUZZA D. Svo： Fast semi-direct monocular visual odometry[C]//2014 IEEE International Conference on Robotics and Automation（ICRA）. Hong Kong：IEEE， 2014：15-22.

[8]ENGEL J， KOLTUN V， CREMERS D. Direct sparse odometry[J]. arXiv preprint arXiv： 1607. 02565， 2016.

[9]羅鴻城. 基于卷積神經(jīng)網(wǎng)絡(luò)的實時單目稠密建圖方法研究[D]. 武漢：華中科技大學(xué)，2019.

[10]謝瑒. 無人機(jī)自主導(dǎo)航的單目視覺SLAM技術(shù)研究[D]. 南京：南京航空航天大學(xué)，2019.

[11]張劍華，王燕燕，王曾媛，等. 單目同時定位與建圖中的地圖恢復(fù)融合技術(shù)[J].中國圖象圖形學(xué)報，2018，23（3）：372-383.

[12]VOGIZTZIS G， HERNNDEZ C. Video-based， real-time multi-view stereo[J]. Image and Vision Computing， 2011， 29（7）： 434-441.

[13]PIZZOIL M， FORSTER C， SCARAMUZZA D. Remode： Probabilistic， monocular dense reconstruction in real time[C]//2014 IEEE International Conference on Robotics and Automation（ICRA）. Hong Kong：IEEE， 2014：2609-2616.

[14]高翔， 張濤， 劉毅，等. 視覺SLAM十四講， 從理論到實踐[M]. 2版. 北京：電子工業(yè)出版社， 2019.

[15]PRETTO A， MENEGATTI E， PAGELLO E. Omnidirectional dense large-scale mapping and navigation based on meaningful triangulation[C]//2011 IEEE International Conference on Robotics and Automation（ICRA 2011）. Shanghai， China：IEEE，2011： 3289-3296.

[16]RUECKAUER B， DELBRUCK T. Evaluation of event-based algorithms for optical flow with ground-truth from inertial measurement sensor[J]. Frotiers in neuroscience， 2016，10（137）：1-17.

[17]STRASDAT H， MONTIEL J M， DAVISON A J. Visual slam： Why filter？[J]. Image and Vision Computing， 2012，30（2）：65-77.

[18]HIRSCHMULLER H， SCHARSTEIN D. Evaluation of cost functions for stereo matching[C]//2007 IEEE Conference on Computer Vision and Pattern Recognition. Minneapolis：IEEE， 2007：1-8.

[19]HANDA A， NEWCOMBE R A， ANGELI A， et al. Real-time camera tracking： When is high frame-rate best？[M]//FITZGIBBON A， LAZEBNIK S， PERONA P， et al. Computer Vision-ECCV 2012. Lecture Notes in Computer Science. Berlin/Heidelberg：Springer， 2012：222-235.