基于深度预测的单目SLAM绝对尺度估计

张建博，袁 亮，何 丽，冉 腾，唐鼎新

(新疆大学 机械工程学院，新疆 乌鲁木齐 830047)

0 引 言

由于单目相机具有成本低、适用范围广和校准过程简单等优势，使得单目视觉同时定位与地图构建(simulta-neous localization and mapping，SLAM)成为机器人在未知环境中自主定位的一个重要研究方向。但是单目存在尺度不确定性的缺点，无法通过单帧图像得到场景中真实的深度信息，使单目SLAM出现尺度模糊的问题，因此需要一种单目的绝对尺度估计方法来恢复单目的绝对尺度。Ji Zhang等[1]使用激光雷达来辅助单目恢复场景的真实尺度；文献[2-4]利用惯性测量单元(inertial measurement unit，IMU)辅助单目恢复绝对尺度；Shiyu Song等[5]提出利用物体先验尺寸来恢复单目尺度；文献[6]利用相机的先验高度信息获得单目绝对尺度；Sen Wang等提出的DeepVO[7]和陈宗海等提出的LSTMVO[8]是一种端到端的深度学习框架，通过输入离线的视频序列可以同时输出深度图和相机位姿。

此外，实现精确的自主定位是SLAM最重要的任务之一，而单目SLAM的尺度不确定性会引起尺度漂移的问题，从而严重影响了定位精度。Engel J等提出基于直接法的DSO[9]利用光束法平差(bundle adjustment，BA)减少尺度漂移。Mur-Artal等提出基于特征点的ORB-SLAM[10]，利用局部BA和全局回环检测方法减少尺度漂移。

本文利用MonoDepth[11]深度预测网络估计单目的绝对尺度，以此来弥补单目固有的尺度模糊的缺点。利用绝对尺度实时校正尺度漂移，从而提高单目SLAM的定位精度。

1 单目视觉SLAM

单目视觉SLAM中一般采用针孔相机模型，其成像原理如图1所示，空间中一点投影在2维相机成像平面上，得到一个2维像素点P，但仅通过成像平面上的一个像素点P无法确定出该空间点的具体位置，因为从相机光心过归一化平面整条射线上的所有点都可以投影到该像素点，所以当确定了点P的真实深度时才能得到它的空间位置。

图1 单目尺度不确定性原理

传统单目视觉SLAM是通过三角化过程来恢复像素点相对深度的。首先对连续两帧二维图像提取特征点，再根据图像特征进行特征点匹配，最后利用对级约束关系恢复出两帧图像之间相机的运动，求解原理如图2所示，单目相机获取的相邻帧图像序列I1，I2经过特征提取和匹配，得到匹配好的其中一对特征点p1,p2，分别对应空间中一点P=[X,Y,Z]T在I1，I2上的投影，在针孔相机模型中有以下空间几何关系

图2 单目相机运动求解原理

s1p1=KP，s2p2=K(RP+t)

(1)

式中：K为单目相机的内参矩阵，R、t为两帧图像的相机位姿变换，s1、s2为尺度变换因子。

齐次坐标下，取x1、x2为两个像素点p1、p2在归一化坐标平面上的点，则有

x1=K-1p1，x2=K-1p2

(2)

(3)

(4)

对极约束中包括了旋转和平移，其中

(5)

求解相对位姿变化R、t需要求解本质矩阵E。

由于对级约束是等式为0的约束，使得E具有尺度等价性，由E分解得到的R、t也具有尺度等价性。对匹配点的归一化坐标x1=[u1,v1,1]、x2=[u2,v2,1],其对级约束关系为

(6)

中间项e1到e9组成本质矩阵E，对E进行SVD奇异值分解，恢复出相机的运动R、t，即得到了两帧图像I1、I2之间的位姿坐标变化T

(7)

对同一空间点P，求解方程如下

s1x1=s2Rx2+t

(8)

s2R2x2+t=0

(9)

解得s1即可得到p1点相对深度z，最终得到相机坐标系下估计的空间点P的相对坐标P=(x,y,z)T，但由于无法确定求解的平移量t与对应世界坐标系下的实际移动距离的比例，也即t与实际位移相差一个比例因子。由于尺度模糊，相机位姿估计缺乏绝对尺度的校正，以及相机的观测噪声和特征点的误匹配等其它因素的影响，最终会导致更严重的尺度漂移问题，给相机位姿估计带来随时间逐步增大的累积误差，并最终使处于未知环境中的机器人定位精度严重下降。

