基于鱼眼相机和RGBD相机的异构协同SLAM

张宇桐，宋建梅，丁艳，刘锦鹏

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

同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）技术［1-3］，是指一个处在未知环境当中的运动载体，通过携带的传感器来获取环境相关信息，构建一个表征周围环境信息的地图，同时以该地图为参照来实现自主定位的技术。常用的视觉SLAM系统分为单目SLAM、双目SLAM和红绿蓝-深度相机SLAM（Red-Green-Blue-Depth SLAM，RGBD-SLAM）3大类。单目SLAM具有硬件简单、计算效率高、能满足实时性要求等优点，因此受到了人们广泛的关注［4-7］。此外，单目SLAM可以选用鱼眼相机等大视场相机。通过增大视野，可提升单帧图像看到的场景范围，进而可获得更多的场景纹理信息，实现更加稳定的定位和建图［8-9］。2015年，Caruso 等［10］基于LSD-SLAM（Large-Scale Direct Monocular SLAM）［11］提出了适用于全景或鱼眼相机的SLAM系统，该系统即使在相对较低的分辨率（240×240）下，定位精度也超过了使用针孔模型时获得的精度。2016年，Zhang等［12］基于半直接法视觉里程计［13］（Semi-direct Visual Odometry，SVO）设计了适用于鱼眼相机和折反射相机的半直接法SLAM系统，在室内环境中，该系统的相机轨迹精度比使用针孔相机的SVO系统更高。2018年， Matsuki等［14］对直接稀疏里程计（Direct Sparse Odometry，DSO）系统［15］做了改进，提出了适用于广角图像的全向直接稀疏里程计（Omnidirectional DSO）系统，使DSO系统的轨迹精度和鲁棒性均得以提高。同年，Wang等提出了CubemapSLAM［9］系统，将鱼眼图像投影到一个立方体的5个表面，并生成包含5个子视野的立方体表面展开图，来实现鱼眼图像的去畸变，并被Jo等［16］成功应用在了一个自动泊车系统中。2019年，Liu等［17］利用增强型统一相机模型［18］（Enhanced Unified Camera Model，EUCM）对ORB-SLAM［7］系统进行了改进，使ORBSLAM能够利用鱼眼相机进行实时且鲁棒的定位建图。2021年，Garcia等［19］利用卷积神经网络（Convolutional Neural Network，CNN）对Liu等的工作［17］进行了改进。他们利用CNN对每一帧鱼眼图像进行语义分割，并剔除天空等离群点较多区域的特征点，进一步提高了SLAM系统的定位精度。

为了进一步增大SLAM系统的环境感知能力，基于多目鱼眼的全景SLAM系统相继被提出。2016年， Urban和Hinz提出了Multicol-SLAM［20］系统。该系统适用于任意数量、任意模型相机以任意相对位姿刚性耦合的多相机系统，其鲁棒性相比于单相机SLAM系统有较大提升。2020年，Ji等基于LadyBug多目鱼眼相机开发了一种全景SLAM系统（PANSLAM）［21］，将6幅鱼眼图像投影到一个球面上，并对相机标定模型、特征匹配跟踪、回环检测、SLAM初始化过程都做了改进，使其在长基线拍摄条件下能够实现比ORB-SLAM2［22］、Multicol-SLAM［20］和Cubemap-SLAM［9］等系统更高的轨迹精度和鲁棒性。2021年，Zhang和Huang［23］设计了一个基于双鱼眼全景相机Ricoh的SLAM系统，利用SPHORB（Spherical ORB）特征［24］对全景图像进行特征提取和匹配，使得SLAM系统的精度和鲁棒性优于使用ORB（Oriented FAST and Rotated BRIEF）特征的SLAM系统。

单目SLAM无法获得尺度信息，即地图与真实环境的比例尺，这导致单目SLAM存在初始化困难、容易发生尺度漂移且无法在相机纯旋转运动的条件下工作等问题。双目SLAM系统采用双目相机之间的特征匹配信息，借助三角测距原理，获取场景的深度信息，进而可以构建含有尺度信息的地图，因此不易发生尺度漂移现象。但由于双目SLAM系统需要在线计算每一帧双目图像的匹配信息，因此存在计算量大的缺点。此外，单目和双目SLAM均无法对纹理较少的环境构建稠密地图。为了克服这些问题，基于RGBD相机的SLAM系统［25-30］受到了人们的广泛关注。由于RGBD相机可利用结构光或光线飞行时间（Time of Flight，TOF）等技术，直接测量得到视野中每一个像素的深度值，故RGBD-SLAM系统相比于双目SLAM系统具备更实时的尺度信息获取能力。Audras等［25］提出了一种直接法RGBD-SLAM系统，基于光度信息，对每一帧RGBD图像进行光度匹配来实现SLAM过程。同年，Newcombe等［26］提出了KinectFusion系统。KinectFusion采用截断有符号距离函数（Truncated Signed Distance Functions，TSDF），利用曲面模型来表示场景的几何信息。通过使用迭代最近点［31］（Iterative Closest Point，ICP）的方法拼接场景曲面，以曲面生长的形式构建全局地图。Endres等提出了RGBD-SLAM v2算法［29］，该算法前端利用尺度不变特征变换（Scale-Invariant Feature Transform，SIFT）特征［32］进行特征提取和匹配来计算相机轨迹，后端执行位姿图优化并带有回环检测功能。

