基于魚眼相機(jī)和RGBD相機(jī)的異構(gòu)協(xié)同SLAM

張宇桐，宋建梅，丁艷，劉錦鵬

北京理工大學(xué) 宇航學(xué)院，北京 100081

同時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）技術(shù)［1-3］，是指一個(gè)處在未知環(huán)境當(dāng)中的運(yùn)動(dòng)載體，通過(guò)攜帶的傳感器來(lái)獲取環(huán)境相關(guān)信息，構(gòu)建一個(gè)表征周圍環(huán)境信息的地圖，同時(shí)以該地圖為參照來(lái)實(shí)現(xiàn)自主定位的技術(shù)。常用的視覺SLAM系統(tǒng)分為單目SLAM、雙目SLAM和紅綠藍(lán)-深度相機(jī)SLAM（Red-Green-Blue-Depth SLAM，RGBD-SLAM）3大類。單目SLAM具有硬件簡(jiǎn)單、計(jì)算效率高、能滿足實(shí)時(shí)性要求等優(yōu)點(diǎn)，因此受到了人們廣泛的關(guān)注［4-7］。此外，單目SLAM可以選用魚眼相機(jī)等大視場(chǎng)相機(jī)。通過(guò)增大視野，可提升單幀圖像看到的場(chǎng)景范圍，進(jìn)而可獲得更多的場(chǎng)景紋理信息，實(shí)現(xiàn)更加穩(wěn)定的定位和建圖［8-9］。2015年，Caruso 等［10］基于LSD-SLAM（Large-Scale Direct Monocular SLAM）［11］提出了適用于全景或魚眼相機(jī)的SLAM系統(tǒng)，該系統(tǒng)即使在相對(duì)較低的分辨率（240×240）下，定位精度也超過(guò)了使用針孔模型時(shí)獲得的精度。2016年，Zhang等［12］基于半直接法視覺里程計(jì)［13］（Semi-direct Visual Odometry，SVO）設(shè)計(jì)了適用于魚眼相機(jī)和折反射相機(jī)的半直接法SLAM系統(tǒng)，在室內(nèi)環(huán)境中，該系統(tǒng)的相機(jī)軌跡精度比使用針孔相機(jī)的SVO系統(tǒng)更高。2018年， Matsuki等［14］對(duì)直接稀疏里程計(jì)（Direct Sparse Odometry，DSO）系統(tǒng)［15］做了改進(jìn)，提出了適用于廣角圖像的全向直接稀疏里程計(jì)（Omnidirectional DSO）系統(tǒng)，使DSO系統(tǒng)的軌跡精度和魯棒性均得以提高。同年，Wang等提出了CubemapSLAM［9］系統(tǒng)，將魚眼圖像投影到一個(gè)立方體的5個(gè)表面，并生成包含5個(gè)子視野的立方體表面展開圖，來(lái)實(shí)現(xiàn)魚眼圖像的去畸變，并被Jo等［16］成功應(yīng)用在了一個(gè)自動(dòng)泊車系統(tǒng)中。2019年，Liu等［17］利用增強(qiáng)型統(tǒng)一相機(jī)模型［18］（Enhanced Unified Camera Model，EUCM）對(duì)ORB-SLAM［7］系統(tǒng)進(jìn)行了改進(jìn)，使ORBSLAM能夠利用魚眼相機(jī)進(jìn)行實(shí)時(shí)且魯棒的定位建圖。2021年，Garcia等［19］利用卷積神經(jīng)網(wǎng)絡(luò)（Convolutional Neural Network，CNN）對(duì)Liu等的工作［17］進(jìn)行了改進(jìn)。他們利用CNN對(duì)每一幀魚眼圖像進(jìn)行語(yǔ)義分割，并剔除天空等離群點(diǎn)較多區(qū)域的特征點(diǎn)，進(jìn)一步提高了SLAM系統(tǒng)的定位精度。

為了進(jìn)一步增大SLAM系統(tǒng)的環(huán)境感知能力，基于多目魚眼的全景SLAM系統(tǒng)相繼被提出。2016年， Urban和Hinz提出了Multicol-SLAM［20］系統(tǒng)。該系統(tǒng)適用于任意數(shù)量、任意模型相機(jī)以任意相對(duì)位姿剛性耦合的多相機(jī)系統(tǒng)，其魯棒性相比于單相機(jī)SLAM系統(tǒng)有較大提升。2020年，Ji等基于LadyBug多目魚眼相機(jī)開發(fā)了一種全景SLAM系統(tǒng)（PANSLAM）［21］，將6幅魚眼圖像投影到一個(gè)球面上，并對(duì)相機(jī)標(biāo)定模型、特征匹配跟蹤、回環(huán)檢測(cè)、SLAM初始化過(guò)程都做了改進(jìn)，使其在長(zhǎng)基線拍攝條件下能夠?qū)崿F(xiàn)比ORB-SLAM2［22］、Multicol-SLAM［20］和Cubemap-SLAM［9］等系統(tǒng)更高的軌跡精度和魯棒性。2021年，Zhang和Huang［23］設(shè)計(jì)了一個(gè)基于雙魚眼全景相機(jī)Ricoh的SLAM系統(tǒng)，利用SPHORB（Spherical ORB）特征［24］對(duì)全景圖像進(jìn)行特征提取和匹配，使得SLAM系統(tǒng)的精度和魯棒性優(yōu)于使用ORB（Oriented FAST and Rotated BRIEF）特征的SLAM系統(tǒng)。

單目SLAM無(wú)法獲得尺度信息，即地圖與真實(shí)環(huán)境的比例尺，這導(dǎo)致單目SLAM存在初始化困難、容易發(fā)生尺度漂移且無(wú)法在相機(jī)純旋轉(zhuǎn)運(yùn)動(dòng)的條件下工作等問(wèn)題。雙目SLAM系統(tǒng)采用雙目相機(jī)之間的特征匹配信息，借助三角測(cè)距原理，獲取場(chǎng)景的深度信息，進(jìn)而可以構(gòu)建含有尺度信息的地圖，因此不易發(fā)生尺度漂移現(xiàn)象。但由于雙目SLAM系統(tǒng)需要在線計(jì)算每一幀雙目圖像的匹配信息，因此存在計(jì)算量大的缺點(diǎn)。此外，單目和雙目SLAM均無(wú)法對(duì)紋理較少的環(huán)境構(gòu)建稠密地圖。為了克服這些問(wèn)題，基于RGBD相機(jī)的SLAM系統(tǒng)［25-30］受到了人們的廣泛關(guān)注。由于RGBD相機(jī)可利用結(jié)構(gòu)光或光線飛行時(shí)間（Time of Flight，TOF）等技術(shù)，直接測(cè)量得到視野中每一個(gè)像素的深度值，故RGBD-SLAM系統(tǒng)相比于雙目SLAM系統(tǒng)具備更實(shí)時(shí)的尺度信息獲取能力。Audras等［25］提出了一種直接法RGBD-SLAM系統(tǒng)，基于光度信息，對(duì)每一幀RGBD圖像進(jìn)行光度匹配來(lái)實(shí)現(xiàn)SLAM過(guò)程。同年，Newcombe等［26］提出了KinectFusion系統(tǒng)。KinectFusion采用截?cái)嘤蟹?hào)距離函數(shù)（Truncated Signed Distance Functions，TSDF），利用曲面模型來(lái)表示場(chǎng)景的幾何信息。通過(guò)使用迭代最近點(diǎn)［31］（Iterative Closest Point，ICP）的方法拼接場(chǎng)景曲面，以曲面生長(zhǎng)的形式構(gòu)建全局地圖。Endres等提出了RGBD-SLAM v2算法［29］，該算法前端利用尺度不變特征變換（Scale-Invariant Feature Transform，SIFT）特征［32］進(jìn)行特征提取和匹配來(lái)計(jì)算相機(jī)軌跡，后端執(zhí)行位姿圖優(yōu)化并帶有回環(huán)檢測(cè)功能。