2 基于深度预测的绝对尺度估计

针对上述的问题，本文研究了单目SLAM的位姿估计过程，提出利用MonoDepth深度预测网络获取单目图像的绝对深度信息，来估计单目SLAM的绝对尺度，并通过特征点选取算法筛选含有可靠深度值的ORB特征点，最终用真实深度值进行相机的位姿估计与优化。

2.1 深度预测

本文采用深度预测网络MonoDepth进行单目图像的深度估计。MonoDepth是一个端到端的单目无监督深度估计网络，网络是全卷积架构，主要包括两部分：编码器和解码器。MonoDepth深度预测网络不需要深度图的真值来训练，而是以双目图像的左图作为输入，经过卷积神经网络，生成左图的预测视差图，并加入仅在训练中使用的双目摄像机的右图，用左右视图一致性损失函数来增强左右视差图的一致性，从而使结果更准确，最终得到左图的深度预测图。MonoDepth网络总的损失函数如下

(10)

式中：上下标l和r分别表示深度预测网络训练时所用的双目相机的左图和右图，α为各部分权重，C表示总的损失函数，它包括3部分：外观匹配损失Cap、视差平滑损失Cds和左右视差一致性损失Clr。

经过深度预测网络得到图像中各像素的双目视差值。根据实际需求，本文调整了MonoDepth网络的输入和输出部分，在输入部分将网络调整为可以适应KITTI数据集的输入图片大小，而不受原始网络本身只能输入固定尺寸的限制，并通过批量预测以提高效率；而在网络的输出部分，原始网络输出为双目视差图，而为得到可以直接用于位姿估计的真实深度值，我们在网络的后处理阶段通过如下公式计算得到对应像素的真实深度值

(11)

式中：d表示预测的深度值，disp表示网络预测的视差值。网络的训练需要通过双目相机获取的双目图像，b表示双目相机的基线长度。f表示双目相机的焦距。

2.2 特征点选取

利用MonoDepth深度预测网络获得的深度值来恢复二维图像中特征点的真实三维坐标，但并不是所有深度值都会用作估计。由于只有距离较近的点对位姿估计起较大作用，而距离较远的特征点在相邻帧中位置变化非常小，如在室外场景下，天空与前方树木交界处或道路尽头等距离远但像素梯度变化大的地方检测到的特征点。在SLAM中很难利用这些特征点进行相机的位姿估计，但这些特征点仍进行了大量重复的计算，这不但占用了较多的计算资源，还会带来更大的噪声和误差，故需要剔除距离较远的特征点。

本文采用的策略是判断深度值是否为可靠深度，将从图像中提取的ORB特征点分为近点和远点，实验中，设置近点深度阈值为20 m，将超出阈值的深度作为不可靠深度，然后剔除与不可靠深度值关联的特征点，仅保留近点的特征点用于进行后续的特征点匹配以及相机位姿估计，此处描述的特征点选取算法的伪代码可以表示为如算法1所示：