此外，能够兼容多种不同类型相机的SLAM系统相继被提出。Mur-Artal等对其构建的ORB-SLAM［7］进行了改进，提出了ORBSLAM2［22］系统。该系统是一个开源的，同时支持单目、双目、RGBD相机模型的，具备前端、后端、回环检测功能的完整SLAM系统。ORBSLAM2的轨迹精度相比于以往的基于ICP方法的RGBD-SLAM系统［26-28］有较大提升。Sumikura等提出了OpenVSLAM系统［33］，该系统支持单目、双目、RGBD、鱼眼、全景相机等多种相机模型，并且支持地图的二次利用。Campos等对ORB-SLAM2做了进一步改进并提出了ORBSLAM3［34］，此系统在ORB-SLAM2的基础上，增加了鱼眼相机模型、融合惯性测量单元（Inertial Measurement Unit，IMU）的视觉惯性SLAM和多地图管理的功能，进一步提高了系统的鲁棒性。

Yousif等［35］在2017年提出了MonoRGBD-SLAM系统。该系统是迄今为止唯一一个结合单目广角相机和RGBD相机的异构协同SLAM系统。单目广角相机和RGBD相机的异构协同机制使得MonoRGBD-SLAM系统能够结合2种相机的优势，增强系统在地图构建任务中的鲁棒性，进而能够更容易地构建更大规模的地图。然而该系统将广角相机的视野分成了9块与RGBD相机内参相同的子视野，并且选用SIFT特征进行特征点提取和匹配，这使得系统的计算量非常大。此外，如果要将广角相机替换为更大视场角的鱼眼相机（如210°视场角），则鱼眼相机的视野需要被切分出更多个子视野，以覆盖鱼眼相机的全部视野。

综上所述，在现有的工作中，基于鱼眼相机和RGBD相机的异构协同SLAM问题没有很好的解决。为了在不对鱼眼相机进行视野切分的前提下，结合鱼眼相机可获取更多纹理信息的优势和RGBD相机可直接获取稠密深度信息的优势，提出基于单目鱼眼相机（210°视场角）和RGBD相机的异构协同SLAM系统。首先基于之前的工作［36］中设计的FSD-BRIEF特征描述子，实现特征点空间方向和描述子的计算。利用特征点姿态矩阵信息对匹配点对之间的特征点灰度质心方向一致性检验方法进行设计，以对候选匹配点对进行筛选。为了克服异构图像之间因存在较大尺度差异和视野差异而导致的特征点匹配困难的问题，提出异构图像之间的分步式光流-投影匹配方法，实现鱼眼相机和RGBD相机之间的特征点匹配和相对位姿估计。最后基于ORB-SLAM2框架提出基于单目鱼眼相机和RGBD相机的异构协同SLAM系统框架。本文主要工作如下：

1）设计一个新的基于鱼眼相机和RGBD相机的异构协同SLAM系统。

2）提出一个新的特征点灰度质心方向一致性检验法，用于筛选更准确的候选匹配点对。

3）设计一个新的分步式光流-投影匹配方法，实现异构图像之间高性能的匹配和相机位姿估计。

1 鱼眼-RGBD异构协同SLAM框架

设计的异构协同SLAM系统框架如图1所示，其包括预处理、前端和后端3个部分。

图1 异构协同SLAM系统框图Fig.1 Framework of heterogeneous collaborative SLAM system

预处理部分用于对输入图像进行特征提取，为前端的特征匹配提供信息输入。采用FAST［37］特征点提取算法，在原始图像上提取特征点，并利用FSD-BRIEF［36］计算每一个特征点的描述子和姿态矩阵。

