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

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

2、可避免3。动态对象会给相机位姿估计带来误差4。因此，如何解决动态环境中动态点带来误差的问题成为视觉SLAM的关键。全景相机具有大视场的特性，即使面对快速运动的情况，依然可以获得帧之间的对应关系5。近年来，诸多学者使用大视场相机进行SLAM。Matsukih等将鱼眼相机用于DSO6（Direct Sparse Odometry），证明了大视场相机用于DSO算法效果较好7。为了精确估计相机位置，文章8-10采用鲁棒特征和传统的扩展卡尔曼滤波（EKF）-SLAM方法，但这些方法的处理速度太低，并且缺乏有效的反馈定位方法。Lukierski等人提出一种密集3D重建算法，不依赖关键点，能够更好地采集强梯

3、度区域图像的像素，用来估计相机位置。文献11-12基于全景相机使用稀疏视觉上的方法进行视觉里程计研究，更好地定位相机。针对动态环境，Li等13提出了实时深度边缘的RGB-D SLAM系统。该系统计算每个关键帧中点是静态点的可能性，使用静态加权方法降低动态物体对相机位姿估计的影响。但是，该系统需要依赖RGB-D相机提供的深度信息，这对于普通相机是不适用的。文献14-15利用深度学习对动态目标的检测，达到了去除动态特征点，提高位姿估计精度的目的。上述工作都只能利用相邻两帧之间的信息来判断场景中对象是动态还是静态的。为了利用更多图像帧的信息，更加准确地判断场景中对象的运动状态，本文提出基于逆深度滤波

4、的双目折反射全景相机动态SLAM系统，以去除动态点为目标，利用静态点对相机进行位姿估计。使用双目全景相机通过立体视觉计算的距离信息对视觉里程计进行补偿推断，采用一种基于贝叶斯滤波的逆深度滤波器，对动态地图点进行剔除，在动态环境下得到更加精确的相机位姿估计结果。此方法可以解决传统SLAM系统面对动态环境时计算轨迹出现漂移的现象，并且计算的位姿结果也更加精确。2 双目折反射全景相机系统2.1折反射全景相机投影模型折反射全景相机通过顶端反射镜扩大相机的视场，由反射镜和成像模组两部分组成。其结构如图1所示。图1折反射全景相机Fig.1Catadioptric panoramic camera为了表示世

5、界坐标系下点到像素坐标系下的投影过程，应用球面投影模型对折反射全景相机的投影模型进行分析16。球面投影模型的成像过程如图2所示。图2折反射全景相机投影过程Fig.2Catadioptric panoramic camera projection process首先，将世界坐标系下的空间点Xc以球心为投影中心投影到单位球上于点Xs，坐标为：Xs=xs,ys,zsT=XcXc（1）折反射全景相机反射镜的镜面参数由来表示。将投影中心由球心移动到Cp=(0,0,)T，以Cp为投影中心将单位球上的三维点Xs投影到与Cp距离为1的二维平面上对应点Xm，对应关系如下：Xm=D(Xs)=xs+zs,ys+zs

6、,1T（2）最后，经过广义投影矩阵K，完成图像坐标系到像素坐标系之间的变换，关系如下：p=Kxm=fx00fxsfy0u0v01xm（3）上述推导公式表示世界坐标系下的点在全景相机下的成像过程，很容易得到像素点到空间点的反投影过程。2.2双目全景相机三角测量双目折反射全景相机系统可以利用双目图像计算特征点的深度信息。将深度信息输入逆深度滤波器，对动态地图点进行剔除。这里通过三角化来计算特征点深度，针对折反射全景相机的投影模型，将特征点反投影到球面，在球面上分析三角化过程。如图3所示，空间点P同时被双目折反射全景相机观测到，pl和pr分别是左右两个折反射相机上对应的特征点反投影到球面上的向量。其

7、中利用两个几何关系相对固定的相机以不同视角获取被测物体的影像信息，再利用同一个视觉信息在不同相机下的匹配特征不同进行双目相机标定17-18。两个相机间的位姿关系通过标定是已知的，根据对极几何的定义，pl和pr满足以下关系：srpr=slRpl+t（4）图3双目折反射全景相机三角化Fig. 3Triangulation of binocular catadioptric panoramic camera在式（4）的两侧同时乘一个pr，得到：srprpr=0=slprRpl+prt（5）通过方程可以求得sl。于是本文计算了特征点的深度信息，可以确定其在空间中的坐标。将深度信息输入到逆深度滤波器模块

