基于逆深度滤波的双目折反射全景相机动态SLAM系统

在同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）理论不断突破与发展的基础上，移动机器人等技术的研究水平显著提高。视觉传感器的引入拓宽了SLAM的应用。视觉SLAM通过图像序列来估计相机运动，根据图像特征的提取方法主要分为基于特征点的视觉SLAM方法和直接SLAM法两种^［1-2］。双目立体视觉可以很容易在各种场景中提取视差，从而得到场景中的深度信息。双目SLAM既可以在运动时估计深度，亦可在静止时估计，对周围的物体和自身移动的轨迹的感知更加精确。但是现实中不存在理想静态环境，移动对象的存在不可避免^［3］。动态对象会给相机位姿估计带来误差^［4］。因此，如何解决动态环境中动态点带来误差的问题成为视觉SLAM的关键。

全景相机具有大视场的特性，即使面对快速运动的情况，依然可以获得帧之间的对应关系^［5］。近年来，诸多学者使用大视场相机进行SLAM。Matsukih等将鱼眼相机用于DSO^［6］（Direct Sparse Odometry），证明了大视场相机用于DSO算法效果较好^［7］。为了精确估计相机位置，文章［8-10］采用鲁棒特征和传统的扩展卡尔曼滤波（EKF）-SLAM方法，但这些方法的处理速度太低，并且缺乏有效的反馈定位方法。Lukierski等人提出一种密集3D重建算法，不依赖关键点，能够更好地采集强梯度区域图像的像素，用来估计相机位置。文献［11-12］基于全景相机使用稀疏视觉上的方法进行视觉里程计研究，更好地定位相机。

针对动态环境，Li等^［13］提出了实时深度边缘的RGB-D SLAM系统。该系统计算每个关键帧中点是静态点的可能性，使用静态加权方法降低动态物体对相机位姿估计的影响。但是，该系统需要依赖RGB-D相机提供的深度信息，这对于普通相机是不适用的。文献［14-15］利用深度学习对动态目标的检测，达到了去除动态特征点，提高位姿估计精度的目的。

上述工作都只能利用相邻两帧之间的信息来判断场景中对象是动态还是静态的。为了利用更多图像帧的信息，更加准确地判断场景中对象的运动状态，本文提出基于逆深度滤波的双目折反射全景相机动态SLAM系统，以去除动态点为目标，利用静态点对相机进行位姿估计。使用双目全景相机通过立体视觉计算的距离信息对视觉里程计进行补偿推断，采用一种基于贝叶斯滤波的逆深度滤波器，对动态地图点进行剔除，在动态环境下得到更加精确的相机位姿估计结果。此方法可以解决传统SLAM系统面对动态环境时计算轨迹出现漂移的现象，并且计算的位姿结果也更加精确。

深度滤波器采用概率的方式对空间点进行深度估计，随着新的图像帧的进入，对深度的后验概率分布进行更新。Forster等^［19］发现在大规模场景下逆深度的统计直方图更接近高斯均匀混合分布，因此，提出了逆深度滤波器的方法，就是使用空间点深度值的倒数代替深度值在深度滤波器中对地图点进行处理。逆深度滤波器将最新时刻求得的逆深度观测值，与上一时刻的逆深度估计值进行融合，直到地图点的逆深度值收敛。使用逆深度滤波器可以将地图点分为有效的和无效的测量值两种类别。

虽然文献［20］中模型最初是用来找到不同基线下最准确的逆深度测量值的，但是该模型也适合检测动态环境下双目折反射全景相机SLAM系统中的动态地图点，因此将逆深度滤波器应用到双目折反射全景相机的动态SLAM系统中，使用逆深度滤波器来区分地图点中的动态点和静态点。式（6）给出了逆深度滤波器的概率模型：

针对双目折反射全景相机的动态SLAM系统，本文对式（6）中各符号进行了重新定义。使用π代表计算出的地图点是静态点的概率；对应的1−π代表的是该点是动态点的概率。N(x|Z,τ2)是指静态点是服从Z为均值，τ2为方差的高斯分布，其中x代表由双目全景相机计算得到的逆深度值，Z代表真实的逆深度值，τ2代表一个像素对逆深度的扰动；U(x|Zmin,Zmax)是指动态点在(Zmin,Zmax)内服从均匀分布，可以由场景中几何的先验得到Zmin和Zmax。对于一个地图点，可以随着新的图像帧的进入，不断更新对概率π的估计，直到它收敛，以此来区分动态地图点和静态地图点。

如图4所示，基于ORB-SLAM2的框架，添加了逆深度滤波线程。使用双目折反射全景相机对空间点的深度进行测量，然后转换成逆深度。将得到的地图点输入逆深度滤波器，随着观测次数的增加，地图点的逆深度值逐渐收敛。对于收敛的地图点，判断它是静态点的概率π是否大于阈值，区分出动态地图点和静态地图点。然后剔除动态地图点，只有静态地图点被输入到跟踪模块用于相机的位姿估计，以此达到更加鲁棒和精确的相机位姿计算。

图4  双目折反射全景相机动态SLAM算法框图

Fig. 4  Block diagram of dynamic SLAM algorithm for binocular panoramic camera

现假设有一组通过双目折反射全景相机测量得到的含有噪声的地图点的逆深度值x1,x2,⋯xN，它们之间是相互独立的，现在要估计真实的逆深度值Z和概率π，可以通过求最大后验概率得到。根据贝叶斯公式，其后验具有以下形式：