此外，能夠兼容多種不同類型相機(jī)的SLAM系統(tǒng)相繼被提出。Mur-Artal等對(duì)其構(gòu)建的ORB-SLAM［7］進(jìn)行了改進(jìn)，提出了ORBSLAM2［22］系統(tǒng)。該系統(tǒng)是一個(gè)開源的，同時(shí)支持單目、雙目、RGBD相機(jī)模型的，具備前端、后端、回環(huán)檢測(cè)功能的完整SLAM系統(tǒng)。ORBSLAM2的軌跡精度相比于以往的基于ICP方法的RGBD-SLAM系統(tǒng)［26-28］有較大提升。Sumikura等提出了OpenVSLAM系統(tǒng)［33］，該系統(tǒng)支持單目、雙目、RGBD、魚眼、全景相機(jī)等多種相機(jī)模型，并且支持地圖的二次利用。Campos等對(duì)ORB-SLAM2做了進(jìn)一步改進(jìn)并提出了ORBSLAM3［34］，此系統(tǒng)在ORB-SLAM2的基礎(chǔ)上，增加了魚眼相機(jī)模型、融合慣性測(cè)量單元（Inertial Measurement Unit，IMU）的視覺慣性SLAM和多地圖管理的功能，進(jìn)一步提高了系統(tǒng)的魯棒性。

Yousif等［35］在2017年提出了MonoRGBD-SLAM系統(tǒng)。該系統(tǒng)是迄今為止唯一一個(gè)結(jié)合單目廣角相機(jī)和RGBD相機(jī)的異構(gòu)協(xié)同SLAM系統(tǒng)。單目廣角相機(jī)和RGBD相機(jī)的異構(gòu)協(xié)同機(jī)制使得MonoRGBD-SLAM系統(tǒng)能夠結(jié)合2種相機(jī)的優(yōu)勢(shì)，增強(qiáng)系統(tǒng)在地圖構(gòu)建任務(wù)中的魯棒性，進(jìn)而能夠更容易地構(gòu)建更大規(guī)模的地圖。然而該系統(tǒng)將廣角相機(jī)的視野分成了9塊與RGBD相機(jī)內(nèi)參相同的子視野，并且選用SIFT特征進(jìn)行特征點(diǎn)提取和匹配，這使得系統(tǒng)的計(jì)算量非常大。此外，如果要將廣角相機(jī)替換為更大視場(chǎng)角的魚眼相機(jī)（如210°視場(chǎng)角），則魚眼相機(jī)的視野需要被切分出更多個(gè)子視野，以覆蓋魚眼相機(jī)的全部視野。

綜上所述，在現(xiàn)有的工作中，基于魚眼相機(jī)和RGBD相機(jī)的異構(gòu)協(xié)同SLAM問(wèn)題沒有很好的解決。為了在不對(duì)魚眼相機(jī)進(jìn)行視野切分的前提下，結(jié)合魚眼相機(jī)可獲取更多紋理信息的優(yōu)勢(shì)和RGBD相機(jī)可直接獲取稠密深度信息的優(yōu)勢(shì)，提出基于單目魚眼相機(jī)（210°視場(chǎng)角）和RGBD相機(jī)的異構(gòu)協(xié)同SLAM系統(tǒng)。首先基于之前的工作［36］中設(shè)計(jì)的FSD-BRIEF特征描述子，實(shí)現(xiàn)特征點(diǎn)空間方向和描述子的計(jì)算。利用特征點(diǎn)姿態(tài)矩陣信息對(duì)匹配點(diǎn)對(duì)之間的特征點(diǎn)灰度質(zhì)心方向一致性檢驗(yàn)方法進(jìn)行設(shè)計(jì)，以對(duì)候選匹配點(diǎn)對(duì)進(jìn)行篩選。為了克服異構(gòu)圖像之間因存在較大尺度差異和視野差異而導(dǎo)致的特征點(diǎn)匹配困難的問(wèn)題，提出異構(gòu)圖像之間的分步式光流-投影匹配方法，實(shí)現(xiàn)魚眼相機(jī)和RGBD相機(jī)之間的特征點(diǎn)匹配和相對(duì)位姿估計(jì)。最后基于ORB-SLAM2框架提出基于單目魚眼相機(jī)和RGBD相機(jī)的異構(gòu)協(xié)同SLAM系統(tǒng)框架。本文主要工作如下：

1）設(shè)計(jì)一個(gè)新的基于魚眼相機(jī)和RGBD相機(jī)的異構(gòu)協(xié)同SLAM系統(tǒng)。

2）提出一個(gè)新的特征點(diǎn)灰度質(zhì)心方向一致性檢驗(yàn)法，用于篩選更準(zhǔn)確的候選匹配點(diǎn)對(duì)。

3）設(shè)計(jì)一個(gè)新的分步式光流-投影匹配方法，實(shí)現(xiàn)異構(gòu)圖像之間高性能的匹配和相機(jī)位姿估計(jì)。

1 魚眼-RGBD異構(gòu)協(xié)同SLAM框架

設(shè)計(jì)的異構(gòu)協(xié)同SLAM系統(tǒng)框架如圖1所示，其包括預(yù)處理、前端和后端3個(gè)部分。

圖1 異構(gòu)協(xié)同SLAM系統(tǒng)框圖Fig.1 Framework of heterogeneous collaborative SLAM system

預(yù)處理部分用于對(duì)輸入圖像進(jìn)行特征提取，為前端的特征匹配提供信息輸入。采用FAST［37］特征點(diǎn)提取算法，在原始圖像上提取特征點(diǎn)，并利用FSD-BRIEF［36］計(jì)算每一個(gè)特征點(diǎn)的描述子和姿態(tài)矩陣。

FSD-BRIEF描述子是基于球面透視模型設(shè)計(jì)的。在提取的FSD-BRIEF描述子中，重點(diǎn)考慮了球面透視模型具有的各向同性性質(zhì)。在FSD-BRIEF描述子的計(jì)算過(guò)程中，將描述子模板放在球形投影面的切平面上，并建立描述子模板和原始圖像之間的坐標(biāo)映射關(guān)系，實(shí)現(xiàn)描述子模板向原始圖像的投影和描述子計(jì)算，以充分利用球面透視模型的各向同性來(lái)實(shí)現(xiàn)描述子對(duì)圖像畸變的不變性，如圖2所示。此外，F(xiàn)SDBRIEF描述子可充分利用魚眼圖像全部視野，且無(wú)需對(duì)圖像進(jìn)行畸變矯正或者子視野截取。同時(shí)，F(xiàn)SD-BRIEF描述子對(duì)RGBD相機(jī)和魚眼相機(jī)均具有通用性，提取的描述子可直接用于RGBD圖像和魚眼圖像的異構(gòu)匹配。

圖2 FSD-BRIEF特征示意圖Fig.2 General view of FSD-BRIEF descriptor

在圖1的異構(gòu)協(xié)同SLAM系統(tǒng)中，設(shè)計(jì)的基于特征點(diǎn)三維灰度質(zhì)心方向一致性的候選匹配點(diǎn)篩選方法主要利用FSD-BRIEF給出的特征點(diǎn)姿態(tài)矩陣信息，對(duì)特征點(diǎn)方向的一致性進(jìn)行判斷。該方法主要用于前端的特征點(diǎn)投影匹配和后端特征再匹配環(huán)節(jié)中候選匹配點(diǎn)的篩選，以提高特征匹配的正確率。

前端部分主要用于對(duì)輸入的當(dāng)前圖像幀和同構(gòu)、異構(gòu)相鄰圖像幀之間的特征點(diǎn)進(jìn)行分步式光流-投影匹配，利用幾何約束估計(jì)每一幀圖像對(duì)應(yīng)的相機(jī)位姿，得到相機(jī)的運(yùn)動(dòng)軌跡。利用ORB-SLAM2的相對(duì)局部地圖定位（Tracklocalmap）模塊，對(duì)當(dāng)前幀和局部定位地圖的關(guān)鍵幀進(jìn)行匹配，并建立每一幀圖像和地圖點(diǎn)之間的匹配關(guān)系。然后在每一個(gè)成功定位的圖像幀中篩選出一部分圖像幀作為關(guān)鍵幀輸入到后端。

后端負(fù)責(zé)對(duì)前端輸出的新關(guān)鍵幀與關(guān)聯(lián)的所有一級(jí)共視幀之間進(jìn)行再一次的特征匹配，并通過(guò)三角化的方式在定位地圖中構(gòu)造出更多的地圖點(diǎn)，來(lái)為前端提供更多可用于定位的三維路標(biāo)點(diǎn)。同時(shí)，通過(guò)滑窗光束法平差（Bundle Adjustment，BA）的方式，對(duì)新關(guān)鍵幀、所有一級(jí)共視幀的位姿和所有關(guān)聯(lián)的地圖點(diǎn)位置進(jìn)行優(yōu)化求解，來(lái)保證定位地圖的全局一致性。

2 候選匹配點(diǎn)篩選方法設(shè)計(jì)