8、可以区分出地图点的类别。3 逆深度滤波关键算法深度滤波器采用概率的方式对空间点进行深度估计，随着新的图像帧的进入，对深度的后验概率分布进行更新。Forster等19发现在大规模场景下逆深度的统计直方图更接近高斯均匀混合分布，因此，提出了逆深度滤波器的方法，就是使用空间点深度值的倒数代替深度值在深度滤波器中对地图点进行处理。逆深度滤波器将最新时刻求得的逆深度观测值，与上一时刻的逆深度估计值进行融合，直到地图点的逆深度值收敛。使用逆深度滤波器可以将地图点分为有效的和无效的测量值两种类别。虽然文献20中模型最初是用来找到不同基线下最准确的逆深度测量值的，但是该模型也适合检测动态环境下双目折反射全景相

9、机SLAM系统中的动态地图点，因此将逆深度滤波器应用到双目折反射全景相机的动态SLAM系统中，使用逆深度滤波器来区分地图点中的动态点和静态点。式（6）给出了逆深度滤波器的概率模型：p(x|Z,)=N(x|Z,2)+(1)U(x|Zmin,Zmax)（6）针对双目折反射全景相机的动态SLAM系统，本文对式（6）中各符号进行了重新定义。使用代表计算出的地图点是静态点的概率；对应的1代表的是该点是动态点的概率。N(x|Z,2)是指静态点是服从Z为均值，2为方差的高斯分布，其中x代表由双目全景相机计算得到的逆深度值，Z代表真实的逆深度值，2代表一个像素对逆深度的扰动；U(x|Zmin,Zmax)是指动

10、态点在(Zmin,Zmax)内服从均匀分布，可以由场景中几何的先验得到Zmin和Zmax。对于一个地图点，可以随着新的图像帧的进入，不断更新对概率的估计，直到它收敛，以此来区分动态地图点和静态地图点。如图4所示，基于ORB-SLAM2的框架，添加了逆深度滤波线程。使用双目折反射全景相机对空间点的深度进行测量，然后转换成逆深度。将得到的地图点输入逆深度滤波器，随着观测次数的增加，地图点的逆深度值逐渐收敛。对于收敛的地图点，判断它是静态点的概率是否大于阈值，区分出动态地图点和静态地图点。然后剔除动态地图点，只有静态地图点被输入到跟踪模块用于相机的位姿估计，以此达到更加鲁棒和精确的相机位姿计算。图4

11、双目折反射全景相机动态SLAM算法框图Fig. 4Block diagram of dynamic SLAM algorithm for binocular panoramic camera现假设有一组通过双目折反射全景相机测量得到的含有噪声的地图点的逆深度值x1,x2,xN，它们之间是相互独立的，现在要估计真实的逆深度值Z和概率，可以通过求最大后验概率得到。根据贝叶斯公式，其后验具有以下形式：p(Z,|x1,x2,xN)=p(Z,)p(x1,x2,xN|Z,)p(x1,x2,xN)p(Z,)p(x1,x2,xN|Z,).（7）由于深度测量值相互独立，因此式（7）右边可写成：p(Z,)p(x1

12、,x2,xN|Z,)p(Z,)np(xn|Z,).（8）真实后验概率可以用贝塔-高斯分布来近似18，如下：q(Z,|aN,bN,N,N)=Beta(|aN,bN)N(Z|N,N),（9）其中aN和bN是Beta分布的参数。为了减少计算量，继续推导公式的迭代形式是必要的。通过推导可以得到迭代形式：q(Z,|aN,bN,N,N)q(Z,|aN1,bN1,N1,N1)p(xN|Z,).（10）通过匹配Z和的第一阶和第二阶矩，用BetaGaussian分布近似式（10）的后验，导出了aN,bN,N和2N的更新公式。推导过程参考文献21。计算q(Z,|aN,bN,N,N)的一阶矩，用来近似估计和Z：Z=

