最小化光度误差先验的视觉SLAM算法

韩健英，王 浩，方宝富

(合肥工业大学 计算机与信息学院，合肥 230009)

1 引 言

近年来，自动驾驶、机器人、无人机、三维重建和AR/VR等技术得到飞速发展，SLAM(Simultaneous Localization and Mapping)也成为了研究的热点问题.因为在未知环境中，机器人准确定位需要依靠精确的环境地图；而想要构建精确地图，机器人也要知道自己在环境中的准确位置，SLAM可以构建一个完整的、全局一致的环境地图，以便为后期的导航、避障、运动规划等应用提供必要的支持[1-3].

如今，相比基于激光雷达实现的激光SLAM[4,5]，视觉传感器(单目、双目、RGB-D相机)不仅更便宜，还可以提供更丰富的环境信息.其中，RGB-D相机可以同时提供RGB图像和对应的深度图，节省大量的计算深度的运算开销.因此，在室内的定位建图中，用RGB-D相机实现视觉SLAM越来越受欢迎.

视觉里程计(Visual Odometry，VO)作为视觉SLAM的前端[6]，根据相机采集到的图像信息，利用对极几何[7]、ICP(Iterative Closest Point)[8]或PnP(Perspective-n-Point)[9]等算法计算出相机的位姿，为后端优化提供了初始值.

Davison[10]提出了一个实时的视觉SLAM系统(MonoSLAM)，该系统使用的是单目相机，在前端跟踪稀疏特征点以加快系统运行速度，在后端使用扩展卡尔曼滤波(EKF)，把地图中所有的路标点和单目相机的当前位姿作为状态量，以此来维护和更新状态量的均值和协方差矩阵.但MonoSLAM缺少回环检测[11]的能力，地图中路标点的数量不能过多，在跟踪特征点时也很容易丢失，一般应用在比较小的场景.

而自从RGB-D传感器出现以来，基于RGB-D相机构建视觉SLAM系统也获得越来越多研究人员的关注.最早的RGB-D SLAM系统是由Newcombe等人[12]提出的KinectFusion，使用ICP算法计算最优的相机位姿，但其同样不能检测回环，容易产生较大误差，只有在小范围的空间内才能保证精度.由Whelan等人[13]提出的Kintinuous是对 KinectFusion 的改进，借助于一个循环缓冲器并使用重定位、位姿图优化等方法达到实现回环检测的目的，可以在一个较大的未知环境中构建地图.而Endres等人[14]提出一个开源的RGB-D SLAM系统，该算法使用SURF[15]特征算法来检测特征点并计算特征描述子，使用改进的ICP算法来计算相机的位姿.

当采用直接法估计相机位姿时，需要根据两帧图片之间的像素灰度值差异来约束计算.随着直接法的发展，其精度得到了极大的提高.Newcombe等人[16]提出了DTAM，这是一个单目的视觉SLAM，可以实时建立场景的三维模型.它使用了逆深度[17]参数，在数值稳定性上效果更好，可以做到稳定跟踪.但是DTAM建立了稠密的地图，并采用了全局位姿优化，因此其运算量很大，模型扩展性能被抑制.Forster等人[18]提出的SVO采用直接法来跟踪稀疏特征点，由于不需要计算描述子，运行速度非常快，但SVO是一种视觉里程计，没有回环检测，很难消除累计误差，建立地图的一致性也难以保证，其主要搭载俯视相机，应用在无人机等领域.Engel等人[19]提出的LSD-SLAM使用单目相机构建了一个基于直接法的SLAM系统，可以在普通的CPU上进行运算，但当相机快速移动时，LSD-SLAM容易跟踪失败.