為了在特征匹配過(guò)程中，篩選出更準(zhǔn)確的候選點(diǎn)，本節(jié)基于FSD-BRIEF［36］中構(gòu)建的特征點(diǎn)姿態(tài)坐標(biāo)系，利用特征點(diǎn)姿態(tài)矩陣信息對(duì)圖像匹配過(guò)程中，基于三維灰度質(zhì)心方向一致性的候選匹配點(diǎn)篩選方法進(jìn)行設(shè)計(jì)。

定義單位向量r為特征點(diǎn)在相機(jī)坐標(biāo)系中觀測(cè)方向?qū)?yīng)的向量，向量c為特征點(diǎn)三維灰度質(zhì)心的位置向量，如圖3所示。FSD-BRIEF［36］中構(gòu)建的特征點(diǎn)姿態(tài)坐標(biāo)系用ObXbYbZb表示，其原點(diǎn)Ob與相機(jī)坐標(biāo)系的原點(diǎn)Oc重合，ObZb軸的方向與向量r重合，即指向特征點(diǎn)的觀測(cè)方向，ObYb軸與向量r×c的方向一致，ObXb軸與平面YbObZb垂直，且在向量c的同側(cè)，以表征特征點(diǎn)的灰度質(zhì)心方向。

圖3 相機(jī)中的特征點(diǎn)姿態(tài)坐標(biāo)系示意圖Fig.3 Attitude coordinate system of a feature point in camera

灰度質(zhì)心方向反映了該特征點(diǎn)對(duì)應(yīng)三維物體點(diǎn)的空間幾何方向信息，如圖4所示。其中，坐標(biāo)系Ob1Xb1Yb1Zb1和Ob2Xb2Yb2Zb2是一對(duì)分別位于2個(gè)相機(jī)中的特征點(diǎn)對(duì)應(yīng)的姿態(tài)坐標(biāo)系。同一個(gè)三維物體點(diǎn)在不同相機(jī)中投影點(diǎn)的灰度質(zhì)心方向是大致一致的。

圖4 匹配特征點(diǎn)的方向一致性原理示意圖Fig.4 Direction consistency of matching feature points

圖5表示特征點(diǎn)方向一致性檢驗(yàn)過(guò)程中的幾何變量圖。假設(shè)空間中有2個(gè)相機(jī)，分別定義為c1和c2。空間點(diǎn)P在相機(jī)c1和c2中對(duì)應(yīng)的特征點(diǎn)為p1和p2，對(duì)應(yīng)的姿態(tài)坐標(biāo)系分別為Ob1Xb1Yb1Zb1和Ob2Xb2Yb2Zb2，簡(jiǎn)記為b1和b2。定義向量b2X為特征點(diǎn)姿態(tài)坐標(biāo)系b2的X軸對(duì)應(yīng)的單位向量，其代表特征點(diǎn)p2對(duì)應(yīng)的灰度質(zhì)心方向。定義向量b1X是特征點(diǎn)姿態(tài)坐標(biāo)系b1的X軸對(duì)應(yīng)的單位向量，其代表特征點(diǎn)p1對(duì)應(yīng)的灰度質(zhì)心方向。定義向量b1Z是特征點(diǎn)姿態(tài)坐標(biāo)系b1的Z軸對(duì)應(yīng)的單位向量，其代表特征點(diǎn)p1與空間點(diǎn)P連線對(duì)應(yīng)的方向向量。將向量b2X平移到特征點(diǎn)姿態(tài)坐標(biāo)系b1中，并向平面Xb1Ob1Yb1投影，得到投影向量b′2X，則有

圖5 特征點(diǎn)方向一致性中的幾何變量圖Fig.5 Geometric variables in feature points’ direction consistency

式中：I3×3表示3×3單位矩陣。

定義特征點(diǎn)p1和p2之間的灰度質(zhì)心方向夾角γ為向量b1X和向量b′2X的夾角，其計(jì)算公式為

定義閾值γth為灰度質(zhì)心方向夾角γ的最大容忍誤差，那么，如果γ＜γth，則判定特征點(diǎn)p1和p2方向一致，否則判定特征點(diǎn)p1和p2方向不一致，在本文算法中，γth選為20°。

在實(shí)際計(jì)算過(guò)程中，向量b2X在相機(jī)坐標(biāo)系c2中的坐標(biāo)b2X，c2是已知的，它是特征點(diǎn)p2在相機(jī)c2中的姿態(tài)矩陣Rc2b2的第1列，向量b1X和b1Z在相機(jī)坐標(biāo)系c1中的坐標(biāo)b1X，c1和b1Z，c1是已知的，即特征點(diǎn)p1在相機(jī)c1中的姿態(tài)矩陣Rc1b1的第1列和第3列。γ的計(jì)算需要在同一個(gè)參考坐標(biāo)系下完成，所以將b2X的坐標(biāo)轉(zhuǎn)到相機(jī)坐標(biāo)系c1中，則得到γ的計(jì)算公式為

式中：Rc1c2是相機(jī)坐標(biāo)系c2到相機(jī)坐標(biāo)系c1的姿態(tài)轉(zhuǎn)換矩陣。

為了驗(yàn)證基于三維灰度質(zhì)心方向一致性的候選匹配點(diǎn)篩選方法對(duì)算法匹配性能的提升作用，本文進(jìn)行了算法消融實(shí)驗(yàn)，并采用PR曲線［38］（召回率、1-精度）作為匹配性能的評(píng)價(jià)指標(biāo)。召回率和1-精度的定義為

式中：ε′為描述子距離閾值自變量。在匹配過(guò)程中，若一對(duì)特征點(diǎn)描述子距離最小且小于閾值ε′，才被選擇為算法輸出的匹配點(diǎn)對(duì)。

在實(shí)驗(yàn)中，分別使用魚眼相機(jī)和RGBD相機(jī)對(duì)如圖6所示的靶標(biāo)圖案以不同角度拍攝14張圖片，部分拍攝示例圖片如圖7所示。

圖6 用于測(cè)試特征匹配算法的靶標(biāo)圖案Fig.6 Target image for feature matching algorithm test

圖7 魚眼相機(jī)和RGBD相機(jī)拍攝的部分示例圖片F(xiàn)ig.7 Sample pictures taken by fisheye and RGBD cameras

實(shí)驗(yàn)時(shí)，對(duì)采集的每張圖片中的靶標(biāo)圖案區(qū)域提取300個(gè)FAST特征點(diǎn)，并提取FSDBRIEF描述子。利用投影匹配法對(duì)RGBD圖片和魚眼圖片進(jìn)行特征點(diǎn)匹配，搜索鄰域半徑設(shè)為30個(gè)像素寬度。未加入特征點(diǎn)方向一致性篩選條件的匹配算法記為版本1，加入特征點(diǎn)方向一致性篩選條件的匹配算法記為版本2。匹配結(jié)果對(duì)應(yīng)的PR曲線如圖8所示

圖8 2個(gè)版本匹配算法的PR曲線結(jié)果Fig.8 PR curves result of two versions of matching algorithms

圖8中，召回率值越大，代表算法從正確匹配點(diǎn)對(duì)集合中找到的點(diǎn)對(duì)比例越高，PR曲線越靠近上方。1-精度值越小，代表算法輸出結(jié)果中正確的匹配點(diǎn)對(duì)數(shù)占比越大，匹配精度越高，錯(cuò)誤率越小，PR曲線越靠左。所以，算法輸出的PR曲線越靠近左上方，即越靠近坐標(biāo)點(diǎn)（0，1），代表算法的匹配性能越好。

從圖8中可以看出，版本2的PR曲線結(jié)果比版本1更加靠近（0，1）點(diǎn)，版本2的曲線末端“召回率”比版本1高3.4%，1-精度（錯(cuò)誤率）比版本1低15.6%。實(shí)驗(yàn)結(jié)果表明，設(shè)計(jì)的基于三維灰度質(zhì)心方向一致性的候選匹配點(diǎn)篩選方法能夠有效提升特征匹配算法的召回率和1-精度。

3 分步式光流投影匹配方法設(shè)計(jì)

在異構(gòu)協(xié)同SLAM系統(tǒng)的前端中，針對(duì)魚眼圖像和RGBD圖像之間因存在較大的視野和尺度差異而導(dǎo)致匹配困難的問(wèn)題，提出一種分步式光流-投影匹配方法，以提升系統(tǒng)中異構(gòu)圖像特征匹配的性能。