FSD-BRIEF描述子是基于球面透视模型设计的。在提取的FSD-BRIEF描述子中，重点考虑了球面透视模型具有的各向同性性质。在FSD-BRIEF描述子的计算过程中，将描述子模板放在球形投影面的切平面上，并建立描述子模板和原始图像之间的坐标映射关系，实现描述子模板向原始图像的投影和描述子计算，以充分利用球面透视模型的各向同性来实现描述子对图像畸变的不变性，如图2所示。此外，FSDBRIEF描述子可充分利用鱼眼图像全部视野，且无需对图像进行畸变矫正或者子视野截取。同时，FSD-BRIEF描述子对RGBD相机和鱼眼相机均具有通用性，提取的描述子可直接用于RGBD图像和鱼眼图像的异构匹配。

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

在图1的异构协同SLAM系统中，设计的基于特征点三维灰度质心方向一致性的候选匹配点筛选方法主要利用FSD-BRIEF给出的特征点姿态矩阵信息，对特征点方向的一致性进行判断。该方法主要用于前端的特征点投影匹配和后端特征再匹配环节中候选匹配点的筛选，以提高特征匹配的正确率。

前端部分主要用于对输入的当前图像帧和同构、异构相邻图像帧之间的特征点进行分步式光流-投影匹配，利用几何约束估计每一帧图像对应的相机位姿，得到相机的运动轨迹。利用ORB-SLAM2的相对局部地图定位（Tracklocalmap）模块，对当前帧和局部定位地图的关键帧进行匹配，并建立每一帧图像和地图点之间的匹配关系。然后在每一个成功定位的图像帧中筛选出一部分图像帧作为关键帧输入到后端。

后端负责对前端输出的新关键帧与关联的所有一级共视帧之间进行再一次的特征匹配，并通过三角化的方式在定位地图中构造出更多的地图点，来为前端提供更多可用于定位的三维路标点。同时，通过滑窗光束法平差（Bundle Adjustment，BA）的方式，对新关键帧、所有一级共视帧的位姿和所有关联的地图点位置进行优化求解，来保证定位地图的全局一致性。

2 候选匹配点筛选方法设计

为了在特征匹配过程中，筛选出更准确的候选点，本节基于FSD-BRIEF［36］中构建的特征点姿态坐标系，利用特征点姿态矩阵信息对图像匹配过程中，基于三维灰度质心方向一致性的候选匹配点筛选方法进行设计。

定义单位向量r为特征点在相机坐标系中观测方向对应的向量，向量c为特征点三维灰度质心的位置向量，如图3所示。FSD-BRIEF［36］中构建的特征点姿态坐标系用ObXbYbZb表示，其原点Ob与相机坐标系的原点Oc重合，ObZb轴的方向与向量r重合，即指向特征点的观测方向，ObYb轴与向量r×c的方向一致，ObXb轴与平面YbObZb垂直，且在向量c的同侧，以表征特征点的灰度质心方向。

图3 相机中的特征点姿态坐标系示意图Fig.3 Attitude coordinate system of a feature point in camera

灰度质心方向反映了该特征点对应三维物体点的空间几何方向信息，如图4所示。其中，坐标系Ob1Xb1Yb1Zb1和Ob2Xb2Yb2Zb2是一对分别位于2个相机中的特征点对应的姿态坐标系。同一个三维物体点在不同相机中投影点的灰度质心方向是大致一致的。

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

图5表示特征点方向一致性检验过程中的几何变量图。假设空间中有2个相机，分别定义为c1和c2。空间点P在相机c1和c2中对应的特征点为p1和p2，对应的姿态坐标系分别为Ob1Xb1Yb1Zb1和Ob2Xb2Yb2Zb2，简记为b1和b2。定义向量b2X为特征点姿态坐标系b2的X轴对应的单位向量，其代表特征点p2对应的灰度质心方向。定义向量b1X是特征点姿态坐标系b1的X轴对应的单位向量，其代表特征点p1对应的灰度质心方向。定义向量b1Z是特征点姿态坐标系b1的Z轴对应的单位向量，其代表特征点p1与空间点P连线对应的方向向量。将向量b2X平移到特征点姿态坐标系b1中，并向平面Xb1Ob1Yb1投影，得到投影向量b′2X，则有

图5 特征点方向一致性中的几何变量图Fig.5 Geometric variables in feature points’ direction consistency

式中：I3×3表示3×3单位矩阵。

定义特征点p1和p2之间的灰度质心方向夹角γ为向量b1X和向量b′2X的夹角，其计算公式为

定义阈值γth为灰度质心方向夹角γ的最大容忍误差，那么，如果γ＜γth，则判定特征点p1和p2方向一致，否则判定特征点p1和p2方向不一致，在本文算法中，γth选为20°。

在实际计算过程中，向量b2X在相机坐标系c2中的坐标b2X，c2是已知的，它是特征点p2在相机c2中的姿态矩阵Rc2b2的第1列，向量b1X和b1Z在相机坐标系c1中的坐标b1X，c1和b1Z，c1是已知的，即特征点p1在相机c1中的姿态矩阵Rc1b1的第1列和第3列。γ的计算需要在同一个参考坐标系下完成，所以将b2X的坐标转到相机坐标系c1中，则得到γ的计算公式为