Klein等人[20]提出了基于关键帧的PTAM，该算法用多线程方法实现了并行跟踪和建图的过程，并在后端使用了非线性优化来优化位姿.PTAM满足了视觉SLAM的实时性要求，但跟踪时容易丢失.而Artal和Tardos等人[21]提出ORB-SLAM算法，采用跟踪线程、局部建图线程和回环检测线程并行计算和优化位姿的方法.并且，他们又在ORB-SLAM基础上，提出了ORB-SLAM2[22]算法.ORB-SLAM和ORB-SLAM2都采用了多线程的方案，满足了视觉SLAM实时性要求.该系统使用ORB特征[23]算法，提取图像的特征和描述子，无论是前端、后端或回环检测，都是采用ORB特征进行匹配计算，使用PnP算法求解相机位姿.ORB-SLAM2还采取多种优化策略，使其有很高精度.但ORB-SLAM2使用的匀速模型和参考帧模型，存在精度不足的问题，在剧烈抖动的时候，也很容易跟踪失败.

本文是在ORB-SLAM2的基础上开展进一步的研究，提出了一种定位更加准确并且鲁棒性能更高的视觉SLAM算法：将最小化光度误差和ORB-SLAM2所采用的最小化重投影误差相结合，设计一个组合优化的视觉SLAM框架：在前端的跟踪线程中，为了提高系统鲁棒性能，首先使用双线性插值法得到特征点的灰度值，根据两帧之间的光度误差来得到相机初步的位姿.但是，因为光照变化、曝光等原因，估计的位姿可能并不准确，为了进一步提高当前帧位姿的精确度，通过最小化重投影误差对当前帧位姿再次优化.并且在关键帧选择机制上，提出一种新的关键帧选择方法：基于当前帧与上一关键帧的位姿变化，度量两帧之间的距离.当其超过设定的阈值时，说明两帧之间的场景变化较大，可以将当前帧设置为关键帧，参与到SLAM算法的优化与建图.

2 算法流程与框架

本文在ORB-SLAM2的基础上展开进一步研究，其中在ORB-SLAM2的跟踪线程中，每次在估计当前帧的初始位姿时，采用匀速模型或者参考帧模型.与真实值相比较，一般误差较大，需要对这个初始值进行多次迭代优化.

为了改进上述缺点，提高相机的定位精度，本文提出在视觉SLAM算法前端中，采用双线性插值的方法得到特征点的灰度值，提高系统的鲁棒性，根据最小化光度误差计算出当前帧与参考关键帧之间的位姿变化.因为基于光度误差计算位姿的速度较快，所以并不会影响视觉SLAM算法的实时性.但当环境的光照变化比较剧烈时，灰度不变假设不能被满足.此时通过最小化光度误差得到的位姿计算两帧图片的内点数量小于阈值，说明基于光度误差计算得到的位姿精度不足.

为了进一步提高当前帧位姿的精度，本文以最小化光度误差得到的估计位姿为初始值，通过最小化重投影误差对其再次优化.两次迭代优化来降低相机定位的误差，保证计算得到的相机位姿足够精确，进而求得当前帧在世界坐标系中的准确位姿，建立统一的坐标系，可以得到较高精度的环境地图.本文改进后的视觉SLAM算法前端跟踪线程的流程图如图1所示.

图1 最小化光度误差先验的视觉SLAM算法跟踪线程Fig.1 Tracking threads of visual SLAM algorithm for minimizing photometric error prior

由图1可以看出，当视觉SLAM系统从传感器中获得RGB图像和深度图后，首先要提取图像的ORB特征，并根据深度图计算出相应特征点的空间坐标并构建出图像帧，完成初始化.根据提取的ORB特征点，其灰度值要通过双线性插值的方法得到，增加鲁棒性.通过最小化光度误差和最小化重投影误差来计算并优化当前帧的位姿.

在关键帧的选择上，根据当前帧与上一关键帧的位姿变化，计算两帧之间的运动距离.当距离大于设定阈值时，就把当前帧选为关键帧并加入到关键帧序列，参与到局部建图和后端优化过程.

2.1 基于光度误差的位姿估计

在本文提出的优化算法中，首先根据最小化光度误差计算当前帧的初始位姿.当一个新的图像帧进入系统时，通过当前帧与参考关键帧之间的特征点灰度值差异计算出两者之间的位姿变换，即两帧之间的旋转矩阵R和位移向量t.为了方便计算，根据文献[24,25]，相机位姿可以用李代数ξ表示，方便求导与优化.