在系統(tǒng)初始階段時(shí)，利用暴力匹配方法對(duì)2個(gè)相機(jī)之間的特征點(diǎn)進(jìn)行初始匹配，并利用MLPnP［39］方法求解2個(gè)相機(jī)之間的初始相對(duì)位姿。在后續(xù)的異構(gòu)圖像匹配過(guò)程中，利用求出的初始相對(duì)位姿信息，針對(duì)魚眼圖像設(shè)計(jì)的匹配過(guò)程分5步進(jìn)行。針對(duì)RGBD圖像設(shè)計(jì)的匹配過(guò)程分4步進(jìn)行。

3.1 魚眼圖像

針對(duì)魚眼圖像設(shè)計(jì)的分步式光流-投影匹配過(guò)程如圖9所示。

圖9 魚眼圖像分步式光流-投影匹配過(guò)程示意圖Fig.9 Step-by-step optical flow and projection matching process for fisheye image

假設(shè)RGBD相機(jī)上一幀圖像和魚眼相機(jī)上一幀圖像的相機(jī)位姿都已知，將RGBD圖像中提取的所有深度有效的特征點(diǎn)投影到魚眼相機(jī)上一幀圖像中，然后利用光流法，將這些點(diǎn)從魚眼相機(jī)上一幀圖像跟蹤到魚眼相機(jī)當(dāng)前幀圖像，進(jìn)而可以確定一組RGBD相機(jī)上一幀圖像和魚眼相機(jī)當(dāng)前幀圖像之間的匹配點(diǎn)對(duì)，利用MLPnP［39］算法，計(jì)算得到魚眼相機(jī)當(dāng)前幀圖像的相機(jī)位姿。然后，將RGBD相機(jī)上一幀圖像所有深度信息有效的特征點(diǎn)，和魚眼相機(jī)上一幀圖像中關(guān)聯(lián)的所有地圖點(diǎn)投影到魚眼相機(jī)當(dāng)前幀圖像中，并在投影點(diǎn)的鄰域找到與之匹配的特征點(diǎn)。利用MLPnP［39］方法，對(duì)魚眼相機(jī)當(dāng)前幀圖像進(jìn)行再一次的位姿估計(jì)，最后通過(guò)重投影誤差確定魚眼相機(jī)當(dāng)前幀圖像和RGBD相機(jī)上一幀圖像之間正確的匹配點(diǎn)對(duì)。

圖10為魚眼相機(jī)當(dāng)前幀光流跟蹤過(guò)程的示意圖。在圖10中：FL表示魚眼相機(jī)上一幀圖像；DL表示RGBD相機(jī)上一幀圖像；FC表示魚眼相機(jī)當(dāng)前幀圖像。假設(shè)根據(jù)歷史匹配信息，圖像FL和圖像DL之間的相對(duì)相機(jī)位姿是已知的。記圖像DL中，第i個(gè)含有有效深度信息的特征點(diǎn)像素坐標(biāo)為pi，DL，構(gòu)成的點(diǎn)坐標(biāo)集合記為{pi，DL}。pi，DL對(duì)應(yīng)的三維點(diǎn)為Pi，其在圖像DL相機(jī)坐標(biāo)系中的坐標(biāo)為Pi，DL。將點(diǎn)Pi向圖像FL中投影，得到投影點(diǎn)的像素坐標(biāo)，有

圖10 魚眼相機(jī)當(dāng)前幀光流跟蹤過(guò)程示意圖Fig.10 Schematic diagram of current frame optical flow tracking process of fisheye camera

式中：TFL，DL表示從圖像DL的相機(jī)坐標(biāo)系到圖像FL的相機(jī)坐標(biāo)系的坐標(biāo)變換矩陣；ΠFL表示從圖像FL對(duì)應(yīng)相機(jī)坐標(biāo)系下的三維坐標(biāo)到圖像FL圖像像素坐標(biāo)的投影函數(shù)。

記點(diǎn)Pi在世界坐標(biāo)系中的坐標(biāo)為Pi，w，有

式中：Tw，DL代表從圖像DL的相機(jī)坐標(biāo)系到世界坐標(biāo)系的坐標(biāo)變換矩陣。

記圖像FL中，第j個(gè)與三維地圖點(diǎn)有匹配關(guān)系的特征點(diǎn)像素坐標(biāo)為p′j，F(xiàn)L，其對(duì)應(yīng)的地圖點(diǎn)為P′j，在世界坐標(biāo)系中的坐標(biāo)為P′j，w。

使用金字塔Lucas-Kanade（LK）光流法，將點(diǎn)坐標(biāo)集合{}和{p′j，F(xiàn)L}從圖像FL跟蹤到圖像FC，得到點(diǎn)集合{}和{}。對(duì)于點(diǎn)，有

式（7）代表圖像FL中的點(diǎn)坐標(biāo)通過(guò)使用LK光流法從圖像FL跟蹤到圖像FC中，得到點(diǎn)。同理，對(duì)于點(diǎn)，有

可得到一組從世界坐標(biāo)系空間點(diǎn)坐標(biāo)到圖像FC像素坐標(biāo)的3D-2D匹配點(diǎn)對(duì)集合利用MLPnP［39］方法，計(jì)算得到圖像FC的位姿矩陣，即

式中：eFC(p，P)表示圖像FC對(duì)應(yīng)的相機(jī)坐標(biāo)系下，三維點(diǎn)P到像素點(diǎn)p的重投影誤差。

圖11為魚眼相機(jī)當(dāng)前幀投影匹配過(guò)程的示意圖。在圖11中，點(diǎn)坐標(biāo)集{}和{}分別是點(diǎn)集{Pi，w}中的點(diǎn)Pi，w和點(diǎn)集{Pj′，w}中的點(diǎn)Pj′，w投影到圖像FC中得到的投影點(diǎn)構(gòu)成的點(diǎn)集，即

圖11 魚眼相機(jī)當(dāng)前幀投影匹配過(guò)程示意圖Fig.11 Schematic diagram of current frame projection matching process of fisheye camera

式中：ΠFC表示從圖像FC對(duì)應(yīng)相機(jī)坐標(biāo)系下的三維坐標(biāo)，到圖像FC圖像像素坐標(biāo)的投影函數(shù)。

記圖像FC中提取的特征點(diǎn)集合為{}，其中第k個(gè)特征點(diǎn)為。對(duì)于{}的每一個(gè)點(diǎn)，在{p″k，F(xiàn)C}中選出位于以為中心，半徑為Δpth的圓形鄰域內(nèi)，并且灰度質(zhì)心方向與在圖像DL中的特征點(diǎn)pi，DL符合方向一致性約束的特征點(diǎn)作為候選匹配特征點(diǎn)，通過(guò)比較特征描述子的漢明距離，選出描述子距離最小，并且小于描述子距離閾值Dth的點(diǎn)作為正確匹配點(diǎn)，得到點(diǎn)對(duì)應(yīng)的匹配點(diǎn)，即

式中：γ(pi，DL，p″ki，F(xiàn)C)表示特征點(diǎn)pi，DL和之間的三維灰度質(zhì)心方向夾角表示像素點(diǎn)和之間的像素坐標(biāo)歐氏距離；H(Dpi，DL，)表示特征點(diǎn)pi，DL和對(duì)應(yīng)的二進(jìn)制描述子和之間的漢明距離。

將式（11）的匹配過(guò)程用映射ProjectMatch(·)表示，有

同理，對(duì)于點(diǎn)集{}的每一個(gè)點(diǎn)，在{}中找到匹配點(diǎn)，滿足：

根據(jù)求取的位姿矩陣結(jié)果，計(jì)算中每一組匹配關(guān)系的重投影誤差，并剔除外點(diǎn)，篩選出魚眼圖像當(dāng)前幀和RGBD圖像上一幀之間正確的匹配點(diǎn)對(duì)。

3.2 RGBD圖像

針對(duì)RGBD圖像設(shè)計(jì)的分步式光流-投影匹配過(guò)程如圖12所示。

圖12 RGBD圖像分步式光流-投影匹配過(guò)程示意圖Fig.12 Step-by-step optical flow and projection matching process for RGBD image