式中：Rc1c2是相机坐标系c2到相机坐标系c1的姿态转换矩阵。

为了验证基于三维灰度质心方向一致性的候选匹配点筛选方法对算法匹配性能的提升作用，本文进行了算法消融实验，并采用PR曲线［38］（召回率、1-精度）作为匹配性能的评价指标。召回率和1-精度的定义为

式中：ε′为描述子距离阈值自变量。在匹配过程中，若一对特征点描述子距离最小且小于阈值ε′，才被选择为算法输出的匹配点对。

在实验中，分别使用鱼眼相机和RGBD相机对如图6所示的靶标图案以不同角度拍摄14张图片，部分拍摄示例图片如图7所示。

图6 用于测试特征匹配算法的靶标图案Fig.6 Target image for feature matching algorithm test

图7 鱼眼相机和RGBD相机拍摄的部分示例图片Fig.7 Sample pictures taken by fisheye and RGBD cameras

实验时，对采集的每张图片中的靶标图案区域提取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-精度值越小，代表算法输出结果中正确的匹配点对数占比越大，匹配精度越高，错误率越小，PR曲线越靠左。所以，算法输出的PR曲线越靠近左上方，即越靠近坐标点（0，1），代表算法的匹配性能越好。

从图8中可以看出，版本2的PR曲线结果比版本1更加靠近（0，1）点，版本2的曲线末端“召回率”比版本1高3.4%，1-精度（错误率）比版本1低15.6%。实验结果表明，设计的基于三维灰度质心方向一致性的候选匹配点筛选方法能够有效提升特征匹配算法的召回率和1-精度。

3 分步式光流投影匹配方法设计

在异构协同SLAM系统的前端中，针对鱼眼图像和RGBD图像之间因存在较大的视野和尺度差异而导致匹配困难的问题，提出一种分步式光流-投影匹配方法，以提升系统中异构图像特征匹配的性能。

在系统初始阶段时，利用暴力匹配方法对2个相机之间的特征点进行初始匹配，并利用MLPnP［39］方法求解2个相机之间的初始相对位姿。在后续的异构图像匹配过程中，利用求出的初始相对位姿信息，针对鱼眼图像设计的匹配过程分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相机上一帧图像所有深度信息有效的特征点，和鱼眼相机上一帧图像中关联的所有地图点投影到鱼眼相机当前帧图像中，并在投影点的邻域找到与之匹配的特征点。利用MLPnP［39］方法，对鱼眼相机当前帧图像进行再一次的位姿估计，最后通过重投影误差确定鱼眼相机当前帧图像和RGBD相机上一帧图像之间正确的匹配点对。

图10为鱼眼相机当前帧光流跟踪过程的示意图。在图10中：FL表示鱼眼相机上一帧图像；DL表示RGBD相机上一帧图像；FC表示鱼眼相机当前帧图像。假设根据历史匹配信息，图像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图像像素坐标的投影函数。

记点Pi在世界坐标系中的坐标为Pi，w，有

式中：Tw，DL代表从图像DL的相机坐标系到世界坐标系的坐标变换矩阵。

记图像FL中，第j个与三维地图点有匹配关系的特征点像素坐标为p′j，FL，其对应的地图点为P′j，在世界坐标系中的坐标为P′j，w。

使用金字塔Lucas-Kanade（LK）光流法，将点坐标集合{}和{p′j，FL}从图像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图像像素坐标的投影函数。

记图像FC中提取的特征点集合为{}，其中第k个特征点为。对于{}的每一个点，在{p″k，FC}中选出位于以为中心，半径为Δpth的圆形邻域内，并且灰度质心方向与在图像DL中的特征点pi，DL符合方向一致性约束的特征点作为候选匹配特征点，通过比较特征描述子的汉明距离，选出描述子距离最小，并且小于描述子距离阈值Dth的点作为正确匹配点，得到点对应的匹配点，即

式中：γ(pi，DL，p″ki，FC)表示特征点pi，DL和之间的三维灰度质心方向夹角表示像素点和之间的像素坐标欧氏距离；H(Dpi，DL，)表示特征点pi，DL和对应的二进制描述子和之间的汉明距离。

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

同理，对于点集{}的每一个点，在{}中找到匹配点，满足：

根据求取的位姿矩阵结果，计算中每一组匹配关系的重投影误差，并剔除外点，筛选出鱼眼图像当前帧和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像素坐标的投影函数。