由于深度测量值相互独立，因此式（7）右边可写成：

真实后验概率可以用贝塔-高斯分布来近似^［18］，如下：

其中aN和bN是Beta分布的参数。为了减少计算量，继续推导公式的迭代形式是必要的。通过推导可以得到迭代形式：

通过匹配Z和π的第一阶和第二阶矩，用Beta×Gaussian分布近似式（10）的后验，导出了aN,bN,μN和σ2N的更新公式。推导过程参考文献［21］。计算q(Z,π|aN,bN,μN,σN)的一阶矩，用来近似估计π和Z：

因此，每当有新的一帧图片传入系统，本文使用式（11）对逆深度Z进行更新，使用式（12）对该地图点是静态点的概率π进行更新。对π设定一个阈值，若地图点收敛后π大于该阈值，则认为该地图点为静态点；否则剔除该地图点，不参与折反射全景相机的位姿解算。

图5是逆深度滤波的具体实现示意图。首先，通过双目折反射全景相机初始化出一组地图点，随着新的双目图像输入系统，生成新地图点的观测并对已存在的地图点生成重复观测，不断对地图点的深度和概率π更新。地图点被分为已收敛的地图点和未收敛的地图点，未收敛的地图点继续更新直到收敛。已经收敛的地图点是静态点的概率π大于等于阈值时被判断是静态点，参与全景相机的位姿估计；概率π小于阈值的是动态点，被剔除。

图5  逆深度滤波算法示意图

Fig.5  Schematic diagram of inverse depth filtering algorithm

为了验证基于逆深度滤波的双目折反射全景相机动态SLAM系统的性能，在公开数据集^［22］上进行实验。该数据集由装载2个折反射全景相机的汽车在城市街道中采集，共包含12 607帧图像。图像中有大量的动态物体和静态物体，非常适合对动态环境下折反射全景相机的SLAM系统性能进行测试。

在数据集中选取多段包含动态对象的图像序列，并分为低动态序列和高动态序列。选取了一段低动态和两段高动态的图像序列进行实验，对本文提出的SLAM算法进行验证，对SLAM系统分别评估了绝对轨迹误差（Absolute trajectory Error， ATE）和相对轨迹误差（Relative Pose Error， RPE）两个指标。ATE是估计轨迹与ground truth之间的误差，它可以直接反映出位姿估计算法的精度和全局一致性，通常用于评估整个SLAM系统的性能。RPE是评价系统漂移的指标，它是固定时间段内两个位姿变化量的差。考虑ATE和RPE在平移上的误差，其均方根误差（Root Mean Square Error，RMSE）的计算如下：

其中：N表示轨迹中包含的位姿个数，i=1,2,⋯,N；Tgt,i和Testi,i分别表示真实轨迹和SLAM系统估计出的轨迹；trans是取括号内变量的平移部分。

本文使用RMSE、平均误差、中值误差和标准差（Standard Deviation， SD）评价ATE指标。将使用本文方法计算出的轨迹分别与单目全景视觉里程计方法及真值进行对比。其中，单目全视觉里程计（Visual Odometry， VO）是基于ORB-SLAM2改进的适合折反射全景相机的算法，把计算出的RMSE、平均误差、中值误差和SD进行统计，结果如表1所示。使用相对位移评价RPE指标，图6是本文计算出的轨迹与真值之间的误差，其中6（a）是低动态序列，6（b）和6（c）为高动态序列。分析图6可知，本文计算出的轨迹与真值的误差一直在较小的范围内变化，没有因为动态对象而出现轨迹的突变和漂移，证明了本文算法的鲁棒性。

图6  不同序列位姿估计结果与真值间的RPE

Fig.6  RPE between pose estimation results and ground truth for different sequences

图7中是不同方法得到的轨迹与真值的对比。本文计算出的轨迹与真值更加接近，而基于ORB-SLAM2改进的折反射全景VO^［23］在低动态环境下与本文计算的轨迹最接近，因为在低动态环境下RANSAC算法将动态点作为外点，受到动态点的影响小。而在另外两个序列中存在高动态目标，此时RANSAC算法失效，本文的系统可以有效去除动态地图点并准确估计出相机位姿。

图7  不同方法得到的轨迹与真值的对比结果

Fig.7  Comparison of trajectories calculated by different methods with ground truth

分析表1中的数据，本文的系统在3个序列中的表现都比折反射全景VO要好，对比ATE 的RMSE，在3个序列上本文系统比折反射全景VO分别提高了17.14%，95.88%，96.35%。在面对复杂的动态环境时，本文系统能够有效地去除动态地图点，位姿估计精度相比传统方法有大幅的提升。

为了在复杂的动态环境下能够准确地估计折反射全景相机的位姿，本文提出了基于逆深度滤波的双目折反射全景相机动态SLAM系统，基于ORB-SLAM2的框架设计了双目折反射全景相机的SLAM系统，并在跟踪模块和局部建图模块中间添加了基于贝叶斯滤波的逆深度滤波器模块，对折反射全景相机求出的地图点进行状态估计，剔除动态地图点，只使用收敛的静态地图点对折反射全景相机进行位姿估计，在动态环境下得到了更加精确的相机位姿估计结果。该系统在3个序列中的绝对轨迹误差 的RMSE，比折反射全景VO分别提高了17.14%，95.88%和96.35%。在面对动态环境时，当传统的SLAM系统计算的轨迹出现漂移，基于逆深度滤波的双目折反射全景相机动态SLAM系统始终运行稳定，具有更强的鲁棒性。