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

北京理工大學 宇航學院，北京 100081

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

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

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

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

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

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

1）設計一個新的基于魚眼相機和RGBD相機的異構協(xié)同SLAM系統(tǒng)。

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

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

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

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

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

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

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

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

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

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

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

2 候選匹配點篩選方法設計

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

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

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

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

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

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

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

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

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

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

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

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

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

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

在實驗中，分別使用魚眼相機和RGBD相機對如圖6所示的靶標圖案以不同角度拍攝14張圖片，部分拍攝示例圖片如圖7所示。

圖6 用于測試特征匹配算法的靶標圖案Fig.6 Target image for feature matching algorithm test

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

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

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

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

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

3 分步式光流投影匹配方法設計

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

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

3.1 魚眼圖像

針對魚眼圖像設計的分步式光流-投影匹配過程如圖9所示。

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

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

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

圖10 魚眼相機當前幀光流跟蹤過程示意圖Fig.10 Schematic diagram of current frame optical flow tracking process of fisheye camera

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

記點Pi在世界坐標系中的坐標為Pi，w，有

式中：Tw，DL代表從圖像DL的相機坐標系到世界坐標系的坐標變換矩陣。

記圖像FL中，第j個與三維地圖點有匹配關系的特征點像素坐標為p′j，F(xiàn)L，其對應的地圖點為P′j，在世界坐標系中的坐標為P′j，w。

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

式（7）代表圖像FL中的點坐標通過使用LK光流法從圖像FL跟蹤到圖像FC中，得到點。同理，對于點，有

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

式中：eFC(p，P)表示圖像FC對應的相機坐標系下，三維點P到像素點p的重投影誤差。

圖11為魚眼相機當前幀投影匹配過程的示意圖。在圖11中，點坐標集{}和{}分別是點集{Pi，w}中的點Pi，w和點集{Pj′，w}中的點Pj′，w投影到圖像FC中得到的投影點構成的點集，即

圖11 魚眼相機當前幀投影匹配過程示意圖Fig.11 Schematic diagram of current frame projection matching process of fisheye camera

式中：ΠFC表示從圖像FC對應相機坐標系下的三維坐標，到圖像FC圖像像素坐標的投影函數(shù)。

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

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

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

同理，對于點集{}的每一個點，在{}中找到匹配點，滿足：

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

3.2 RGBD圖像

針對RGBD圖像設計的分步式光流-投影匹配過程如圖12所示。

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

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

圖13為RGBD相機當前幀光流跟蹤過程的示意圖。在圖13中，DL表示RGBD相機上一幀圖像，DC表示RGBD相機當前幀圖像。利用光流法，對點集{pi，DL}從圖像DL跟蹤到圖像DC，得到圖像DC中的點集{}，有

圖13 RGBD相機當前幀光流跟蹤過程示意圖Fig.13 Schematic diagram of current frame optical flow tracking process of RGBD camera

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

式中：eDC( )表示圖像DC對應相機坐標系下，三維點P到像素點p的重投影誤差。

圖14為RGBD相機當前幀投影匹配過程的示意圖。將三維點集{Pi，w}的每一個點Pi，w投影到圖像DC的圖像平面中，形成的點集記為{}，有

圖14 RGBD相機當前幀投影匹配過程示意圖Fig.14 Schematic diagram of current frame projection matching process of RGBD camera

式中：ΠDC表示從圖像DC對應相機坐標系下的三維坐標，到圖像DC像素坐標的投影函數(shù)。

記圖像DC中提取的特征點集為{}。利用式（11）所示的投影匹配方法，尋找點集{}中的每一個點，在點集{}中對應的匹配點，有

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

利用投影匹配方法，尋找每一個在中的匹配點，有

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

將點向圖像FC中進行投影，得到其在圖像FC中的像素坐標，有

對于每一個投影點，在圖像FC中的特征點集合{}中進行投影匹配，找到對應的匹配點，有

將點向圖像DL中進行投影，得到其在圖像DL中的像素坐標，有

式中：ΠDL表示從圖像DL對應相機坐標系下的三維坐標，到圖像DL像素坐標的投影函數(shù)。

對于每一個投影點，在圖像DL中的特征點集合{pi，DL}中進行投影匹配，找到對應的匹配點，有

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

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

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

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

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

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

圖16 新關鍵幀的共視幀和共視點關系示意圖Fig.16 Common view frames and points of a new key frame

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

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

5 實驗與分析

為了驗證提出的異構協(xié)同SLAM系統(tǒng)的性能，通過3組實驗從特征匹配性能、系統(tǒng)魯棒性和相機軌跡精度方面，對設計的異構協(xié)同SLAM系統(tǒng)性能進行實驗驗證。

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

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

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

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

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

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

5.1 性能驗證

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

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

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

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

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

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

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

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

5.2 各相機通道的性能驗證

本實驗通過考察異構協(xié)同SLAM的RGBD相機SLAM通道和魚眼相機SLAM通道在不同條件下的地圖構建完整度，來驗證異構協(xié)同SLAM系統(tǒng)2個相機通道的性能。

5.2.1 魯棒性驗證

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

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

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

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

圖23 不同拍攝條件下RGBD-SLAM通道構建的場景點云地圖Fig.23 Point cloud maps constructed by RGBDSLAM channel under different conditions

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

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

5.2.2 軌跡漂移程度驗證

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

圖24 室外條件下SLAM算法的相機軌跡和構建的場景點云地圖Fig.24 Camera trajectories and point cloud maps constructed under different conditions

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

5.3 軌跡精度驗證

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

為了對算法輸出的相機軌跡精度進行評估，在每個相機的機身上安裝定位球，通過Optitrack運動捕捉系統(tǒng)，對相機的真實運動軌跡進行精確測定和記錄，并將該測定結果作為真值。

實驗選用ORBSLAM2-RGBD、OpenVSLAMRGBD、ORBSLAM3-Fisheye、OpenVSLAMFisheye作為對比基準。ORBSLAM2-RGBD和OpenVSLAM-RGBD以RGBD視頻序列作為輸入，ORBSLAM3-Fisheye和OpenVSLAM-Fisheye以單目魚眼圖像序列作為輸入，本文算法以RGBD圖像序列和魚眼圖像序列作為輸入。

在軌跡精度評測過程中，首先對算法輸出的相機軌跡與Optitrack測得的相機軌跡真值進行配準，然后計算每條算法軌跡對應的均方根誤差（Root Mean Squared Error，RMSE）作為軌跡誤差，該值越小代表軌跡精度越高。

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

本實驗中，共使用9組自行拍攝的數(shù)據(jù)集。SLAM算法的RMSE實驗結果如圖25所示。其中，8號數(shù)據(jù)集對應的相機運動軌跡估計結果如圖26所示，軌跡誤差曲線圖如圖27所示。

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

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

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

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

6 結論

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

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

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

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

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