记图像DC中提取的特征点集为{}。利用式（11）所示的投影匹配方法，寻找点集{}中的每一个点，在点集{}中对应的匹配点，有

在图像FC中，对于提取的特征点集{}的第k个特征点，记其关联的三维点为，其在世界坐标系中的坐标为。记在图像DC中的投影点记为，有

利用投影匹配方法，寻找每一个在中的匹配点，有

由于图像DC是RGBD图像，所以对于点集{}中的点，利用深度信息可以获得其对应三维点在图像DC相机坐标系中的坐标，记为。利用第3步中求得的图像DC相机位姿矩阵，对点向世界坐标系中进行反投影，得到其在世界坐标系中的坐标，有

将点向图像FC中进行投影，得到其在图像FC中的像素坐标，有

对于每一个投影点，在图像FC中的特征点集合{}中进行投影匹配，找到对应的匹配点，有

将点向图像DL中进行投影，得到其在图像DL中的像素坐标，有

式中：ΠDL表示从图像DL对应相机坐标系下的三维坐标，到图像DL像素坐标的投影函数。

对于每一个投影点，在图像DL中的特征点集合{pi，DL}中进行投影匹配，找到对应的匹配点，有

根据式（18）、式（20）、式（23）和式（25），可以得到4组匹配点对集合，根据这些匹配信息，对图像DC的相机位姿进行第二次估计，得到图像DC的相机位姿矩阵，有

根据求取的位姿矩阵结果，

提出的分步式光流-投影匹配方法在异构图像流中的迭代运行过程如图15所示。从图15中可以看出，在该运行过程中，RGBD视频流和鱼眼视频流中的同构图像和异构图像之间都有通过特征匹配建立的几何约束，进而实现SLAM过程中2个相机的异构协同。

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

4 异构协同SLAM系统的后端

在提出的异构协同SLAM算法中，2个相机的SLAM通道共用一个后端。如图16所示，定义与新关键帧关联的地图点为一级共视点集合{Point1}。所有与集合{Point1}中的任意点相关联的，除新关键帧以外的关键帧集合为一级共视帧集合{Frame1}。定义与集合{Frame1}中的任意帧相关联，且不在{Point1}集合中的地图点构成的集合为二级共视点集合{Point2}，与集合{Point2}中的任意点相关联，且不在集合{Frame1}中的关键帧构成的集合记为二级共视帧集合{Frame2}。采用滑窗BA优化的方法，将新关键帧对应的所有一级以内共视帧的位姿，和他们关联的三维地图点位置进行BA优化，而二级共视帧的位姿固定不变。

图16 新关键帧的共视帧和共视点关系示意图Fig.16 Common view frames and points of a new key frame

定义表示二级以内共视点集合中第i个待优化的地图点世界坐标，表示一级以内共视帧集合{}中第j个待优化关键帧的相机位姿矩阵表示二级共视帧集合{}中第k个关键帧的相机位姿矩阵。定义表示待优化三维地图点集合{}中，第i个地图点在待优化帧集合{}中第j帧匹配的特征点的广义像素坐标。表示待优化三维地图点集合{}中，第i个地图点在二级共视帧集合{}中第k帧匹配的特征点的广义像素坐标。构造如下所示代价函数：

通过优化求解代价函数（27），对集合{}和集合{}中所有元素的取值进行优化求解，实现定位地图的滑窗BA优化。

5 实验与分析

为了验证提出的异构协同SLAM系统的性能，通过3组实验从特征匹配性能、系统鲁棒性和相机轨迹精度方面，对设计的异构协同SLAM系统性能进行实验验证。

3组实验均在笔记本电脑上运行，其主频为2.30 GHz，处理器为Intel i7-11 800 H，操作系统为Ubuntu 20.04。

使用的RGBD相机是Intel RealSense D435相机，分辨率设置为640×480。RGBD相机采用Brown-Conrady相机模型，其横、纵焦距fx、fy分别为604.590、604.373；主点坐标cx、cy为321.359、237.346；畸变系数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；畸变系数k1、k2、k3、k4分别为-0.076 5、0.009 08、-0.011 7、0.003 73。

提出的异构协同SLAM系统的初始化流程如图17所示。

图17 异构协同SLAM系统初始化流程Fig.17 Initialization process of heterogeneous collaborative SLAM system