算法1: 特征点选取算法 输入: 单目图像I 设定深度阈值th. 输出: 经过筛选可以用于位姿估计的ORB特征点 (1)对单目图像提取所有特征点KPS (2) I经过MonoDepth网络预测各像素点视差值disp (3) for 第i个ORB特征点: (4) 根据式(11)计算第i个特征点对应深度值di (5) if (th

2.3 绝对尺度估计

由于对每一帧图像都进行了深度估计，每个像素点都具有真实的尺度信息，故在初始化阶段，不用像传统单目SLAM算法通过三角化过程才能得到缺乏绝对尺度的相对位姿，而是从初始帧即可获得空间中点的真实3维位置信息。

首先提取图像中ORB特征点，ORB特征点包含了Oriented FAST关键点和BRIEF描述子，具有检测速度快、鲁棒性好的优点。由于我们在初始帧即可获得真实深度，故通过2.1节获得的真实深度信息以及ORB特征点的像素位置索引，对ORB特征点进行绝对深度值的数据关联，即可以直接获得空间点P的真实深度d，根据给定ORB特征点的像素坐标(u,v)和得到的真实深度估计值d，计算该像素点在相机坐标系上的世界坐标(X,Y,Z)

(12)

式中：fx、fy是相机的焦距，cx、cy是图像原点相对于光心成像点的偏移量。

通过上述计算便得到了初始帧图像中特征点的真实尺度信息，然后在提取初始帧中ORB特征点的过程中，通过执行特征点选取算法，剔除与不可靠深度值进行数据关联的ORB特征点，最后利用保留的特征点进行特征点匹配和相机位姿估计。

2.4 特征点匹配与位姿估计

根据真实深度值估计出初始帧中特征点的绝对尺度，继续对第二帧图像同样进行ORB特征点的提取，然后通过特征点选取算法剔除含有不可靠深度值的特征点，将保留的含有可靠深度值的ORB特征点与第一帧图像上的特征点进行特征点匹配，传统的暴力匹配算法计算效率低，本文采用计算速度快、而且更加适合大量特征点匹配的FLANN算法进行特征点匹配。

当特征点匹配成功后，我们根据得到的3D-2D特征点对(其中3D点是指第一帧相机坐标系下已知3D位置的空间点，而2D点是指第二帧图像中与第一帧图像特征点匹配的2D像素点)，利用EPNP算法计算同一空间点P在第二帧相机坐标系下的3D空间点位置，这样便获得了连续两帧图像各相机坐标系下同一空间点的3D位置，最后根据ICP算法对连续帧间相机的位姿T进行估计。

由于观测点噪声的存在，空间点P在第二帧相机坐标系下的2D投影点与通过特征点匹配获得的p2存在误差(也称为重投影误差)，解决办法是继续构建非线性最小二乘BA公式，同时优化相机位姿和空间点位置，使得系统整体的重投影误差最小，以此来进一步降低相机位姿估计的误差，SLAM系统中非线性最小二乘的BA公式如下

(13)

同理，后续的相机位姿同样根据已知绝对尺度的3D-2D点对进行求解，用式(13)来优化相机位姿和空间点位置，并通过真实的深度值实时校正尺度漂移。

本文算法流程如图3所示。

图3 本文算法流程

3 实验与结果

为评估本文所提出的方法，在室外公共数据集KITTI上进行了实验。实验主要分为3部分，第一部分为SLAM定位精度对比实验，与当前最主流的两种单目SLAM算法进行了定量的对比：基于特征点的单目ORB-SLAM(关闭回环检测)算法和基于直接法的DSO算法；在第二部分，为更好地验证基于深度预测网络的SLAM绝对尺度估计方法用于机器人位姿估计的性能，本文还与同样基于深度学习的视觉里程计方法在定位精度方面进行了对比；最后，为更直观表现算法构建全局轨迹的准确性，在实验的第三部分定性对比了本文算法与ORB-SLAM算法得到的全局轨迹结果。

本文算法运行的硬件配置为Intel Core i5-4460M CPU，主频为3.2 GHz，内存为8 G，GTX1030显卡电脑。实验系统采用Ubuntu16.04操作系统。

3.1 SLAM定位精度对比实验

为定量对比在没有回环检测功能的情况下各算法的定位精度，在实验中关闭了ORB-SLAM回环检测，DSO算法本身不具备回环检测功能，本文也只用绝对深度信息来估计相机位姿。

表1展示了在KITTI数据集上3种算法的均方根误差(root mean squared error，RMSE)，每组数据都进行了5次实验并取其平均值,测试图像来自于KITTI数据集中0号相机的灰度图。

表1 KITTI数据集上的均方根误差(RMSE)

在实际情况下，机器人经常处于没有回环路径的未知场景中，想要获得准确的全局定位，只能依靠稳定精确的位姿增量估计，从而使机器人在陌生环境中更好确定自身位置信息。为验证算法在所有场景下的定位精度，实验中关闭了ORB-SLAM算法的回环检测功能。

本文算法通过加入场景的绝对尺度减少单目SLAM的尺度漂移，而不依赖回环检测来校正相机全局位姿。从表1中可以看出，KITTI数据集的07序列场景下，本文的定位精度相比于单目ORB-SLAM提高了64.23%，而KITTI-07序列在起点处有一个回环路径。同样具有多处回环的00、05和06序列，也分别提高了34.20%、65.49%和63.56%。除了在有回环的场景下定位精度提高外，在09序列和10序列两个无回环但长距离的场景下，相对于DSO算法在定位精度上也分别提高了21.62%和75.10%，而对比ORB-SLAM算法分别提高了11.38%和67.36%。这验证了本文算法在无论有、无回环的场景下，都能够表现出较高的定位精度和较强的鲁棒性。

由于01序列较为空旷的场景和较大的光强变化，3种算法均无法完成测试。而在较短直线路段的03、04序列，本文算法相比于DSO算法定位精度较高，但相对ORB-SLAM定位精度略有所下降。主要原因是本文算法将场景中被列为远点的ORB特征点剔除，而近处的路面特征点较少，从而产生了误差。

3.2 基于深度学习的VO定位精度对比实验

为了更好地体现算法定位精度的性能，本文还使用KITTI官方评价指标对同样基于深度学习的DeepVO算法和LSTMVO算法进行了对比，其中平移均方根误差trel以“%”来度量，旋转均方根误差rrel以“°/100 m”来度量。两种算法的数据来源于对应论文，文中只提供了KITTI数据集的部分序列上的结果，而本文算法分别在KITTI数据集00-10所有序列上进行了测试，每组结果取5次实验的平均值。

如表2所示，对3种算法进行了定量评估，加粗数据表示更优的结果。DeepVO算法和LSTMVO算法属于视觉里程计范畴，并采用训练好的端到端的神经网络来估计相机位姿，而本文算法是通过几何以及优化的方式来估计位姿，从表中可以看出，在定位精度上相对另外两种算法的均值分别提高了50.77%、59.72%和29.87%、81.37%。在KITTI数据集00-10所有序列上，仅有04序列上DeepVO算法在平移均方根误差方面比本文高31.40%，其它结果均低于本文算法得到的结果。这也验证了相比于纯使用训练端到端的神经网络来预测相机位姿的方法，通过深度预测网络估计的单目绝对尺度来辅助SLAM进行位姿估计的定位精度更高。

表2 与深度学习算法的对比结果

3.3 轨迹对比实验

为定性的评估全局轨迹结果，且由于ORB-SLAM的定位精度明显优于DSO，故只选择与单目ORB-SLAM进行了轨迹对比。实验分别测试了在无回环、一个回环、较多回环和多回环4种场景下的轨迹对比，分别对应着KITTI数据集中的10、07、05和00这4个序列，并对平移和旋转误差曲线也进行了对比。

图4～图7展示了本文算法和单目ORB-SLAM(无回环)在KITTI数据集中10、07、05和00序列的全局轨迹误差、平移误差和旋转误差的对比曲线，其中GT表示KITTI数据集提供的标准真值数据，OUR表示的是本文算法得到的结果。

图4 序列10的轨迹、平移和旋转误差曲线

图5 序列07的轨迹、平移和旋转误差曲线

图6 序列05的轨迹、平移和旋转误差曲线

图7 序列00的轨迹、平移和旋转误差曲线

由于单目ORB-SLAM存在尺度漂移问题，严重依赖回环检测校正全局位姿，但大多数真实场景下缺少回环路径，当机器人在探索没有回环路径的未知场景时，机器人很难通过回环检测来校正全局位姿，所以会随时间产生累积误差。而本文通过深度预测网络为单目SLAM增加了准确的绝对尺度信息，从而在增量的估计过程中减少了单目SLAM算法的累积漂移误差，而不再需要回环检测来校正全局位姿。从图中可以看出，本文算法不仅在多回环场景下的00序列相比于单目ORB-SLAM更靠近真实轨迹,而且也可以适用于如10序列没有回环的场景。这也进一步验证了通过深度预测网络来估计单目SLAM的绝对尺度，能够显著地降低累积漂移误差，从而提高相机的定位精度。

4 结束语

本文提出一种通过结合深度预测网络MonoDepth恢复单目绝对尺度的低成本、高定位精度的单目SLAM算法，能够利用真实深度信息来有效解决单目SLAM的尺度漂移问题，并通过剔除不可靠深度值的特征点可以消除远点对定位精度的影响，从而利用准确的深度信息减少累积定位误差。KITTI数据集上的实验结果表明，结合深度预测网络的单目SLAM绝对尺度估计方法，在无论有、无回环路径的室外场景下都能有效提高SLAM的定位精度。