假設(shè)RGBD相機(jī)上一幀圖像和魚眼相機(jī)當(dāng)前幀圖像的相機(jī)位姿都已知。利用光流法，將RGBD相機(jī)上一幀圖像跟蹤到RGBD相機(jī)當(dāng)前幀圖像，利用MLPnP［39］方法，計(jì)算得到RGBD相機(jī)當(dāng)前幀圖像的相機(jī)位姿。將RGBD相機(jī)當(dāng)前幀圖像中，深度信息有效的特征點(diǎn)投影到RGBD相機(jī)上一幀圖像和魚眼相機(jī)當(dāng)前幀圖像中，通過(guò)投影匹配方法獲得一組點(diǎn)匹配關(guān)系。利用MLPnP［39］方法，對(duì)RGBD相機(jī)當(dāng)前幀圖像進(jìn)行再一次的位姿估計(jì)。通過(guò)重投影誤差確定RGBD相機(jī)當(dāng)前幀圖像和魚眼相機(jī)上一幀圖像之間正確的匹配點(diǎn)對(duì)。

圖13為RGBD相機(jī)當(dāng)前幀光流跟蹤過(guò)程的示意圖。在圖13中，DL表示RGBD相機(jī)上一幀圖像，DC表示RGBD相機(jī)當(dāng)前幀圖像。利用光流法，對(duì)點(diǎn)集{pi，DL}從圖像DL跟蹤到圖像DC，得到圖像DC中的點(diǎn)集{}，有

圖13 RGBD相機(jī)當(dāng)前幀光流跟蹤過(guò)程示意圖Fig.13 Schematic diagram of current frame optical flow tracking process of RGBD camera

由于在RGBD圖像DL中，{pi，DL}中的每一個(gè)深度有效點(diǎn)，都對(duì)應(yīng)一個(gè)坐標(biāo)已知的三維空間點(diǎn)Pi，那么可以獲得一組匹配點(diǎn)對(duì)集合{(Pi，w)}。利用該匹配點(diǎn)對(duì)集合，可以求得圖像DC對(duì)應(yīng)的相機(jī)位姿矩陣，即

式中：eDC( )表示圖像DC對(duì)應(yīng)相機(jī)坐標(biāo)系下，三維點(diǎn)P到像素點(diǎn)p的重投影誤差。

圖14為RGBD相機(jī)當(dāng)前幀投影匹配過(guò)程的示意圖。將三維點(diǎn)集{Pi，w}的每一個(gè)點(diǎn)Pi，w投影到圖像DC的圖像平面中，形成的點(diǎn)集記為{}，有

圖14 RGBD相機(jī)當(dāng)前幀投影匹配過(guò)程示意圖Fig.14 Schematic diagram of current frame projection matching process of RGBD camera

式中：ΠDC表示從圖像DC對(duì)應(yīng)相機(jī)坐標(biāo)系下的三維坐標(biāo)，到圖像DC像素坐標(biāo)的投影函數(shù)。

記圖像DC中提取的特征點(diǎn)集為{}。利用式（11）所示的投影匹配方法，尋找點(diǎn)集{}中的每一個(gè)點(diǎn)，在點(diǎn)集{}中對(duì)應(yīng)的匹配點(diǎn)，有

在圖像FC中，對(duì)于提取的特征點(diǎn)集{}的第k個(gè)特征點(diǎn)，記其關(guān)聯(lián)的三維點(diǎn)為，其在世界坐標(biāo)系中的坐標(biāo)為。記在圖像DC中的投影點(diǎn)記為，有

利用投影匹配方法，尋找每一個(gè)在中的匹配點(diǎn)，有

由于圖像DC是RGBD圖像，所以對(duì)于點(diǎn)集{}中的點(diǎn)，利用深度信息可以獲得其對(duì)應(yīng)三維點(diǎn)在圖像DC相機(jī)坐標(biāo)系中的坐標(biāo)，記為。利用第3步中求得的圖像DC相機(jī)位姿矩陣，對(duì)點(diǎn)向世界坐標(biāo)系中進(jìn)行反投影，得到其在世界坐標(biāo)系中的坐標(biāo)，有

將點(diǎn)向圖像FC中進(jìn)行投影，得到其在圖像FC中的像素坐標(biāo)，有

對(duì)于每一個(gè)投影點(diǎn)，在圖像FC中的特征點(diǎn)集合{}中進(jìn)行投影匹配，找到對(duì)應(yīng)的匹配點(diǎn)，有

將點(diǎn)向圖像DL中進(jìn)行投影，得到其在圖像DL中的像素坐標(biāo)，有

式中：ΠDL表示從圖像DL對(duì)應(yīng)相機(jī)坐標(biāo)系下的三維坐標(biāo)，到圖像DL像素坐標(biāo)的投影函數(shù)。

對(duì)于每一個(gè)投影點(diǎn)，在圖像DL中的特征點(diǎn)集合{pi，DL}中進(jìn)行投影匹配，找到對(duì)應(yīng)的匹配點(diǎn)，有

根據(jù)式（18）、式（20）、式（23）和式（25），可以得到4組匹配點(diǎn)對(duì)集合，根據(jù)這些匹配信息，對(duì)圖像DC的相機(jī)位姿進(jìn)行第二次估計(jì)，得到圖像DC的相機(jī)位姿矩陣，有

根據(jù)求取的位姿矩陣結(jié)果，

提出的分步式光流-投影匹配方法在異構(gòu)圖像流中的迭代運(yùn)行過(guò)程如圖15所示。從圖15中可以看出，在該運(yùn)行過(guò)程中，RGBD視頻流和魚眼視頻流中的同構(gòu)圖像和異構(gòu)圖像之間都有通過(guò)特征匹配建立的幾何約束，進(jìn)而實(shí)現(xiàn)SLAM過(guò)程中2個(gè)相機(jī)的異構(gòu)協(xié)同。

圖15 分步式光流-投影匹配方法在異構(gòu)圖像流中的迭代運(yùn)行示意圖Fig.15 Iterative operation of step-by-step optical flow and projection matching method in heterogeneous image flow

4 異構(gòu)協(xié)同SLAM系統(tǒng)的后端

在提出的異構(gòu)協(xié)同SLAM算法中，2個(gè)相機(jī)的SLAM通道共用一個(gè)后端。如圖16所示，定義與新關(guān)鍵幀關(guān)聯(lián)的地圖點(diǎn)為一級(jí)共視點(diǎn)集合{Point1}。所有與集合{Point1}中的任意點(diǎn)相關(guān)聯(lián)的，除新關(guān)鍵幀以外的關(guān)鍵幀集合為一級(jí)共視幀集合{Frame1}。定義與集合{Frame1}中的任意幀相關(guān)聯(lián)，且不在{Point1}集合中的地圖點(diǎn)構(gòu)成的集合為二級(jí)共視點(diǎn)集合{Point2}，與集合{Point2}中的任意點(diǎn)相關(guān)聯(lián)，且不在集合{Frame1}中的關(guān)鍵幀構(gòu)成的集合記為二級(jí)共視幀集合{Frame2}。采用滑窗BA優(yōu)化的方法，將新關(guān)鍵幀對(duì)應(yīng)的所有一級(jí)以內(nèi)共視幀的位姿，和他們關(guān)聯(lián)的三維地圖點(diǎn)位置進(jìn)行BA優(yōu)化，而二級(jí)共視幀的位姿固定不變。

圖16 新關(guān)鍵幀的共視幀和共視點(diǎn)關(guān)系示意圖Fig.16 Common view frames and points of a new key frame

定義表示二級(jí)以內(nèi)共視點(diǎn)集合中第i個(gè)待優(yōu)化的地圖點(diǎn)世界坐標(biāo)，表示一級(jí)以內(nèi)共視幀集合{}中第j個(gè)待優(yōu)化關(guān)鍵幀的相機(jī)位姿矩陣表示二級(jí)共視幀集合{}中第k個(gè)關(guān)鍵幀的相機(jī)位姿矩陣。定義表示待優(yōu)化三維地圖點(diǎn)集合{}中，第i個(gè)地圖點(diǎn)在待優(yōu)化幀集合{}中第j幀匹配的特征點(diǎn)的廣義像素坐標(biāo)。表示待優(yōu)化三維地圖點(diǎn)集合{}中，第i個(gè)地圖點(diǎn)在二級(jí)共視幀集合{}中第k幀匹配的特征點(diǎn)的廣義像素坐標(biāo)。構(gòu)造如下所示代價(jià)函數(shù)：

通過(guò)優(yōu)化求解代價(jià)函數(shù)（27），對(duì)集合{}和集合{}中所有元素的取值進(jìn)行優(yōu)化求解，實(shí)現(xiàn)定位地圖的滑窗BA優(yōu)化。

5 實(shí)驗(yàn)與分析