系统先采用ORBSLAM2-RGBD的初始化方法，完成SLAM系统RGBD通道的地图初始化。然后通过暴力匹配和MLPnP［39］方法，对RGBD和鱼眼图像序列不断进行特征点匹配和位姿估计。如果算法能够在某一对RGBD和鱼眼图像帧，计算出包含50组以上内点的相对相机位姿，则判定该鱼眼帧的相机位姿成功估计。此时RGBD相机和鱼眼相机的相对位姿已知，进而可以利用提出的异构图像分步式光流-投影匹配方法，对RGBD图像和鱼眼图像进行连续且稳定的特征点匹配和相对相机位姿估计。通过利用异构匹配算法估计出的一系列鱼眼相机位姿，以及鱼眼图像之间匹配的特征点对，对鱼眼相机视野中的特征点进行三角化，构造三维地图点，以实现鱼眼SLAM通道的初始化。

5.1 性能验证

本实验利用在不同真实场景中拍摄的鱼眼图像和RGBD图像视频流，对本文提出的匹配方法性能进行验证。在视频流拍摄过程中，选取花园、走廊、实验室、办公室等场景，共拍摄6组数据集，每一组数据集中，2个相机视频流的图像帧数相等。

在一个数据集中，若2个相机视频流的帧数均为n，算法对第i帧鱼眼图像和第i帧RGBD图像进行匹配，当i≠1时，算法还对第i帧鱼眼图像和第i-1帧RGBD图像进行匹配。所以，在帧数为n的视频流数据集中，算法总共的匹配次数为2n-1。

为了对提出的异构匹配算法性能进行分析比较，实验中采用暴力匹配方法、基于词袋模型（Bag of Words，BoW）的匹配法和快速最近邻逼近搜索函数库（FastApproximate Nearest Neighbor Search Library，FLANN）匹配法作为对比算法。对于暴力匹配法和FLANN匹配法，本实验使用OpenCV提供的函数。对于BoW匹配法，本实验采用ORBSLAM2中提供的函数。

对所有参与实验验证的匹配方法输出的匹配结果，均采用MLPnP［39］的方法对鱼眼和RGBD相机的相对位姿进行估计。然后借助位姿信息，通过将所有RGBD深度有效点对应的三维点投影到鱼眼图像中进行投影匹配，并通过重投影误差的大小判断每一组匹配点对是否是内点。实验中，当投影点和对应匹配点之间的像素距离误差＜3像素时，判定其为内点。如果内点数量≥50，则认为算法成功地实现了一次相对位姿估计。算法对异构相机之间的相对位姿估计成功率越高，代表其匹配性能越好。

本文算法的部分匹配结果示例图如图18所示。算法在每一组数据集中的匹配实验结果如表1和图19、图20所示，平均每帧匹配用时如表2所示。其中，在图20所示的条形分布图中，一个横条代表算法在一个数据集中，所进行的异构匹配与异构相机之间的相对位姿估计。绿色区域代表成功的相对位姿估计。红色区域代表失败的相对位姿估计。绿色区域占比越大，代表相对位姿估计的成功率越高，匹配性能越好。

表1 算法在不同数据集中的匹配成功次数和成功率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 算法在不同数据集中的匹配成功率柱状图Fig.19 Histogram of matching success rates in different datasets

图20 算法在不同数据集中的成功匹配与失败匹配的条形分布图Fig.20 Bar graph of successful matching and failed matching in different datasets

从图18中可以看出，提出的分步式光流-投影匹配方法在不同场景中，并且无论RGBD相机视野和鱼眼相机视野交叠区域位于鱼眼图像中央还是边缘，均能够找到较多的匹配点，具有较好的匹配性能。从图19、图20和表1中可以看出，在6个实验数据集中，本文算法对异构相机之间相对位姿估计的成功率均远高于暴力匹配、BoW和FLANN匹配方法，并始终在90%以上的成功率。表2中，本文算法的平均每帧用时为53.539 ms，计算速度慢于其他匹配算法，但仍然能够满足工程应用需求。实验结果表明，提出的分步式光流-投影匹配方法在异构相机视频流之间表现出突出的匹配性能和异构相机相对位姿估计性能。

5.2 各相机通道的性能验证

本实验通过考察异构协同SLAM的RGBD相机SLAM通道和鱼眼相机SLAM通道在不同条件下的地图构建完整度，来验证异构协同SLAM系统2个相机通道的性能。

5.2.1 鲁棒性验证

为了验证异构协同SLAM系统RGBD相机通道的鲁棒性，本实验利用相机在真实室内环境拍摄的视频序列对RGBD通道构建的点云地图的完整度进行评估。实验通过手持移动拍摄的方式在室内环境中采集视频序列，作为SLAM系统的输入。