图2 空间点的投影示意图Fig.2 Projection of space points

如图2所示，对于一个空间点Pc(Xc,Yc,Zc)，其在两帧图像I1和I2上的投影为p1和p2，根据针孔相机模型：

(1)

其中puv是空间点Pc在像素坐标系下的坐标；π(·)为变换函数，将相机坐标系下空间点Pc转为相应的像素坐标；fx、fy、cx、cy为相机的内参.

由于两帧图像之间并没有执行特征匹配算法，所以并不知道哪个p2与p1对应着同一个空间点.但显然的是，若当前帧的估计位姿与真实值有较大差异时，p2与p1的灰度值就会有明显的差异.当有N个点对时，总的差异越大，说明估计的位姿越不准确，就要对估计位姿进行调整优化.每一次优化的方向，都要使总的特征点之间灰度值差异不断减小.定义光度误差为：

e=I1(p1)-I2(p2)

(2)

其中I(p)为像素p在图像I中的灰度，优化目标为该误差的二范数.当系统在环境中检测到N个空间点Pi(i=1,2…N)时，整个优化问题就会变为：

(3)

这里优化变量为相机位姿ξ，为了得到最佳的ξ，降低误差，就需要计算出每次优化的方向，即误差e关于ξ的导数,根据李代数扰动模型：

e(ξ⊕δξ)=I1(π(Pi))-I2(π(exp(δξ∧)exp(ξ∧)Pi))

(4)

就可得到误差e关于相机位姿ξ的雅可比矩阵J，其值可以表示为：

(5)

(6)

(7)

由此就可以根据计算出的雅可比矩阵J，得出每一步迭代优化的方向.使用g2o[26]中的LM(列文伯格-马夸尔特)算法计算增量，对位姿进行迭代优化，计算出最佳的当前帧位姿ξ，使总的光度误差最小.

因为两帧图像之间有很多灰度值相同的像素点，容易误匹配.为了提升算法的鲁棒性能，要考虑到每个特征点的相邻像素灰度值.本文在计算像素坐标为(i,j)的特征点的灰度值I(i,j)时，没有直接选择(i,j)处的灰度值，而是取该特征点周围的一些像素，通过双线性插值的方法得到特征点的灰度值I(i,j)：

I(i,j)=α1×I(i,j)+α2×I(i+1,j)+α3×
I(i,j+1)+α4×I(i+1,j+1)

(8)

可以看出，本文在灰度值的取值上，考虑到了它周边的一些像素灰度值.这样，在计算两帧之间一对特征点的灰度误差时，把该特征点的周围元素考虑进去，可以增强系统的鲁棒性能.

给定一个当前帧的位姿ξ，当绝大部分的特征点对都是正确匹配，说明此时位姿已经足够精确.但是此时如果还有某几个误差项的数据是错误的，并且这几个特征点对的误差很大.优化算法为了降低误差，仍会持续调整ξ的值，使这几个点对的误差减小.然而为了优化这个错误的特征点对，其他正确的特征点对会受到影响，反而降低位姿的精度.为了减少外点对优化问题的影响，可以使用一个鲁棒核函数ρ(·)，增强本文算法的鲁棒性能：

(9)

其中，

(10)

可以看到，当每一个特征点对的灰度值误差e的绝对值大于阈值δ后，函数增长由平方形式变成了线性的.这样做避免了当某个错误的特征点对的灰度值误差很大，在对位姿进行优化时，对其他正确点对的影响过大，导致优化效果不好，同时其也保留了可导的性质.

因为基于光度误差计算位姿是快速的，所以并不会影响视觉SLAM算法的实时性.但是由此得出当前帧的位姿，会受到环境因素干扰.在这种情况下，可能会导致位姿准确性不佳，此时两帧之间的内点数目较少.为了进一步提高当前帧位姿的准确性，以最小化光度误差优化得到的位姿为初始值，本文算法将再基于重投影误差对当前帧位姿进一步优化，提高定位精度.