為了驗(yàn)證提出的異構(gòu)協(xié)同SLAM系統(tǒng)的性能，通過(guò)3組實(shí)驗(yàn)從特征匹配性能、系統(tǒng)魯棒性和相機(jī)軌跡精度方面，對(duì)設(shè)計(jì)的異構(gòu)協(xié)同SLAM系統(tǒng)性能進(jìn)行實(shí)驗(yàn)驗(yàn)證。

3組實(shí)驗(yàn)均在筆記本電腦上運(yùn)行，其主頻為2.30 GHz，處理器為Intel i7-11 800 H，操作系統(tǒng)為Ubuntu 20.04。

使用的RGBD相機(jī)是Intel RealSense D435相機(jī)，分辨率設(shè)置為640×480。RGBD相機(jī)采用Brown-Conrady相機(jī)模型，其橫、縱焦距fx、fy分別為604.590、604.373；主點(diǎn)坐標(biāo)cx、cy為321.359、237.346；畸變系數(shù)k1、k2、p1、p2、k3分別為0.136、-0.265、-0.000 316、0.002 5、0。

使用的魚眼相機(jī)是一個(gè)配有210°視場(chǎng)角魚眼鏡頭的USB相機(jī)，分辨率設(shè)置為1 024×768。魚眼相機(jī)采用Kanala-Brandt4相機(jī)模型，其橫、縱焦距fx、fy分別為257.280、257.280；主點(diǎn)坐標(biāo)cx、cy為582.006、419.655；畸變系數(shù)k1、k2、k3、k4分別為-0.076 5、0.009 08、-0.011 7、0.003 73。

提出的異構(gòu)協(xié)同SLAM系統(tǒng)的初始化流程如圖17所示。

圖17 異構(gòu)協(xié)同SLAM系統(tǒng)初始化流程Fig.17 Initialization process of heterogeneous collaborative SLAM system

系統(tǒng)先采用ORBSLAM2-RGBD的初始化方法，完成SLAM系統(tǒng)RGBD通道的地圖初始化。然后通過(guò)暴力匹配和MLPnP［39］方法，對(duì)RGBD和魚眼圖像序列不斷進(jìn)行特征點(diǎn)匹配和位姿估計(jì)。如果算法能夠在某一對(duì)RGBD和魚眼圖像幀，計(jì)算出包含50組以上內(nèi)點(diǎn)的相對(duì)相機(jī)位姿，則判定該魚眼幀的相機(jī)位姿成功估計(jì)。此時(shí)RGBD相機(jī)和魚眼相機(jī)的相對(duì)位姿已知，進(jìn)而可以利用提出的異構(gòu)圖像分步式光流-投影匹配方法，對(duì)RGBD圖像和魚眼圖像進(jìn)行連續(xù)且穩(wěn)定的特征點(diǎn)匹配和相對(duì)相機(jī)位姿估計(jì)。通過(guò)利用異構(gòu)匹配算法估計(jì)出的一系列魚眼相機(jī)位姿，以及魚眼圖像之間匹配的特征點(diǎn)對(duì)，對(duì)魚眼相機(jī)視野中的特征點(diǎn)進(jìn)行三角化，構(gòu)造三維地圖點(diǎn)，以實(shí)現(xiàn)魚眼SLAM通道的初始化。

5.1 性能驗(yàn)證

本實(shí)驗(yàn)利用在不同真實(shí)場(chǎng)景中拍攝的魚眼圖像和RGBD圖像視頻流，對(duì)本文提出的匹配方法性能進(jìn)行驗(yàn)證。在視頻流拍攝過(guò)程中，選取花園、走廊、實(shí)驗(yàn)室、辦公室等場(chǎng)景，共拍攝6組數(shù)據(jù)集，每一組數(shù)據(jù)集中，2個(gè)相機(jī)視頻流的圖像幀數(shù)相等。

在一個(gè)數(shù)據(jù)集中，若2個(gè)相機(jī)視頻流的幀數(shù)均為n，算法對(duì)第i幀魚眼圖像和第i幀RGBD圖像進(jìn)行匹配，當(dāng)i≠1時(shí)，算法還對(duì)第i幀魚眼圖像和第i-1幀RGBD圖像進(jìn)行匹配。所以，在幀數(shù)為n的視頻流數(shù)據(jù)集中，算法總共的匹配次數(shù)為2n-1。

為了對(duì)提出的異構(gòu)匹配算法性能進(jìn)行分析比較，實(shí)驗(yàn)中采用暴力匹配方法、基于詞袋模型（Bag of Words，BoW）的匹配法和快速最近鄰逼近搜索函數(shù)庫(kù)（FastApproximate Nearest Neighbor Search Library，F(xiàn)LANN）匹配法作為對(duì)比算法。對(duì)于暴力匹配法和FLANN匹配法，本實(shí)驗(yàn)使用OpenCV提供的函數(shù)。對(duì)于BoW匹配法，本實(shí)驗(yàn)采用ORBSLAM2中提供的函數(shù)。

對(duì)所有參與實(shí)驗(yàn)驗(yàn)證的匹配方法輸出的匹配結(jié)果，均采用MLPnP［39］的方法對(duì)魚眼和RGBD相機(jī)的相對(duì)位姿進(jìn)行估計(jì)。然后借助位姿信息，通過(guò)將所有RGBD深度有效點(diǎn)對(duì)應(yīng)的三維點(diǎn)投影到魚眼圖像中進(jìn)行投影匹配，并通過(guò)重投影誤差的大小判斷每一組匹配點(diǎn)對(duì)是否是內(nèi)點(diǎn)。實(shí)驗(yàn)中，當(dāng)投影點(diǎn)和對(duì)應(yīng)匹配點(diǎn)之間的像素距離誤差＜3像素時(shí)，判定其為內(nèi)點(diǎn)。如果內(nèi)點(diǎn)數(shù)量≥50，則認(rèn)為算法成功地實(shí)現(xiàn)了一次相對(duì)位姿估計(jì)。算法對(duì)異構(gòu)相機(jī)之間的相對(duì)位姿估計(jì)成功率越高，代表其匹配性能越好。

本文算法的部分匹配結(jié)果示例圖如圖18所示。算法在每一組數(shù)據(jù)集中的匹配實(shí)驗(yàn)結(jié)果如表1和圖19、圖20所示，平均每幀匹配用時(shí)如表2所示。其中，在圖20所示的條形分布圖中，一個(gè)橫條代表算法在一個(gè)數(shù)據(jù)集中，所進(jìn)行的異構(gòu)匹配與異構(gòu)相機(jī)之間的相對(duì)位姿估計(jì)。綠色區(qū)域代表成功的相對(duì)位姿估計(jì)。紅色區(qū)域代表失敗的相對(duì)位姿估計(jì)。綠色區(qū)域占比越大，代表相對(duì)位姿估計(jì)的成功率越高，匹配性能越好。

表1 算法在不同數(shù)據(jù)集中的匹配成功次數(shù)和成功率Table 1 Matching success times and success rate in different data sets

表2 算法的平均每幀匹配用時(shí)Table 2 Average matching time per frame

圖18 本文匹配算法的異構(gòu)匹配結(jié)果示例圖Fig.18 Several examples of matching results of proposed heterogeneous matching method

圖19 算法在不同數(shù)據(jù)集中的匹配成功率柱狀圖Fig.19 Histogram of matching success rates in different datasets

圖20 算法在不同數(shù)據(jù)集中的成功匹配與失敗匹配的條形分布圖Fig.20 Bar graph of successful matching and failed matching in different datasets

從圖18中可以看出，提出的分步式光流-投影匹配方法在不同場(chǎng)景中，并且無(wú)論RGBD相機(jī)視野和魚眼相機(jī)視野交疊區(qū)域位于魚眼圖像中央還是邊緣，均能夠找到較多的匹配點(diǎn)，具有較好的匹配性能。從圖19、圖20和表1中可以看出，在6個(gè)實(shí)驗(yàn)數(shù)據(jù)集中，本文算法對(duì)異構(gòu)相機(jī)之間相對(duì)位姿估計(jì)的成功率均遠(yuǎn)高于暴力匹配、BoW和FLANN匹配方法，并始終在90%以上的成功率。表2中，本文算法的平均每幀用時(shí)為53.539 ms，計(jì)算速度慢于其他匹配算法，但仍然能夠滿足工程應(yīng)用需求。實(shí)驗(yàn)結(jié)果表明，提出的分步式光流-投影匹配方法在異構(gòu)相機(jī)視頻流之間表現(xiàn)出突出的匹配性能和異構(gòu)相機(jī)相對(duì)位姿估計(jì)性能。

