集成光度在线标定的边缘对准SLAM方法

徐晓苏，游颖辉

（1. 微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2. 东南大学仪器科学与工程学院，南京 210096）

移动机器人在服务、工业自动化等领域具有广泛应用[1]。视觉同步定位与建图（Simultaneous Localization and Mapping, SLAM）是一种基于图像的相机运动估计方法，在移动机器人的导航与控制中起到至关重要的作用，尤其在无外部信号作为参考时。在近二十年里，国内外在视觉SLAM领域有了较多研究，包括基于稳定特征的特征点法[2-4]、基于像素灰度的直接法[5,6]、同时参照灰度和特征的半直接法[7]、通过迭代最近点法（Iterative Closest Point, ICP）跟踪相机的方法[8]等。特征点法对于光照变化较为鲁棒，但存在计算描述子耗时较长、无法充分利用环境信息等问题。直接法可以充分利用环境信息且不依赖环境纹理，但无法有效应对光照变化的环境[9]。半直接法可以避免灰度变化的影响，但仍然受制于图像信息的准确度。基于边缘对准的视觉SLAM方法类似于半直接法，提取图像中的边缘特征，通过边缘对准实现运动估计，能够充分利用环境信息，并对光照变化有一定稳定性[10]，可以克服目前主流方法的缺点。

基于边缘对准的视觉定位方法可以追溯到Eade E等人的研究[11]，该方法首次实现了基于粒子滤波的利用边缘特征的视觉里程计。基于优化的方法则发端于Tarrio J J等人于2015年公开的一种基于边缘特征的单目视觉里程计，该研究首次说明了边缘对准是一个高效的可并行的操作[12]。Zhou Y等人于2019年提出的基于Canny检测子的视觉里程计Canny-VO[13]，该方法提出了边缘配准中常用的欧式域距离变换的两种替代方法：近似最近邻域和定向最近邻域，并将边缘的旋转加入到优化中。文献[14]提出的实时鲁棒的边缘对准SLAM（RESLAM）利用随机蕨实现了回环检测。目前的多数基于边缘对准的方法都未考虑图像光度变化对于边缘提取的影响，更未在设计算法时设计离线或在线光度标定的环节。在光度标定的相关研究中，文献[6]通过对数据集的图像曝光时间、相机的Gamma响应函数以及详细的镜头畸变因子进行了离线光度标定来提高视觉里程计性能，Paul Bergmann等人在2018年提出了一种对自动曝光的视频流进行光度校正的方法[15]，并和文献[6]提出的算法进行组合，有效提升了定位精度和建图效果。

针对图像光度变化导致不同图像提取的边缘特征分布差异较大的问题。本文对文献[14]所提出的基于RGB-D相机的算法进行了优化改进，集成了在线光度标定模块，提升了进行边缘提取时的图像质量，在数据集上验证了在算法中集成在线光度标定对于定位精度有一定提升；在此基础上，对残差项的残差权重函数进行设计，提升了系统多次运行的稳定性。

1 算法整体框架

本文基于机器人软件平台（Robot Operating System, ROS）实现集成在线光度标定的边缘对准视觉里程计，其整体流程如图1所示。该方法分为光度标定节点以及包含前端跟踪线程、局部建图线程、全局建图线程的里程计节点。通过订阅传感器节点或者数据集发布的话题获取图像，获取的图像首先被读入光度标定节点。在光度标定节点，对图像进行提取角点、角点跟踪、估计参数等一系列操作，再通过发布话题的方式将校正后的图像发布。里程计节点订阅经过光度标定后的图像消息，在跟踪线程对图像帧先进行提取图像边缘的操作，然后通过距离变换构建残差方程，迭代求解残差方程实现边缘对准，进而实现对相机运动的跟踪，然后进行关键帧选择。在局部线程内则对跟踪线程选择的关键帧进行回环编码的计算，构建非线性优化滑动窗口，对滑动窗口中的变量进行局部优化，对关键帧的位姿进行优化，优化完成后根据设置的条件执行边缘化策略以维持滑窗中的变量维数在一定数量；编码后的关键帧被存入关键帧数据库，用于进行回环检测、回环验证等环节，最后利用位姿图构建全局优化对位姿做进一步优化。

图1 算法整体流程图Fig.1 The overall flow chart of the algorithm

2 光度标定

光度标定节点通过提取角点建立图像之间的数据关系实现跟踪，进而完成对曝光时间、渐晕等参数的估计，再对图像做校正并发布图像消息话题。

2.1 图像光度建模