2.2 基于重投影误差的位姿估计

与最小化光度误差相比，采用最小化重投影误差，对环境中光照变化的敏感度较小.当获得来自最小化光度误差计算的当前帧位姿ξ时，可以把ξ作为最小化重投影误差优化的初值，对ξ进行再次优化.

图3 视觉SLAM重投影误差示意图Fig.3 Reprojection error of visual SLAM

(11)

(12)

ρ(·)为鲁棒核函数.

针对该函数求解，初始值采用基于光度误差估计的当前帧位姿，利用非线性优化对当前帧位姿迭代优化，就能计算出一个比较精确的相机位姿ξ，使N对特征点的重投影误差最小.

通过两次优化的ξ可以计算出当前帧在世界坐标系中的位姿Tcw，用于将当前帧提取到的空间点映射到世界坐标系中，以此构建一个统一的坐标系：

Tcw=exp(ξ∧)·Trw

(13)

其中Trw是参考关键帧所在的相机坐标系与世界坐标系的变换矩阵.得到Tcw之后，当前帧坐标系就可以跟世界坐标系建立联系，建立统一的全局地图.

可以看出，本文通过两次位姿优化：基于光度误差先求出初始的当前帧位姿；采用最小化重投影误差对求出的初始位姿再次优化，提高了定位的精度.得到当前帧的精确位姿之后，可以根据关键帧选择机制来判断当前帧是否可以作为一个关键帧插入到关键帧序列.

2.3 关键帧选择机制

在视觉SLAM对图像帧的处理过程中，关键帧的选择是很重要的一部分，关键帧选择太密集，虽然有利于提高位姿估计的准确度和建图精度，但是会导致后端优化处理花费更多时间，导致视觉SLAM系统不能实时处理不断生成的关键帧；但是关键帧太少则会使位姿估计的准确度下降，不能建立一个精确的环境地图.

本文提出了一个新的关键帧生成机制：每当RGB-D相机采集到一帧图像时，都将该帧图像与其上一关键帧进行比较，计算其运动幅度，若运动幅度超过阈值，则将该帧存储为关键帧.具体来说就是将当前帧位姿ξ，转化为对应的旋转矩阵R和位移偏量t(tx,ty,tz)，根据R和t来计算两帧之间的运动幅度.但是采用旋转矩阵作为旋转的评价参数时，不能直观的反应旋转的剧烈程度.本文采用将旋转矩阵转化为旋转欧拉角的方法来表示旋转的程度：

(14)

其中atan2()函数返回的是方位角，而R表示为：

θyaw,θpitch,θroll为欧拉角，分别表示偏航角、俯仰角和滚转角.将旋转矩阵转化为欧拉角后，就可以根据欧拉角来衡量旋转幅度的大小：

(15)

正如式(15)，计算的两帧间运动幅度d，可通过计算欧拉角和位移偏量的二范数来作为运动幅度大小的度量值.分别赋予欧拉角和位移偏量的范数一个权值α和β，当d的值超过阈值Threshold时，即可将当前帧设为一个关键帧，加入到关键帧序列.当相机在短时间内运动时，若仅产生纯平移运动，此时相机视野场景变化的范围较小；若相机产生旋转运动时，往往会对相机视野的变化影响巨大.因此在设置α和β的值时，β的值要远大于α，即欧拉角的权重要大于位移偏量的权重.

2.4 优化算法

本文改进了前端中的位姿估计方法和关键帧选择机制，两次对位姿的优化可以使得其精度进一步的提高，关键帧的选择也更加合理.可以把这种视觉SLAM位姿优化算法的伪代码描述为：

算法.Visual SLAM Algorithm For Minimizing Photometric Error Prior

输入：RGB and Depth image

输出：Camera Pose

1.grayImg←Gray(RGB)

2.points←ExtractORB(grayImg)

3.CurFrame←InitializeFrame(grayImg,Depth)

4.whileTruedo

5.ifisTracking()then

6.foreachp∈pointsdo

7.I(p)=bilinearInterpolation(p)

8.endfor