本实验共拍摄5组视频序列。第1组是相机在纹理丰富的室内环境中正常移动拍摄的视频序列（总帧数为863），每一帧图像平均灰度梯度取值范围为2～6，每一帧图像的平均光流取值范围为0～50像素；第2组是相机在快速移动的条件下拍摄的视频序列（总帧数为457），该序列中存在32帧平均光流为80～120像素的图像帧，其中包含有20帧因为运动模糊导致平均灰度梯度＜2的图像帧；第3组的拍摄过程中，相机多次贴近环境中的物体，使得相机视野中出现纹理分布严重不均匀的现象，在总长为930帧的序列图像中，因为拍摄过程中出现相机前方物体与相机的距离＜1 m的情况，图像序列中存在连续15帧平均灰度梯度＜1的视频序列区段；第4组是相机在低帧率条件下拍摄的视频序列（总帧数为310），在图像序列中，存在55帧平均光流为80～120像素的图像帧；第5组是相机在一个白墙较多的纹理缺失环境中拍摄的视频序列，在总长为967帧的图像序列中，存在423帧平均灰度梯度＜1的图像帧。

实验选用ORBSLAM2-RGBD、OpenVSLAMRGBD作为对比算法。通过比较输出的点云地图的完整度，来验证RGBD-SLAM通道的系统鲁棒性。5组视频序列的部分示例图片如图21所示。SLAM算法的定位成功帧和定位失败帧在数据集中的条形分布图的如图22所示，构建的稠密场景点云地图如图23所示。

图21 室内不同拍摄条件下的数据集示例图片Fig.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可以看出，在本文的异构协同SLAM系统中，相比于RGBD-SLAM通道，鱼眼SLAM通道的定位失败帧占比最小，说明鱼眼SLAM通道的鲁棒性较高。相比于OpenVSLAM-RGBD和ORBSLAM2-RGBD，本文算法对应的RGBD-SLAM通道的定位成功帧在相机快速移动条件下、相机贴近景物条件下和低帧率条件下的实验结果中占比最高，在纹理信息不足的条件下占比高于ORBSLAM2-RGBD。

从图23可以看出，当相机在纹理丰富的场景中正常移动拍摄时，3个SLAM参评算法都能够利用视频流的图像信息构建出较为完整的地图。当存在相机快速移动、相机贴近环境物体、低帧率、纹理缺失的不利条件时，OpenVSLAMRGBD和ORBSLAM2-RGBD算法由于跟踪丢失，没有构建出视频流中拍摄到的全部场景，只构建出了部分场景的点云地图，而本文算法的RGBD-SLAM通道都能够构建出更为完整的地图。

5.2.2 轨迹漂移程度验证

本实验利用相机在真实室外环境拍摄的3组视频序列，来验证异构协同SLAM系统的轨迹漂移程度。在数据集拍摄的过程中，相机的轨迹首尾相接形成闭环。实验通过手持移动拍摄的方式采集视频序列，作为SLAM系统的输入。选用ORBSLAM3-Fisheye、OpenVSLAM-Fisheye、ORBSLAM2-RGBD、OpenVSLAM-RGBD作为对比算法。在关闭所有参评算法回环检测功能的前提下，对拍摄场景进行稀疏点云地图构建并输出相机轨迹。通过观察SLAM算法输出的相机轨迹首段和尾段之间的距离，以及地图不同区域的尺度差异，来判断SLAM过程中轨迹漂移的程度。拍摄的室外场景卫星图和对应的相机移动大致轨迹（白色带箭头线条），以及SLAM算法输出的相机轨迹和构建的稀疏点云地图如图24所示。

图24 室外条件下SLAM算法的相机轨迹和构建的场景点云地图Fig.24 Camera trajectories and point cloud maps constructed under different conditions

在图24（a）中，本文算法未发生尺度漂移，且姿态和轨迹漂移较小，相机轨迹首段和尾段几乎重合，而ORBSLAM3-Fisheye、OpenVSLAMFisheye的结果均发生了较大的尺度漂移，其中，ORBSLAM3-Fisheye的轨迹末段比例尺约缩减为其初段的0.015倍，OpenVSLAM-Fisheye的轨迹末段比例尺约缩减为其初段的0.007 5倍，ORBSLAM2-RGBD和OpenVSLAM-RGBD虽没有尺度漂移，但它们的结果发生了约20°～30°的相机姿态漂移和约5～10 m的轨迹漂移。在图24（b）中，本文算法未发生尺度和姿态漂移，但发生了约10 m的轨迹漂移，ORBSLAM3-Fisheye、OpenVSLAM-Fisheye的结果均发生了较大的尺度漂移，其中，ORBSLAM3-Fisheye的轨迹末段比例尺约缩减为其初段的0.01倍，OpenVSLAM-Fisheye的轨迹末段比例尺约缩减为其初段的0.067倍，ORBSLAM2-RGBD中途跟踪丢失，OpenVSLAM-RGBD的结果发生了约40°的相机姿态漂移和约40 m的轨迹漂移。在图24（c）中，本文算法未发生尺度漂移，但发生了约20°的相机姿态漂移和约20 m的轨迹漂移，OpenVSLAM-Fisheye中途跟踪丢失，ORBSLAM3-Fisheye的结果发生了较大的尺度漂移，其轨迹末段比例尺缩减为其初段的0.18倍，ORBSLAM2-RGBD中途跟踪丢失，OpenVSLAM-RGBD的结果发生了近180°的相机姿态漂移和约60 m的轨迹漂移。实验结果表明，在本文提出的异构协同SLAM系统中，单目鱼眼SLAM通道能够利用RGBD-SLAM通道赋予的深度信息有效抑制尺度漂移，RGBD-SLAM通道能够利用鱼眼SLAM通道大视野优势，抑制相机姿态漂移和轨迹漂移。