场景点被光源照亮时，将光线反射到空间中，反射的总光量称为该三维点的辐射L。如果移动观察者接收到该点的辐射与观察者的观察角度无关，则称该场景点的辐射表现为兰伯特反射。场景点的辐射被相机的感光元件接收，记感光元件上位置x接收到的辐射照度为I(x)。理想情况下，不同空间位置具有相同辐射的场景点会产生相同的传感器辐射照度。然而，对于大多数相机而言，采集到的图像像素强度从图像中心向图像边界下降。这种现象被称为渐晕效应，一般是由于光线被透镜部分阻塞或者由于透镜的几何形状引起的。则感光元件上的辐射照度I(x)可以通过I(x) =V(x)L表示，V: Ω→ [ 0,1]为渐晕因子，与图像感光元件中的位置x∊Ω相关。由于感光元件的辐射照度在曝光时间窗口内随时间累积，假设在时间窗口内的传感器感光元件的辐射照度恒定，对于曝光时间e内累积的辐射照度为Iacc(x) =eI(x) 。f: ℝ→ [ 0 ,255]为相机响应函数（Camera Response Function, CRF），从而场景点辐射L与图像输出的像素强度O之间的对应关系可以表示为：

使用经验响应模型（Empiric model of Response,EMoR）对CRF进行建模，并利用基函数hk(x)组合得到如下的全局响应函数：

其中ck∊R，f0(x)为通过原理成分分析（Principle Component Analysis, PCA）获得的平均响应。本文假设渐晕的分布符合径向渐晕模型，即衰减因子在图像中心是对称的，进一步地假设渐晕的中心和图像的中心一致，则渐晕因子可以用一个六阶多项式进行拟合。

其中R(x)是图像点x相对于图像中心的归一化半径。

2.2 参数估计

对于给定的图像F，式(1)中的各项参数仅输出的像素强度O已知，因此进行光度标定还需选择一定数量的场景点通过重投影建立图像帧之间的联系。通过提取Shi-Thomais角点，并采用具有自适应增益的Kanade-Lucas-Tomasi（KLT）光流跟踪算法来获取点对之间的联系。同时为了更好地校正渐晕，需要提取的角点均匀分布在图像上，因此将图像分成一系列小块再提取角点。同时为了增加观测量，在跟踪的角点周围的像素块也被用于计算重投影残差。

记输入的图像帧集合为Fp，跟踪的场景点集合为P，对于在Fp中可视的场景点p∊P，通过跟踪点对可以建立如式(4)所示的残差方程[15]。

通常需要较多的图像来求解优化式(4)中的所有参数，曝光时间可以通过图像相邻帧之间的时差来估计，而渐晕模型和响应模型则需要多个图像帧来估计。因此在本文算法中，将需要估计的参数进行分离估计。每新进一帧图像，先用当前估计的渐晕和响应函数校正图像，再估计曝光时间。因此式(4)可改写为[15]：

其中，M为当前图像帧集合的大小，Pi记为在第i帧可视的场景点，f-1是逆响应函数。对于固定的场景点的辐射照度pL，式(5)仅曝光时间是未知量，求解时间随变量维度线性增长，可以快速求解。全部参数的优化估计通过在后端优化过程中对式(4)求解得到。在本文中对图像进行光度标定的效果如图2所示，图像质量得到了明显改善，提取的边缘特征分布更加均匀。

图2 光度标定前后的图像Fig.2 Images before and after photometric calibration

3 基于边缘对准的视觉SLAM

视觉里程计通过提取经过光度标定的图像中的边缘，并利用距离变换进行边缘对准，实现对相机运动的估计，并在后端构建滑动窗口对位姿再进行优化。

3.1 运动估计

对于在t时刻接收到的图像It和深度图像Zt，假设已完成了对准和时间同步，也即对于图像It上的像素u=(x,y)，在Zt中的对应深度z=Zt(u)。

定义像素点u在三维空间对应的场景点为P,则从像素点u计算三维点P的逆投影函数如下：

其中，fx、fy、cx、cy为相机的内参，通过对相机标定获得。对应地，将三维点P投影至像素坐标系的投影函数具有如下的形式：

式中，ξji为图像帧Fi到Fj的相对变换，对应的位姿变换矩阵为Tji。

在每帧图像中，通过对齐当前帧Fj与关键帧Fi对应的边缘检测结果来估计两帧之间的相对运动ξji，在对齐过程中，将关键帧Fi中的具有有效深度的所有像素点εi重投影至当前帧Fj，并与当前帧检测到的最近的边缘像素对齐。在本文中通过在欧氏距离域上计算距离变换来预先计算图像中每个像素到有效边缘像素的距离，从而得到重投影的边缘像素距离变换残差：