5.2 各相機(jī)通道的性能驗(yàn)證

本實(shí)驗(yàn)通過(guò)考察異構(gòu)協(xié)同SLAM的RGBD相機(jī)SLAM通道和魚眼相機(jī)SLAM通道在不同條件下的地圖構(gòu)建完整度，來(lái)驗(yàn)證異構(gòu)協(xié)同SLAM系統(tǒng)2個(gè)相機(jī)通道的性能。

5.2.1 魯棒性驗(yàn)證

為了驗(yàn)證異構(gòu)協(xié)同SLAM系統(tǒng)RGBD相機(jī)通道的魯棒性，本實(shí)驗(yàn)利用相機(jī)在真實(shí)室內(nèi)環(huán)境拍攝的視頻序列對(duì)RGBD通道構(gòu)建的點(diǎn)云地圖的完整度進(jìn)行評(píng)估。實(shí)驗(yàn)通過(guò)手持移動(dòng)拍攝的方式在室內(nèi)環(huán)境中采集視頻序列，作為SLAM系統(tǒng)的輸入。

本實(shí)驗(yàn)共拍攝5組視頻序列。第1組是相機(jī)在紋理豐富的室內(nèi)環(huán)境中正常移動(dòng)拍攝的視頻序列（總幀數(shù)為863），每一幀圖像平均灰度梯度取值范圍為2～6，每一幀圖像的平均光流取值范圍為0～50像素；第2組是相機(jī)在快速移動(dòng)的條件下拍攝的視頻序列（總幀數(shù)為457），該序列中存在32幀平均光流為80～120像素的圖像幀，其中包含有20幀因?yàn)檫\(yùn)動(dòng)模糊導(dǎo)致平均灰度梯度＜2的圖像幀；第3組的拍攝過(guò)程中，相機(jī)多次貼近環(huán)境中的物體，使得相機(jī)視野中出現(xiàn)紋理分布嚴(yán)重不均勻的現(xiàn)象，在總長(zhǎng)為930幀的序列圖像中，因?yàn)榕臄z過(guò)程中出現(xiàn)相機(jī)前方物體與相機(jī)的距離＜1 m的情況，圖像序列中存在連續(xù)15幀平均灰度梯度＜1的視頻序列區(qū)段；第4組是相機(jī)在低幀率條件下拍攝的視頻序列（總幀數(shù)為310），在圖像序列中，存在55幀平均光流為80～120像素的圖像幀；第5組是相機(jī)在一個(gè)白墻較多的紋理缺失環(huán)境中拍攝的視頻序列，在總長(zhǎng)為967幀的圖像序列中，存在423幀平均灰度梯度＜1的圖像幀。

實(shí)驗(yàn)選用ORBSLAM2-RGBD、OpenVSLAMRGBD作為對(duì)比算法。通過(guò)比較輸出的點(diǎn)云地圖的完整度，來(lái)驗(yàn)證RGBD-SLAM通道的系統(tǒng)魯棒性。5組視頻序列的部分示例圖片如圖21所示。SLAM算法的定位成功幀和定位失敗幀在數(shù)據(jù)集中的條形分布圖的如圖22所示，構(gòu)建的稠密場(chǎng)景點(diǎn)云地圖如圖23所示。

圖21 室內(nèi)不同拍攝條件下的數(shù)據(jù)集示例圖片F(xiàn)ig.21 Sample pictures of data sets under different indoor shooting conditions

圖23 不同拍攝條件下RGBD-SLAM通道構(gòu)建的場(chǎng)景點(diǎn)云地圖Fig.23 Point cloud maps constructed by RGBDSLAM channel under different conditions

從圖22可以看出，在本文的異構(gòu)協(xié)同SLAM系統(tǒng)中，相比于RGBD-SLAM通道，魚眼SLAM通道的定位失敗幀占比最小，說(shuō)明魚眼SLAM通道的魯棒性較高。相比于OpenVSLAM-RGBD和ORBSLAM2-RGBD，本文算法對(duì)應(yīng)的RGBD-SLAM通道的定位成功幀在相機(jī)快速移動(dòng)條件下、相機(jī)貼近景物條件下和低幀率條件下的實(shí)驗(yàn)結(jié)果中占比最高，在紋理信息不足的條件下占比高于ORBSLAM2-RGBD。

從圖23可以看出，當(dāng)相機(jī)在紋理豐富的場(chǎng)景中正常移動(dòng)拍攝時(shí)，3個(gè)SLAM參評(píng)算法都能夠利用視頻流的圖像信息構(gòu)建出較為完整的地圖。當(dāng)存在相機(jī)快速移動(dòng)、相機(jī)貼近環(huán)境物體、低幀率、紋理缺失的不利條件時(shí)，OpenVSLAMRGBD和ORBSLAM2-RGBD算法由于跟蹤丟失，沒有構(gòu)建出視頻流中拍攝到的全部場(chǎng)景，只構(gòu)建出了部分場(chǎng)景的點(diǎn)云地圖，而本文算法的RGBD-SLAM通道都能夠構(gòu)建出更為完整的地圖。

5.2.2 軌跡漂移程度驗(yàn)證

本實(shí)驗(yàn)利用相機(jī)在真實(shí)室外環(huán)境拍攝的3組視頻序列，來(lái)驗(yàn)證異構(gòu)協(xié)同SLAM系統(tǒng)的軌跡漂移程度。在數(shù)據(jù)集拍攝的過(guò)程中，相機(jī)的軌跡首尾相接形成閉環(huán)。實(shí)驗(yàn)通過(guò)手持移動(dòng)拍攝的方式采集視頻序列，作為SLAM系統(tǒng)的輸入。選用ORBSLAM3-Fisheye、OpenVSLAM-Fisheye、ORBSLAM2-RGBD、OpenVSLAM-RGBD作為對(duì)比算法。在關(guān)閉所有參評(píng)算法回環(huán)檢測(cè)功能的前提下，對(duì)拍攝場(chǎng)景進(jìn)行稀疏點(diǎn)云地圖構(gòu)建并輸出相機(jī)軌跡。通過(guò)觀察SLAM算法輸出的相機(jī)軌跡首段和尾段之間的距離，以及地圖不同區(qū)域的尺度差異，來(lái)判斷SLAM過(guò)程中軌跡漂移的程度。拍攝的室外場(chǎng)景衛(wèi)星圖和對(duì)應(yīng)的相機(jī)移動(dòng)大致軌跡（白色帶箭頭線條），以及SLAM算法輸出的相機(jī)軌跡和構(gòu)建的稀疏點(diǎn)云地圖如圖24所示。

圖24 室外條件下SLAM算法的相機(jī)軌跡和構(gòu)建的場(chǎng)景點(diǎn)云地圖Fig.24 Camera trajectories and point cloud maps constructed under different conditions

在圖24（a）中，本文算法未發(fā)生尺度漂移，且姿態(tài)和軌跡漂移較小，相機(jī)軌跡首段和尾段幾乎重合，而ORBSLAM3-Fisheye、OpenVSLAMFisheye的結(jié)果均發(fā)生了較大的尺度漂移，其中，ORBSLAM3-Fisheye的軌跡末段比例尺約縮減為其初段的0.015倍，OpenVSLAM-Fisheye的軌跡末段比例尺約縮減為其初段的0.007 5倍，ORBSLAM2-RGBD和OpenVSLAM-RGBD雖沒有尺度漂移，但它們的結(jié)果發(fā)生了約20°～30°的相機(jī)姿態(tài)漂移和約5～10 m的軌跡漂移。在圖24（b）中，本文算法未發(fā)生尺度和姿態(tài)漂移，但發(fā)生了約10 m的軌跡漂移，ORBSLAM3-Fisheye、OpenVSLAM-Fisheye的結(jié)果均發(fā)生了較大的尺度漂移，其中，ORBSLAM3-Fisheye的軌跡末段比例尺約縮減為其初段的0.01倍，OpenVSLAM-Fisheye的軌跡末段比例尺約縮減為其初段的0.067倍，ORBSLAM2-RGBD中途跟蹤丟失，OpenVSLAM-RGBD的結(jié)果發(fā)生了約40°的相機(jī)姿態(tài)漂移和約40 m的軌跡漂移。在圖24（c）中，本文算法未發(fā)生尺度漂移，但發(fā)生了約20°的相機(jī)姿態(tài)漂移和約20 m的軌跡漂移，OpenVSLAM-Fisheye中途跟蹤丟失，ORBSLAM3-Fisheye的結(jié)果發(fā)生了較大的尺度漂移，其軌跡末段比例尺縮減為其初段的0.18倍，ORBSLAM2-RGBD中途跟蹤丟失，OpenVSLAM-RGBD的結(jié)果發(fā)生了近180°的相機(jī)姿態(tài)漂移和約60 m的軌跡漂移。實(shí)驗(yàn)結(jié)果表明，在本文提出的異構(gòu)協(xié)同SLAM系統(tǒng)中，單目魚眼SLAM通道能夠利用RGBD-SLAM通道賦予的深度信息有效抑制尺度漂移，RGBD-SLAM通道能夠利用魚眼SLAM通道大視野優(yōu)勢(shì)，抑制相機(jī)姿態(tài)漂移和軌跡漂移。