9. MinPhotometricError(CurFrame,RefKeyFrame)

11. MinReprojectionError(CurFrame,RefKeyFrame)

13.endif

14.ifisRelocalization()then

15. Relocalization()

16.endif

17. TrackingLocalMap()

18.d←CalculatingDistance(CurFrame,LastKeyFrame)

19.ifd≥Thresholdthen

20. InsertKeyFrame(CurFrame)

21.endif

22.endwhile

改进后的视觉SLAM算法在计算出当前帧的位姿选择出关键帧后，就可以进入局部建图线程，进行关键帧的处理、地图点的删除与新建、局部优化和关键帧的删除等操作，建立起局部地图.再借助于ORB-SLAM2优秀的后端优化处理和回环检测能力，可以进一步降低定位误差并提高建图的精度.

3 实验评估

本文算法运行的平台配置为Intel 酷睿i5处理器，主频为2.6GHz，8GB内存，采用Ubuntu16.04操作系统，没有使用GPU对该方法进行加速计算，以验证本方法在普通CPU上的实时性能.

(16)

本文在实验中主要采用了TUM数据集中的fr1/360、fr1/desk、fr2/360、fr2/desk、fr3/long_office等序列进行测试.这些序列在采集时包括不同的相机运动状态，如快速移动、缓慢移动、强旋转等.为了避免偶然的情况对实验结果造成影响，在实验中，对每个序列都进行了多次的测量，并将绝对轨迹误差的均方根误差平均值用于对实验进行评估，可以更好地评估系统性能.

3.1 关键帧选择实验

在本文中提出的新的关键帧选择机制，可以在不影响系统实时性能的同时，选择更多关键帧，来为建图与优化提供更多可靠的数据.图4显示的是改进后的SLAM算法和ORB-SLAM2在部分序列下得到的关键帧数目的比较结果.

图4 本文算法和ORB-SLAM2在TUM据集下的关键帧数Fig.4 Number of key frames between our algorithm and ORB-SLAM2 in TUM dataset

从图4中可以看出，与ORB-SLAM2相比，本文提出的优化算法在不同的序列中总是可以提供更多的关键帧，这样，可以为局部或全局优化、回环检测等提供了更多的数据支持，以提高系统定位的准确性.

3.2 算法定位精度评估

视觉SLAM系统的核心问题在于准确的相机定位和精确的环境建图，并且建立的环境地图精度高度依赖相机定位的准确度，所以在本文中，主要评估相机定位的准确度，并且验证优化后的SLAM算法鲁棒性能.表1显示的是本文优化后的SLAM算法与ORB-SLAM2的绝对轨迹误差(ATE)的均方根误差(RMSE)的比较结果.

表1 本文算法与ORB-SLAM2的RMSE值Table 1 RMSE values for our algorithm and ORB-SLAM2

根据表1的比较结果，可以看出，在大部分数据集序列中，与ORB-SLAM2相比，经过改进的 SLAM算法可以提供更高的精度.尤其是在fr1/360、fr2/360和fr1/desk这三个序列中，绝对轨迹误差的均方根误差下降的非常明显，分别下降了31.57%、16.92%和23.56%.但是在使用ORB-SLAM2对fr1/desk序列进行多次测试时发现，经常会发生跟踪失败，导致系统不能正常运行的情况.所以表1中fr1/desk序列的19.83mm的误差结果是在只计算跟踪成功情况下得到的，经过多次实验，比较在跟踪成功条件下的均方根误差.

图5 ORB-SLAM2在fr1/360上的轨迹和误差Fig.5 Trajectory and errors of ORB-SLAM2 on fr1/360

由图5和图6可以看出，在fr1/360序列中，ORB-SLAM2的轨迹误差明显大于本文优化算法的轨迹误差.尤其是在fr1/360序列中，优化后的算法明显降低了轨迹误差.这主要是因为与ORB-SLAM2相比，优化后的算法不仅选择了更加合理的关键帧，利用了图像更多的信息.基于最小化光度误差初步得到了当前帧的相对精确的位姿，而不是简单的将当前位姿定义为前一帧的位姿或参考帧的位姿.再经过最小化重投影误差的优化，两次优化降低了定位的误差，提高了精度.