其中，Dj为当前帧Fj的距离变换，ui为关键帧Fi中边缘像素的位置。联合参考关键帧中的所有具有有效深度的像素点的距离变换残差，构建优化方程：

Huber权重系数的计算公式如下：

T-分布的计算公式如下：

式中，k、vσ、 均为根据传感器误差特性拟合得到的系数。

3.2 后端优化

为了提高算法的精度，在局部建图线程构建滑动窗口对边缘像素的深度、相机位姿和相机内参进行联合优化。记滑动窗口中的图像帧集合为W，将滑动窗口中的每一帧激活的边缘像素点分别投影到其他帧上，类似于式(11)，得到：

式中，Ai为第i个关键帧中激活的残差项集合，δr为误差权重系数，r是包含优化后的相机内参C和逆深度参数ρi的残差项：

其中，为利用优化后的关键帧相对世界的变换矩阵ξi,ξj重投影至关键帧Fj。通过Gauss-Newton优化方法最小化式(11)，从而有：

式中，W∊ℝn×n为对角权重矩阵，r为残差序列，J为r的Jacobian矩阵，对式(14)迭代求解得到优化后的参数。

3.3 关键帧选择及边缘化

为了减少后端非线性优化的计算量，在前端跟踪结束后，对于当前帧分别计算以下4个关键帧指标。

1）平均平方光流Mfov用于衡量视场内边缘像素的整体光度变化。

式中u为边缘像素坐标，u′为重投影后的像素坐标，n为边缘像素的数量。

2）无旋转的平均平方光流Mtran用于衡量因平移引起的光度变化。

式中ut为仅做平移变换后的像素坐标。

3）无位移的平均平方光流Mrota用于衡量因旋转引起的光度变化。

式中ur为仅做旋转变换后的像素坐标。

4）参考关键帧投影到当前帧的像素点数量Nin和未投影到当前帧的像素点数量Nout。

当以上指标符合式(20)(21)所示的条件之一时，将当前帧作为新的关键帧。

滑动窗口内的关键帧需要维持在一定的数量，因此需要采取边缘化部分关键帧的策略，关键帧的边缘化策略有两种选择方式。

1）该关键帧中的边缘像素点在优化过程中激活的数量少于设置的阈值θvis，则该关键帧被边缘化。

2）若不存在符合上述条件的关键帧，则保留最新的两帧关键帧，对其余的关键帧计算帧间的欧氏距离得分si，边缘化部分关键帧使其在空间上分布均匀。

式中,di,j为两个关键帧之间的欧氏距离。

3.4 回环检测

回环检测分为发现回环候选帧，利用回环候选帧的信息修正位姿估计，再验证回环检测是否正确。本文的回环检测是基于随机蕨词袋设计的，其流程如下。

2）利用回环信息修正位姿估计。当相似度得分大于设置的阈值θHam，则将当前帧视为回环候选帧加入到回环处理过程，以关键帧相对世界坐标系的位姿作为节点，滑动窗口关键帧之间的相对位姿作为边，构建如下的位姿图优化问题。图3(a)展示了这一过程，其中箭头方向表示将该帧投影到另一帧。

3）回环验证。将由候选回环关键帧及其邻近关键帧构成的回环验证关键帧集合V投影到当前帧，计算边缘投影重叠的数量，图3(b)展示了这一过程。为了避免重复计算，对回环验证关键帧集合中的关键帧先统计图像中边缘像素的出现次数。对于关键帧iF计算如下的计数地图：

图3 回环检测示意图Fig.3 Schematic diagram of loop detection

式中ξci为关键帧Fi到当前帧Fcur的位姿变换，N为回环验证关键帧V中关键帧数量，对于整个回环验证关键帧集合V的计数地图有：

将候选回环关键帧中具有有效深度的边缘像素点投影至计数地图，对于集合V中每一帧，生成大小为N+1的直方图来计算边缘重叠数量。如果结果符合式(28)，则通过验证。

式中，wi为权重系数。回环验证通过后更新位姿。

4 实验结果与分析

采用TUM RGB-D数据集验证本文所提出的算法。针对光度标定、误差权重系数、回环验证的改进效果，并与ORB-SLAM3[4]进行对比。该数据集包含39个室内环境的图像序列，使用Kinect相机以30 Hz的频率录制的，包含格式为BGR8的RGB图像、格式为32FC的深度图像，轨迹真值是通过具有16个高速摄像头的动作捕捉系统以100 Hz获得的，是验证单目或基于RGB-D相机的视觉里程计性能常用的数据集。