5.3 軌跡精度驗(yàn)證

為了驗(yàn)證異構(gòu)協(xié)同SLAM系統(tǒng)的軌跡精度，本實(shí)驗(yàn)利用相機(jī)在真實(shí)室內(nèi)環(huán)境拍攝的視頻序列對(duì)算法輸出的軌跡精度進(jìn)行評(píng)測(cè)。實(shí)驗(yàn)采用手持移動(dòng)拍攝的方式在室內(nèi)環(huán)境中采集視頻序列，作為SLAM系統(tǒng)的輸入。由于2個(gè)相機(jī)為非時(shí)鐘同步和非剛性連接的關(guān)系，2個(gè)相機(jī)的運(yùn)動(dòng)狀態(tài)是相互獨(dú)立的，所以本文算法對(duì)2個(gè)相機(jī)的軌跡同時(shí)進(jìn)行估計(jì)。

為了對(duì)算法輸出的相機(jī)軌跡精度進(jìn)行評(píng)估，在每個(gè)相機(jī)的機(jī)身上安裝定位球，通過(guò)Optitrack運(yùn)動(dòng)捕捉系統(tǒng)，對(duì)相機(jī)的真實(shí)運(yùn)動(dòng)軌跡進(jìn)行精確測(cè)定和記錄，并將該測(cè)定結(jié)果作為真值。

實(shí)驗(yàn)選用ORBSLAM2-RGBD、OpenVSLAMRGBD、ORBSLAM3-Fisheye、OpenVSLAMFisheye作為對(duì)比基準(zhǔn)。ORBSLAM2-RGBD和OpenVSLAM-RGBD以RGBD視頻序列作為輸入，ORBSLAM3-Fisheye和OpenVSLAM-Fisheye以單目魚眼圖像序列作為輸入，本文算法以RGBD圖像序列和魚眼圖像序列作為輸入。

在軌跡精度評(píng)測(cè)過(guò)程中，首先對(duì)算法輸出的相機(jī)軌跡與Optitrack測(cè)得的相機(jī)軌跡真值進(jìn)行配準(zhǔn)，然后計(jì)算每條算法軌跡對(duì)應(yīng)的均方根誤差（Root Mean Squared Error，RMSE）作為軌跡誤差，該值越小代表軌跡精度越高。

對(duì)于單目SLAM算法，ORBSLAM3-Fisheye和OpenVSLAM-Fisheye，其輸出的軌跡沒有尺度信息，所以單目SLAM算法的軌跡誤差需要在相似變換配準(zhǔn)的條件下計(jì)算才有效。因此，本實(shí)驗(yàn)分別采用相似變換（Sim3）和歐氏變換（SO3）對(duì)軌跡進(jìn)行配準(zhǔn)，并分別計(jì)算RMSE。

本實(shí)驗(yàn)中，共使用9組自行拍攝的數(shù)據(jù)集。SLAM算法的RMSE實(shí)驗(yàn)結(jié)果如圖25所示。其中，8號(hào)數(shù)據(jù)集對(duì)應(yīng)的相機(jī)運(yùn)動(dòng)軌跡估計(jì)結(jié)果如圖26所示，軌跡誤差曲線圖如圖27所示。

圖25 軌跡誤差柱狀圖Fig.25 Trajectory error histogram

圖26 8號(hào)數(shù)據(jù)集中的相機(jī)軌跡Fig.26 Camera trajectories in Dataset 8

圖27 SLAM算法在8號(hào)數(shù)據(jù)集中的軌跡誤差曲線圖Fig.27 Graph of trajectory errors of SLAM algorithm in Dataset 8

從圖25中可以看出，本文算法輸出的RGBD相機(jī)和魚眼相機(jī)的軌跡精度均優(yōu)于其他4種對(duì)比的SLAM算法，且在異構(gòu)協(xié)同SLAM系統(tǒng)中，魚眼相機(jī)的軌跡精度比RGBD相機(jī)的軌跡精度更高。由于ORBSLAM3-Fisheye和OpenVSLAM-Fisheye在2、3、4、6、7、8號(hào)數(shù)據(jù)集中發(fā)生了尺度漂移，因此其軌跡誤差在這些數(shù)據(jù)集中遠(yuǎn)大于其他算法。從圖26中可以看出，相比于其他算法，本文算法的相機(jī)軌跡能夠更好地?cái)M合相機(jī)的真實(shí)運(yùn)動(dòng)軌跡。從圖27中可以看出，本文算法的軌跡誤差曲線均位于其他算法誤差曲線的下方。本節(jié)實(shí)驗(yàn)結(jié)果表明，本文設(shè)計(jì)的異構(gòu)協(xié)同SLAM算法能夠?qū)崿F(xiàn)SLAM系統(tǒng)中魚眼相機(jī)和RGBD相機(jī)的優(yōu)勢(shì)互補(bǔ)，提升了相機(jī)的軌跡精度。

6 結(jié)論

本文設(shè)計(jì)了基于魚眼相機(jī)和RGBD相機(jī)的異構(gòu)協(xié)同SLAM系統(tǒng)。首先基于之前的工作FSD-BRIEF設(shè)計(jì)了基于三維灰度質(zhì)心方向一致性的候選匹配點(diǎn)篩選方法。為了提高異構(gòu)圖像匹配的性能，結(jié)合MLPnP算法，設(shè)計(jì)異構(gòu)圖像之間的分步式光流-投影匹配方法，實(shí)現(xiàn)魚眼相機(jī)和RGBD相機(jī)之間的同時(shí)相對(duì)位姿估計(jì)和特征點(diǎn)匹配。最后基于ORBSLAM2框架，提出魚眼相機(jī)和RGBD相機(jī)異構(gòu)協(xié)同SLAM系統(tǒng)的框架。實(shí)驗(yàn)結(jié)果表明：

1）相比于傳統(tǒng)匹配方法0%～75%的成功率，本文提出的分步式光流-投影匹配方法能夠較大地提升異構(gòu)圖像特征匹配的性能，可將異構(gòu)相機(jī)相對(duì)位姿計(jì)算的成功率提升至90%以上。

2）相比于其他單目魚眼SLAM和RGBDSLAM算法，本文提出的異構(gòu)協(xié)同SLAM系統(tǒng)能夠較大地提升RGBD相機(jī)SLAM通道的魯棒性、抗軌跡漂移能力和魚眼SLAM通道的抗尺度漂移能力，使RGBD-SLAM通道能夠克服相機(jī)快速移動(dòng)、貼近景物、低幀率、紋理缺失等不利條件，在稠密地圖構(gòu)建完整度上有50%以上的提升。同時(shí)，異構(gòu)協(xié)同SLAM系統(tǒng)在室外大場(chǎng)景的數(shù)據(jù)集中，可有效降低魚眼SLAM通道尺度漂移和RGBD-SLAM通道的相機(jī)姿態(tài)漂移、軌跡漂移現(xiàn)象。

3）相比于傳統(tǒng)的RGBD-SLAM和單目魚眼SLAM系統(tǒng)，本文提出的異構(gòu)協(xié)同SLAM系統(tǒng)的軌跡精度有較大提升，如在5.3節(jié)中的8號(hào)數(shù)據(jù)集中，相比于其他SLAM算法，相機(jī)軌跡的均方根誤差減少了66%。

綜上，實(shí)驗(yàn)結(jié)果證明了本文提出的異構(gòu)協(xié)同SLAM系統(tǒng)能夠有效地結(jié)合魚眼相機(jī)和RGBD相機(jī)的優(yōu)勢(shì)，較大地提升了系統(tǒng)的魯棒性和軌跡精度。