图6 本文算法在fr1/360上的轨迹和误差Fig.6 Trajectory and errors of our algorithm on fr1/360

3.3 算法运行时间评估

在本文的优化算法中加入组合优化策略和关键帧的选择机制之后，不仅提高了位姿的精确度，而且这种做法并没有影响其在普通CPU上的实时性.表2显示的是优化后的SLAM算法与ORB-SLAM2处理每帧所需时间的平均值比较结果.

表2 本文算法与ORB-SLAM2平均每帧运行时间Table 2 Average operation time per frame of our algorithm and ORB-SLAM2

从表2可以看出，在大部分序列中，改进后的算法平均每一帧处理所需时间较ORB-SLAM2仅仅只是多3ms左右，这是因为在改进的算法中加入了更多的关键帧，并且实施的组合优化策略在一定程度上需要些时间.但是在实际运行过程中，并不会影响系统运行实时性，却可以提高相机位姿的精确度，降低系统定位误差.

3.4 系统鲁棒性能

在视觉SLAM应用中，由于相机的快速移动、剧烈抖动等情况，会导致SLAM系统不能及时处理这种突发情况，导致前端跟踪失败，系统不能正常运行.但是本文的算法在相机快速移动时仍具有良好的鲁棒性能，在解决两帧图像之间由于匹配失败而导致SLAM系统前端跟踪失败方面，具有很好的定位精度.

在本文3.2中，介绍了在对fr1/desk序列进行实验时发现，ORB-SLAM2很容易跟踪失败.这是因为在该序列第170帧到171帧运行时存在着剧烈的抖动，如图7所示.两帧间的大角度转动造成图像模糊.若仅基于最小化重投影误差计算，此时两帧图像间想要正确匹配特征点比较困难，所以视觉SLAM系统在前端容易进入跟踪失败状态.

图7 两帧间的大角度转动Fig.7 Large angle rotation between two frames

在实验中，针对fr1/desk序列进行多次测试，实验结果显示，对比ORB-SLAM2和改进后的SLAM算法，在运行ORB-SLAM2时，有50%的概率在系统运行到第171帧时前端跟踪失败.如图8(a)所示，在框选的部分上，相机是没有被跟踪到的：在图8(a)标记为1的地方前端跟踪失败，导致ORB-SLAM2在接下来的运行过程中，由于重定位一直失败，在其中一段运行过程中，不能计算出当前帧的位姿和得出有效的环境地图.直至ORB-SLAM2在图8(a)标记为2的地方重定位成功，系统才能重新成功运行.而基于改进后的算法进行实验时，成功率从50%提高到了81.3%，在fr1/desk序列上的第170帧到171帧，前端基于光度误差跟踪成功，得到的相机运动轨迹和误差如图8(b)所示.可以看出，在图8(b)中，改进后的算法仍可以有效且正确的跟踪，成功得到相机的位姿和轨迹.

图8 ORB-SLAM2(a)和本文算法(b)在fr1/desk的估计轨迹Fig.8 Estimated trajectory of ORB-SLAM2 and our algorithm on fr1/desk

4 结 论

本文针对相机位姿估计和关键帧选择方面做出了改进和优化，提高了相机位姿的准确度和点云地图的精度，鲁棒性能也得到提高，同时实时性也可以得到保证.但是仍然存在不足之处.最小化光度误差可以提高初始估计位姿的精度，但要求环境亮度变化幅度较小.这是因为其前提是灰度不变假设，但在真实环境中的亮度可能会发生变换.并且当相机处于低纹理的环境中，特征点较少，本文算法和ORB-SLAM2都不能保证定位的准确性和建图准确性，甚至可能跟踪不到相机的位置.因此在接下来的研究中，要考虑这些不足之处.可以探索在视觉SLAM中加入线特征，线特征有助于在点特征较少的情况下，使用线特征匹配，在前端进行更精确的位姿估计，提高位姿精度和建图的准确性.