实验平台为具有16 GB运行内存，AMD Ryzen7-4800U计算处理单元的个人电脑，操作系统为Ubuntu20.04.2LTS，ROS版本为noetic。将本文提出的两种改进算法与RESLAM[14]、ORB-SLAM3[4]进行对比，验证算法性能。两种改进算法分别是集成光度在线标定利用Huber鲁棒核函数构造残差项系数的RESLAM（下文称RESLAM_PMH）、集成光度标定并且利用T分布构造残差项系数的RESLAM（下文称RESLAM_PMT）。

本文采用的评价指标为绝对轨迹误差（Absolute Trajectory Error,ATE）平移部分的均方根误差（Root Mean Squared Error,RMSE）。时间点i的绝对轨迹误差计算公式如下：

式中，S是估计位姿Pi到位姿真值Qi的刚体运动变换矩阵。本文中主要通过对比各算法的绝对轨迹误差的大小说明算法的精度高低。

为了分析集成光度标定模块后对算法性能的影响，并且为了避免随机的数值波动对评估结果的影响，本文以数据集中的freg1/xyz序列采用RESLAM、RESLAM_PMH、RESLAM_PMT、ORB-SLAM3算法分别进行了20次实验并求其绝对轨迹误差的平均值，其结果如图4所示。

图4 算法的20次运行结果Fig.4 The resultsof 20runsof thealgorithms

由图4可知，基于特征点法的ORB-SLAM3的算法性能在具有较为丰富纹理的freg1_xyz的序列上的运行结果最佳，而RESLAM_PMH、RESLAM_PMT经过光度标定后相对RESLAM都有较大的提升。且RESLAM_PMT在freg1/xyz序列上相较RESLAM和RESLAM_PMH的多次运行结果精度更高，更稳定。更进一步地展示实验组中的其中这一组轨迹及其在x、y、z各轴的对比如图5-6所示。

图5 展示了三种算法估计的freg1/xyz轨迹，可以看出RESLAM_PMT估计的轨迹在序列的大部分段与真实轨迹较为接近，直观地反映了该算法的改进对定位精度有所提升。图6展示了各算法估计的轨迹与真实轨迹的x，y，z轴运动分量对比，可知RESLAM_PMT估计的运动轨迹的各轴分量与真实轨迹更为接近，在y轴和z轴上精度提升表现得更为明显，图中横轴为以相对起始时刻的时间偏移量，起始时刻以科学计数的方式标注在横轴末尾。

图5 freg1/xyz序列轨迹Fig.5 freg1/xyz sequence trajector y

图6 x、y、z轴运动轨迹分量Fig.6 x, y,z axismotiontrajectory comp onents

对freg3/nostructure_texture_near_withloop序列进行半稠密重建，图7展示了三种算法的建图效果，序列中存在回环使得RESLAM和RESLAM_PMH的建图中红色方框内重建的海报图形有较大的重叠，而RESLAM_PMT则更为清晰。

图7 三种算法的稀疏建图Fig.7 The sparse mapping of three algorithms

在数据集中的多个序列上运行算法，其绝对轨迹误差的均方根误差如表1所示，其中的“/”表示只有部分轨迹，也即视觉定位失效时间较长。

从表1中可以看出ORB-SLAM3的算法性能在具有较多纹理的序列中保持着最佳的定位精度，但在freg3/structure_notexture_far序列中存在失效的情况。而本文所设计的RESLAM_PMH和RESLAM_PMT方法，在大多数序列上都优于原有的RESLAM方法，平均轨迹均方根误差分别由0.073 m提高到0.053 m和0.033 m，分别提高27.3%和55.3%。

表1 各算法绝对轨迹误差均方根误差(单位：m)Tab.1 Root mean square error of absolute trajectory error of each algorithm(Unit: m)

5 结 论

本文提出了一种集成在线光度标定的边缘对准视觉SLAM方法。首先集成在线光度标定设计了RESLAM_PMH算法，对图像的渐晕效应、曝光时间进行估计，并依此校正图像像素的强度，提升图像的质量；并对位姿估计中残差项的权重系数进行选择，设计了RESLAM_PMT算法，提高系统对于较大残差的抑制效果；综合提升了算法的定位精度和建图效果。

在数据集中的多个序列上进行多组对比实验，验证了集成在线光度标定后，基于边缘对准的视觉SLAM算法定位精度和建图效果均有所提升。同时与目前较为先进的基于特征点法的开源算法ORB-SLAM3进行对比，反映了基于边缘对准算法在精度上略有不足，但在较为特殊的弱纹理环境下相较特征点法具有明显优势，这为后续的研究提供了较好的借鉴。