13、N（11）=aNaN+bN（12）因此，每当有新的一帧图片传入系统，本文使用式（11）对逆深度Z进行更新，使用式（12）对该地图点是静态点的概率进行更新。对设定一个阈值，若地图点收敛后大于该阈值，则认为该地图点为静态点；否则剔除该地图点，不参与折反射全景相机的位姿解算。图5是逆深度滤波的具体实现示意图。首先，通过双目折反射全景相机初始化出一组地图点，随着新的双目图像输入系统，生成新地图点的观测并对已存在的地图点生成重复观测，不断对地图点的深度和概率更新。地图点被分为已收敛的地图点和未收敛的地图点，未收敛的地图点继续更新直到收敛。已经收敛的地图点是静态点的概率大于等于阈值时被判断是静态点，参与全

14、景相机的位姿估计；概率小于阈值的是动态点，被剔除。图5逆深度滤波算法示意图Fig.5Schematic diagram of inverse depth filtering algorithm4 实验与分析为了验证基于逆深度滤波的双目折反射全景相机动态SLAM系统的性能，在公开数据集22上进行实验。该数据集由装载2个折反射全景相机的汽车在城市街道中采集，共包含12 607帧图像。图像中有大量的动态物体和静态物体，非常适合对动态环境下折反射全景相机的SLAM系统性能进行测试。在数据集中选取多段包含动态对象的图像序列，并分为低动态序列和高动态序列。选取了一段低动态和两段高动态的图像序列进行实验，对

15、本文提出的SLAM算法进行验证，对SLAM系统分别评估了绝对轨迹误差（Absolute trajectory Error， ATE）和相对轨迹误差（Relative Pose Error， RPE）两个指标。ATE是估计轨迹与ground truth之间的误差，它可以直接反映出位姿估计算法的精度和全局一致性，通常用于评估整个SLAM系统的性能。RPE是评价系统漂移的指标，它是固定时间段内两个位姿变化量的差。考虑ATE和RPE在平移上的误差，其均方根误差（Root Mean Square Error，RMSE）的计算如下：Eatrans=1Ni=1Ntrans(T1gt,iTesti,i)22（

16、13）Ertrans=1Nti=1Nttrans(T1gt,iTgt,i+t)1(T1esti,iTesti,i+t)22（14）其中：N表示轨迹中包含的位姿个数，i=1,2,N；Tgt,i和Testi,i分别表示真实轨迹和SLAM系统估计出的轨迹；trans是取括号内变量的平移部分。本文使用RMSE、平均误差、中值误差和标准差（Standard Deviation， SD）评价ATE指标。将使用本文方法计算出的轨迹分别与单目全景视觉里程计方法及真值进行对比。其中，单目全视觉里程计（Visual Odometry， VO）是基于ORB-SLAM2改进的适合折反射全景相机的算法，把计算出的RMS

17、E、平均误差、中值误差和SD进行统计，结果如表1所示。使用相对位移评价RPE指标，图6是本文计算出的轨迹与真值之间的误差，其中6（a）是低动态序列，6（b）和6（c）为高动态序列。分析图6可知，本文计算出的轨迹与真值的误差一直在较小的范围内变化，没有因为动态对象而出现轨迹的突变和漂移，证明了本文算法的鲁棒性。表1不同方法在公共数据集测试的ATE结果Tab.1Comparison of absolute trajectory error （ATE） on public dataset(m)SequenceCatadioptric Pano VOOursRMSEMedianMeanStdRMSEM

18、edianMeanStdLow dynamic sequence（a）0.350.330.310.120.290.280.250.09High dynamic sequence（b）12.639.6910.674.110.520.510.470.16High dynamic sequence（c）15.3512.1412.135.290.560.520.530.19图6不同序列位姿估计结果与真值间的RPEFig.6RPE between pose estimation results and ground truth for different sequences图7中是不同方法得到的轨迹与真值的对比。本文计算出的轨迹与真值更加接近，而基于ORB-SLAM2改进的折反射全景VO23在低动态环境下与本文计算的轨迹最接近，因为在低动态环境下RANSAC算法将动态点作为外点，受到动态点的影响小。而在另外两个序列中存在高动态目标，此时RA