5.3 轨迹精度验证

为了验证异构协同SLAM系统的轨迹精度，本实验利用相机在真实室内环境拍摄的视频序列对算法输出的轨迹精度进行评测。实验采用手持移动拍摄的方式在室内环境中采集视频序列，作为SLAM系统的输入。由于2个相机为非时钟同步和非刚性连接的关系，2个相机的运动状态是相互独立的，所以本文算法对2个相机的轨迹同时进行估计。

为了对算法输出的相机轨迹精度进行评估，在每个相机的机身上安装定位球，通过Optitrack运动捕捉系统，对相机的真实运动轨迹进行精确测定和记录，并将该测定结果作为真值。

实验选用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组自行拍摄的数据集。SLAM算法的RMSE实验结果如图25所示。其中，8号数据集对应的相机运动轨迹估计结果如图26所示，轨迹误差曲线图如图27所示。

图25 轨迹误差柱状图Fig.25 Trajectory error histogram

图26 8号数据集中的相机轨迹Fig.26 Camera trajectories in Dataset 8

图27 SLAM算法在8号数据集中的轨迹误差曲线图Fig.27 Graph of trajectory errors of SLAM algorithm in Dataset 8

从图25中可以看出，本文算法输出的RGBD相机和鱼眼相机的轨迹精度均优于其他4种对比的SLAM算法，且在异构协同SLAM系统中，鱼眼相机的轨迹精度比RGBD相机的轨迹精度更高。由于ORBSLAM3-Fisheye和OpenVSLAM-Fisheye在2、3、4、6、7、8号数据集中发生了尺度漂移，因此其轨迹误差在这些数据集中远大于其他算法。从图26中可以看出，相比于其他算法，本文算法的相机轨迹能够更好地拟合相机的真实运动轨迹。从图27中可以看出，本文算法的轨迹误差曲线均位于其他算法误差曲线的下方。本节实验结果表明，本文设计的异构协同SLAM算法能够实现SLAM系统中鱼眼相机和RGBD相机的优势互补，提升了相机的轨迹精度。

6 结论

本文设计了基于鱼眼相机和RGBD相机的异构协同SLAM系统。首先基于之前的工作FSD-BRIEF设计了基于三维灰度质心方向一致性的候选匹配点筛选方法。为了提高异构图像匹配的性能，结合MLPnP算法，设计异构图像之间的分步式光流-投影匹配方法，实现鱼眼相机和RGBD相机之间的同时相对位姿估计和特征点匹配。最后基于ORBSLAM2框架，提出鱼眼相机和RGBD相机异构协同SLAM系统的框架。实验结果表明：

1）相比于传统匹配方法0%～75%的成功率，本文提出的分步式光流-投影匹配方法能够较大地提升异构图像特征匹配的性能，可将异构相机相对位姿计算的成功率提升至90%以上。

2）相比于其他单目鱼眼SLAM和RGBDSLAM算法，本文提出的异构协同SLAM系统能够较大地提升RGBD相机SLAM通道的鲁棒性、抗轨迹漂移能力和鱼眼SLAM通道的抗尺度漂移能力，使RGBD-SLAM通道能够克服相机快速移动、贴近景物、低帧率、纹理缺失等不利条件，在稠密地图构建完整度上有50%以上的提升。同时，异构协同SLAM系统在室外大场景的数据集中，可有效降低鱼眼SLAM通道尺度漂移和RGBD-SLAM通道的相机姿态漂移、轨迹漂移现象。

3）相比于传统的RGBD-SLAM和单目鱼眼SLAM系统，本文提出的异构协同SLAM系统的轨迹精度有较大提升，如在5.3节中的8号数据集中，相比于其他SLAM算法，相机轨迹的均方根误差减少了66%。

综上，实验结果证明了本文提出的异构协同SLAM系统能够有效地结合鱼眼相机和RGBD相机的优势，较大地提升了系统的鲁棒性和轨迹精度。