1 引言（Introduction）

移动机器人的空间定位与感知能力是其实现自主导航、达成目标的基础。基于视觉的SLAM技术可以同时完成机器人在未知环境下的建图与定位，有效地解决机器人的环境感知与空间定位难题。其中，基于视觉的SLAM技术以更丰富的环境表述和更优秀的泛化能力等优点引起了领域内学者的广泛关注^[1]。

根据对图像信息的提取方法，可将基于视觉的SLAM方法分为3类：特征点法、直接法和半直接法^[2]。特征点法的典型代表ORB-SLAM^[3]于2015年提出，该方法首先通过ORB（oriented FAST and rotated BRIEF）特征匹配获得帧间的数据关联关系，再通过局部光束平差（BA）法对相机姿态和局部地图作进一步的优化，采用基于词袋（BoW）模型的闭环检测方法保证姿态和地图的一致性和准确性。ORB-SLAM方法^[3-5]不断更新迭代，目前的ORB-SLAM3^[5]已经成为当前最为完善的视觉SLAM系统之一，但特征点法在特征描述与处理上的计算损耗极大，因此不适于应用在实时性需求高或算力低的平台上。相比于特征点法，直接法不依赖于图像特征，而是通过最小化光度误差进行多帧间的位姿估计，如Engel等^[6]首先提出了LSD-SLAM方法，其仅使用图像中部分像素灰度，构建了一种半稠密单目视觉SLAM系统，具有较高的定位效率和大场景下的应用能力。Forster等^[7]提出一种半直接视觉里程计（SVO）。SVO首先使用直接法估计初始位姿，然后进行特征对齐获取匹配点对，最后进行与特征点法相同的BA优化。

视觉SLAM方法在光照不均匀场景下表现不佳，其根本原因是在相机不同视角下同一物体上的像素点灰度值不一致。针对此问题，人们在光度标定技术方面进行了大量研究，如文[8-9] 中采用线性的方式对多帧图像的光度参数进行联合求解，实现多幅图的全景拼接，但这些方法都专注于离线应用，算法的运行时间难以符合SLAM系统的实时性需求。为此，Bergman等^[10]又提出一种基于连续帧的在线光度标定技术，利用KLT（Kanade-Lucas-Tomasi）特征追踪法，确定场景点在连续多帧图像上的关联关系，以此建立非线性优化模型，实时地恢复与更新光度参数，对自动曝光式相机采集的灰度图像进行校正。随后结合该技术提出的DSO（直接稀疏里程计）方法^[11]，在一定程度上改善了直接法对光照的敏感程度，提高了直接法的精度与稳定性，但由于DSO方法优化时的误差函数依赖图像而不是空间点的重投影，而灰度图像的非凸性较大，同时又缺少闭环检测，因此DSO方法存在无法消除的累积误差。

目前，相对于直接法和特征点法，半直接法虽实时性较强，其建图与定位精度也达到了一定的水准，但由于沿用了直接法的光度不变假设，所以半直接法易受到环境光照的影响，在运动估计的精度及对环境的泛化能力上仍存在不足；同时，半直接法如SVO在追踪丢失时，仅选取丢失前的最后1帧与当前帧进行闭环检测，而实际运行场景中很难达成该闭环条件，因此检测成功率较低。

为进一步提高复杂光照场景下半直接SLAM算法的轨迹估计精度与鲁棒性，本文针对上述半直接法存在的不足，提出一种基于在线光度标定的半直接视觉SLAM算法。主要贡献如下：

(1) 引入光度成像模型，对输入图像进行光度参数的估计与校正，解决半直接法光照敏感问题。

(2) 提出一种多维度的关键帧选取策略，可在高效选取关键帧的同时有效避免关键帧冗余。

(3) 改进基于光束平差法的高斯－牛顿优化方法，在减少迭代次数的同时有效提升运算结果的准确度。

(4) 添加基于词袋模型^[12]的闭环检测，解决半直接方法在前端跟踪丢失时系统的重定位问题。

2 光度成像模型（Photometric imaging model）

相机能够捕获场景中光线的亮度，并转换成相应的亮度图像^[13]，其成像过程为：真实世界的点被光束照射后反射到空间中，反射后的光照为光的辐照度（radiance），镜头对场景中的辐射能量进行捕获，转换成辐射度（irradiance）^[14-15]，通过相机快门调节图像传感器接收辐射度的时长，随后经过一系列处理将光子转换成亮度值，最终得到亮度图像。

可对场景点的成像过程进行建模，建模后的相机成像过程如图 1所示。

相机的光度成像模型：

$ \begin{align} {\mathit{\boldsymbol{I}}}_{i} ({{\mathit{\boldsymbol{x}}}})={\mathit{\boldsymbol{R}}} (t_{i} {\mathit{\boldsymbol{L}}}_{i} ({{\mathit{\boldsymbol{x}}}}){{M}}_{i} ({{\mathit{\boldsymbol{x}}}})) \end{align} $

(1)

其中，$ {\mathit{\boldsymbol{I}}}_{i} ({{\mathit{\boldsymbol{x}}}}) $表示在$ i $时刻相机采集到的图像上位置为$ {\mathit{\boldsymbol{x}}} $的像素对应的灰度值；$ \mathit{\boldsymbol{R}} $为相机响应函数，$ \mathit{\boldsymbol{R}} $内每个元素都是0~255范围内的整数，其值取决于真实相机调整；$ {{M}}_{i} $为光晕因子，取值范围为$ [0, 1] $，其值取决于图像传感器在空间中的位置；$ t_{i} $为自动曝光相机的快门时间；$ {\mathit{\boldsymbol{L}}}_{i} $为辐照度图像。

视觉SLAM系统的理想观测是相机在不同位置观测到成像亮度相同的同一空间点，对式(1) 进行变换，得到不受光度成像影响的辐照度图像模型，方程表达为

$ \begin{align} {\mathit{\boldsymbol{L}}}_{i} ({{\mathit{\boldsymbol{x}}}})=\frac{{\mathit{\boldsymbol{R}}}^{-1}({\mathit{\boldsymbol{I}}}_{i} ({{\mathit{\boldsymbol{x}}}}))}{t_{i} {{M}}_{i} ({{\mathit{\boldsymbol{x}}}})} \end{align} $

(2)

2.1 光晕因子建模

假设各像素的光晕因子关于中心对称，因此在对相机光圈效应的建模中，使用径向衰减模型对该过程进行近似，得到以6阶多项式形式拟合的光晕因子函数如下：

$ \begin{align} {\mathit{\boldsymbol{M}}}({{\mathit{\boldsymbol{x}}}})=1+a_{1} \mathit{\boldsymbol{r}}^{2} ({{\mathit{\boldsymbol{x}}}})+a_{2} \mathit{\boldsymbol{r}}^{4} ({{\mathit{\boldsymbol{x}}}})+a_{3} \mathit{\boldsymbol{r}}^{6} ({{\mathit{\boldsymbol{x}}}}) \end{align} $

(3)

其中，$ \mathit{\boldsymbol{r}} ({{\mathit{\boldsymbol{x}}}}) $为图像点关于中心的归一化半径，$ a_{1} $、$ a_{2} $、$ a_{3} $为光晕系数。

2.2 响应函数建模

光子产生的曝光量经过非线性转换变为亮度值，该过程可采用经验响应（EMoR）模型进行建模，获得线性组合形式的响应函数：

$ \begin{align} {\mathit{\boldsymbol{R}}}({{\mathit{\boldsymbol{x}}}})={\mathit{\boldsymbol{f}}}_{0} ({{\mathit{\boldsymbol{x}}}})+\sum _{k=1}^n {\mathit{\boldsymbol{b}}}_{k} {\mathit{\boldsymbol{h}}}_{k} ({{\mathit{\boldsymbol{x}}}}) \end{align} $

(4)

其中，$ {\mathit{\boldsymbol{f}}}_{0} ({{\mathit{\boldsymbol{x}}}}) $为平均响应函数，$ {\mathit{\boldsymbol{h}}}_{k} ({{\mathit{\boldsymbol{x}}}}) $为基响应函数，$ {\mathit{\boldsymbol{b}}}_{k} $为响应系数向量。

2.3 曝光时间建模

由于部分自动曝光相机无法获取其准确的曝光时间，为提高本算法的适用度，通过仿射传递函数建立相机曝光时间的参数模型：

$ \begin{align} t_{i} {\mathit{\boldsymbol{L}}}_{i} ={\rm e}^{-\alpha_{i}} ({\mathit{\boldsymbol{I}}}_{i} -\beta_{i}) \end{align} $

(5)

其中$ \alpha_{i} $、$ \beta_{i} $为曝光参数，采用$ {\rm e}^{-\alpha_{i}} $指数形式防止曝光时间为负，同时可提升数学计算的效率。

3 基于在线光度标定的半直接SLAM算法（Semi-direct SLAM algorithm based on online photometric calibration）

本算法由光度校正、运动估计和闭环检测3个线程共同组成，算法流程如图 2所示。

(1) 光度校正：为确保SLAM系统前端的运行效率，采用FAST（feature from accelerated segment test）角点提取特征，引入光度成像模型，联合直接法构建最小化光度残差模型，估计光度参数和粗略的帧间位姿；待优化函数收敛后，将优化后的光度参数导入模型，从原始图像解耦得到不受光度成像影响的辐照度图像。为后续基于图像灰度的视觉SLAM算法提供稳定的输入图像。

(2) 运动估计：首先选取关键帧，从时间、空间和场景描述3个不同维度定义关键帧筛选策略，选择最优的关键帧；然后当新帧进入时，从关键帧队列选择最近的共视关键帧进行光流追踪；最后通过重投影误差优化位姿与地图点。

(3) 闭环检测：根据视觉里程计估计出的位姿，确定邻近的关键帧，分别根据关键帧的词袋向量计算这些邻近关键帧与当前帧的相似度。若成功闭环，则进行全局优化，得到全局一致的环境地图和运动轨迹。

3.1 光度校正

直接/半直接法依赖于“光度不变”这一项强假设，但在实际场景运行中由于环境光照变化、相机参数设置等因素的影响，该项假设难以满足。而光度校正是对新输入的图像作预处理，将其校正为不受相机成像影响的辐照度图像，可在假设条件不满足的情况下有效解决位姿估计精度下降、跟踪丢失等问题。

首先，通过检测帧间FAST角点^[16]的配对关系建立联合光度不变假设与光度成像模型的光度误差方程，构建基于光度误差最小的最大似然估计：

$ \begin{align} {\mathit{\boldsymbol{T}}}_{k, k-1}^{*} =\arg\min _{{{\mathit{\boldsymbol{T}}}}_{k, k-1}} \sum \frac{1}{2} \left\| \delta {\mathit{\boldsymbol{I}}}({\mathit{\boldsymbol{T}}}_{k, k-1}, {\mathit{\boldsymbol{\upsilon}}}_{k}^{\rm C}) \right\|_{2}^{2} \end{align} $

(6)

其中，$ {\mathit{\boldsymbol{T}}}_{k, k-1} $是$ k $与$ k-1 $时刻图像帧$ {\mathit{\boldsymbol{I}}}_{k} $与$ {\mathit{\boldsymbol{I}}}_{k+1} $之间的欧氏变换矩阵，$ \| \delta {\mathit{\boldsymbol{I}}}({\mathit{\boldsymbol{T}}}_{k, k-1}, {\mathit{\boldsymbol{\upsilon}}}_{k}^{\rm C}) \|_{2}^{2} $是光度残差，表示同一空间点在连续2帧图像上投影得到的像素强度差，此处采用二范数的平方确保整体误差为正值，其表达式如式(7) 所示：

$ \begin{align} {\delta} {\mathit{\boldsymbol{I}}}({\mathit{\boldsymbol{T}}}_{k, k-1}, {\mathit{\boldsymbol{\upsilon}}}_{k}^{\rm C})=& t_{k} {\mathit{\boldsymbol{L}}}_{k} ({\mathit{\boldsymbol{\pi}}} ({\mathit{\boldsymbol{T}}}_{\rm CB} {\mathit{\boldsymbol{T}}}_{k, k-1}, {\mathit{\boldsymbol{\rho}}}))- \\ & {\rm e}^{-\alpha_{k-1}} \frac{{\mathit{\boldsymbol{R}}}^{-1} ({\mathit{\boldsymbol{I}}}_{j}^{k-1} ({\mathit{\boldsymbol{T}}}_{\rm CB} {\mathit{\boldsymbol{\rho}}})-\beta_{k-1})}{{{M}}_{k-1}({{\mathit{\boldsymbol{x}}}})} \end{align} $

(7)

其中，$ {\mathit{\boldsymbol{T}}}_{\rm CB} $为机器人车身坐标系$ \varSigma_{\rm B} $变换到相机坐标系$ \varSigma_{\rm C} $的外参矩阵，$ {\mathit{\boldsymbol{\pi}}} $表示从世界坐标系下的空间点坐标$ {\mathit{\boldsymbol{\rho}}} $转换到图像坐标系下的像素点坐标$ {\mathit{\boldsymbol{u}}} $之间的相机投影矩阵：

$ \begin{align} {\mathit{\boldsymbol{\rho}}} ={\mathit{\boldsymbol{T}}}_{\rm BC} {\mathit{\boldsymbol{\pi}}}^{-1}({\mathit{\boldsymbol{u}}}) \end{align} $

(8)

$ {\mathit{\boldsymbol{\upsilon}}}_{k}^{\rm C} $为待优化的状态向量，其包含内容如式(9) 所示：

$ \begin{align} \begin{cases} {\mathit{\boldsymbol{a}}}_{k}^{\rm M} =[a_{1}, a_{2}, a_{3}]\\ {\mathit{\boldsymbol{b}}}_{k}^{\rm R} =[b_{1}, b_{2}, b_{3}, b_{4}] \\ {\mathit{\boldsymbol{\upsilon}}}_{k}^{\rm C} =[{\mathit{\boldsymbol{a}}}_{k}^{\rm M}, {\mathit{\boldsymbol{b}}}_{k}^{\rm R}, \alpha_{k}, \beta_{k}] \end{cases} \end{align} $

(9)

其中，$ {\mathit{\boldsymbol{a}}}_{k}^{\rm M} $、$ {\mathit{\boldsymbol{b}}}_{k}^{\rm R} $分别对应式(3)(4) 中的光晕系数和响应系数向量，$ \alpha_{k} $、$ \beta_{k} $对应式(5) 中的曝光系数。

由于式(6) 中的目标函数为非线性函数，采用高斯－牛顿法迭代求解，估计相机位姿与光度参数。

$ \begin{align} \mathit{\boldsymbol{H}}& = \sum _{i=1}^N {{\mathit{\boldsymbol{J}}}_{i}^{\rm T} {\mathit{\boldsymbol{J}}}_{i}}, \; 并且 \; {\mathit{\boldsymbol{b}}}=-\sum _{i=1}^N {{\mathit{\boldsymbol{J}}_{i}}{\delta} {\mathit{\boldsymbol{I}}}({\mathit{\boldsymbol{T}}}_{k, k-1}, {\mathit{\boldsymbol{\upsilon}}_{k}^{\rm C}})} \\ {\mathit{\boldsymbol{\zeta}}}^{*} & =-{\mathit{\boldsymbol{H}}}^{-1}{\mathit{\boldsymbol{b}}} \end{align} $

(10)

其中，$ N $为观测点产生的残差个数，$ {\mathit{\boldsymbol{J}}}_{i} $为第$ i $个残差项关于状态量的雅可比矩阵，每次迭代结束后，更新估计值$ {\mathit{\boldsymbol{T}}}_{k, k-1} $，$ {\mathit{\boldsymbol{\zeta}}}^{*} $为高斯迭代增量。光度参数在迭代中不断更新直至收敛，每当新的图像帧抵达时，以当前光度参数的估计结果对原始图像进行光度校正，即根据式(2) 获取辐照度图像。这样可以保证相机在运动过程中捕获到的同一物体的灰度值一致，保障特征追踪功能的鲁棒运行。最后，优化变量，将求解出的位姿作为后端优化的初值，为重投影优化求解的稳定性提供保障。

3.2 位姿估计与建图 3.2.1 关键帧选取

利用关键帧代表其邻近帧，可有效减少待优化的帧数，因此关键帧的选取尤为重要。本文基于半直接视觉里程计，视觉前端的特征追踪和后端位姿、地图优化严格依赖于关键帧，选取合适的关键帧可有效提高算法的计算效率。

关键帧选取的原则是选择的关键帧能够较全面地代表邻近的局部帧，以使选用的关键帧更符合本算法的特性。本文融合时间描述、空间描述、场景描述3种维度共同判断是否加入关键帧，判定条件为：

(1) 当前帧与上一关键帧的时间间隔大于阈值$ \tau_{\rm{stamp}} $；

(2) 当前帧位姿与上一关键帧的位姿变化大于阈值$ {\mathit{\boldsymbol{N}}}_{1} $；

(3) 当前光流追踪到的内点数量与上一帧特征点总数的比值小于阈值$ K_{\rm{per}} $；

(4) 当前帧与上一帧的位姿变化量大于阈值$ {\mathit{\boldsymbol{N}}}_{2} $，但与其他关键帧具有较多共视点。

当同时满足第(1)(2) 项条件时，选取当前帧为关键帧，以避免关键帧选取的冗余情况；当第(3) 项或第(4) 项条件满足任意一项时，提取新的关键帧。

3.2.2 特征追踪

用直接法计算帧间位姿，存在无法消除的累积误差；特征点法常采用光束平差的优化方式消除该项误差，且具有较高的精度。本文将采用特征点法的优化模型进行后端优化，而该优化方式重投影模型的建立需依靠帧间匹配点的对应关系。

本节特征追踪环节依靠校正后的辐照度图像，结合光流法与最近共视关键帧匹配原则，用于查找出连续帧上的高质量匹配点。

最近共视关键帧的匹配原理如图 3所示。首先求出当前帧与关键帧之间的欧氏距离，找到距离最接近的共视关键帧；然后采用光流法在辐照度图像上进行特征点的跟踪，获得精确的匹配点。

光流法通过最小化当前帧中图像块与参考帧中图像块的灰度差构建优化方程，方程的表现形式为

$ \begin{align} {\mathit{\boldsymbol{w}}}_{i}^{*} =\arg\min _{{{\mathit{\boldsymbol{w}}}}_{i}^{*}} \frac{1}{2}\| {{\mathit{\boldsymbol{L}}}_{\rm k} ({\mathit{\boldsymbol{w}}}_{i}')-{\mathit{\boldsymbol{L}}}_{\rm r} ({\mathit{\boldsymbol{A}}}_{i} \cdot {\mathit{\boldsymbol{w}}}_{i})} \|^{2}, \; \; \; \forall i \end{align} $

(11)

其中，$ {\mathit{\boldsymbol{w}}}_{i}^{*} $为特征点在当前帧图像中的最佳位置，$ {\mathit{\boldsymbol{L}}}_{\rm k} ({\mathit{\boldsymbol{w}}}_{i}') $是辐照度图像上以像素点为中心的图像块（8 $ \times $ 8）灰度，$ {\mathit{\boldsymbol{L}}}_{\rm r} ({\mathit{\boldsymbol{A}}}_{i} \cdot {\mathit{\boldsymbol{w}}}_{i}) $为参考帧辐照度图像上特征块经过仿射变换后的灰度，$ {\mathit{\boldsymbol{A}}}_{i} $为仿射变换矩阵，用于修正图像块的正确投影位置，最小化光度误差。

该方法可最大程度地降低关键帧与当前帧之间的视角差，并且使特征图块的形变最小，从而实现更精准的帧间匹配。

3.2.3 位姿与地图点优化

由于辐照度图像不受相机光学成像影响，因此经过上述步骤能够获取准确、稳定的帧间匹配点，根据空间点的位置与其被观测的投影位置建立重投影方程，以此优化相机位姿与地图点。其优化的本质是通过最小化观测的先验残差，获得最大的后验估计：

$ \begin{align} \min _{{\mathit{\boldsymbol{\chi}}}} \sum _{d\in {D}} \sum _{j\in {F}} {\omega} \left( \left\| {{\mathit{\boldsymbol{r}}}_{j}^{d} (\bar{\mathit{\boldsymbol{z}}}_{j}^{d}, {\mathit{\boldsymbol{\chi}}})} \right\|^{2}\right) \end{align} $

(12)

其中，$ D $是地图上所有关键帧的集合，$ F $是当前帧被观测到至少2次的特征点的集合，$ \bar{\mathit{\boldsymbol{z}}}_{j}^{d} $表示第$ j $个特征点在关键帧$ d $上的观测值，$ {\mathit{\boldsymbol{r}}}_{j}^{d} $为视觉观测的重投影残差，其定义为

$ \begin{align} {\mathit{\boldsymbol{r}}}_{j}^{d} (\bar{\mathit{\boldsymbol{z}}}_{j}^{d}, {\mathit{\boldsymbol{\chi}}})={\mathit{\boldsymbol{L}}}_{j} ({\mathit{\boldsymbol{u}}}_{j}^{d})-{\mathit{\boldsymbol{\pi}}} ({\mathit{\boldsymbol{T}}}({\mathit{\boldsymbol{\chi}}}){\mathit{\boldsymbol{T}}}_{\rm CB} {\mathit{\boldsymbol{p}}}) \end{align} $

(13)

其中，$ {\mathit{\boldsymbol{L}}}_{j} ({\mathit{\boldsymbol{u}}}_{j}^{d}) $为辐照度图像上的观测点，$ {\mathit{\boldsymbol{\chi}}} $为相机位姿，$ {\mathit{\boldsymbol{p}}} $为与观测点对应的地图点。

式(12) 中的Huber核函数$ {{\omega}} $定义如下：

$ \begin{align} {{\omega}} (s) = \begin{cases} 1, & {s\geqslant \iota} \\ 2\sqrt{s}-1, & {s<\iota} \end{cases} \end{align} $

(14)

其中$ s $为导入核函数的误差量，$ \iota $为阈值，考虑到图像由单位像素构成，此处阈值设置为1。

使用迭代优化方法对重投影方程进行求解，由于该非线性方程非凸且有观测噪声的存在，迭代优化过程中容易陷入局部最优，难以足够准确地保证算法收敛。本文对常规的高斯－牛顿重投影优化方法进行改进，在确保优化过程能顺利收敛的同时，保证了优化结果的准确性。本文对求解重投影优化问题的常规方法高斯－牛顿法进行改进，确保优化过程能够顺利收敛，并且保证了优化结果的准确性。

本文的重投影优化算法伪代码如算法1所示。

首先将3.1节求解出的位姿作为迭代优化的初始值，分别计算误差项$ f({\mathit{\boldsymbol{\chi}}}^{d}) $和重投影残差的雅可比矩阵$ {\mathit{\boldsymbol{J}}}({\mathit{\boldsymbol{\chi}}}^{d}) $，以此不断迭代求解增量方程，如果在此期间更新量足够小，则判定为抵达最优点，循环终止；然后，根据优化后的位姿计算重投影，统计重投影误差小于或等于1个像素点的匹配点的数量；最后，若所统计的匹配点数量大于阈值$ N_{\rm C} $，则判定优化成功，将结果作为优化结果更新到轨迹上。

算法1：重投影迭代优化方法

输入: 辐射度图像${\mathit{\boldsymbol{L}}}({\mathit{\boldsymbol{u}}}_{j}^{d})$、特征点集$F$、地图点集$P$ 输出: 优化后的相机位姿$\mathit{\boldsymbol{T}}$、优化后的地图点集${P}$ 1: $P\leftarrow $ PMArray[n].size;        % 创建待优化地图点 2: for$ d\leftarrow $ 1 to 15 do 3:     $ {\mathit{\boldsymbol{J}}}_{d} \leftarrow {\mathit{\boldsymbol{J}}}({\mathit{\boldsymbol{\chi}}}^{d}) $ 4:     $ {\mathit{\boldsymbol{\xi}}} \leftarrow ({\mathit{\boldsymbol{J}}}_{_{d}}^{\rm T} {\mathit{\boldsymbol{J}}}_{d})^{-1}{\mathit{\boldsymbol{J}}}_{d}^{\rm T} \cdot f({\mathit{\boldsymbol{\chi}}}^{d}) $ 5:     if $\mathit{\boldsymbol{\xi}} \approx 0 $ then 6:         break; 7:     else 8:         ${\mathit{\boldsymbol{\chi}}}^{d+1} \leftarrow {\mathit{\boldsymbol{\chi}}}^{d} +{\mathit{\boldsymbol{\xi}}}$ % 更新增量 9:     end if 10: end for 11: $ {\mathit{\boldsymbol{T}}}\leftarrow {\mathit{\boldsymbol{\chi}}}^{d} +{\mathit{\boldsymbol{\xi}}}$ 12: 创建向量${\mathit{\boldsymbol{V}}}_{1}$; 13: for each $ f_{P_{i}}$ in $F$ do       %$f_{P_{i}}$为特征点集$F$内的点 14:     if (Error($ f_{P_{i}}, P_{i}$) $<$ 2) then %Error为重投影误差 15:         push $ f_{P_{i}}$ to ${\mathit{\boldsymbol{V}}}_{1}$; 16:     endif 17: endfor 18: if number of $\mathit{\boldsymbol{V}}_{1} \geqslant N_{\rm C}$ then       %判定结果是否准确 19:     return ${\mathit{\boldsymbol{T}}}$, $P$; 20: else 21:     return false; 22: endif

3.3 闭环检测

里程计不可避免地存在累积误差，而闭环检测是消除累积误差的有效方法。当相机运动到曾经抵达过的地方时，可以对全局的相机位姿与地图点进行纠正。本文基于词袋模型进行闭环检测。首先搜索邻近关键帧，根据词袋模型对不同关键帧之间的相似度进行计算，判断是否存在闭环。若存在闭环，执行基于当前帧与关键帧Sim3相似变换的位姿图优化，优化全局关键帧位姿与地图点。

4 实验与分析（Experiment and analysis）

本文在SVO方法的基础上融合在线光度标定技术，提出复杂光照环境下更鲁棒的半直接视觉SLAM方法，为验证算法性能，进行了以下实验：(1) 验证本算法在线光度标定结果的准确性；(2) 验证本算法相对于SVO方法在轨迹估计精度方面的提升；(3) 对比本算法与目前主流的视觉SLAM方法的性能；(4) 验证真实场景下的精度。在PC平台进行测试，配置为i7-9750H CPU、2.6 GHz主频、8G内存，未使用GPU加速，系统为Ubuntu 16.04。

4.1 光度标定结果验证 4.1.1 光度参数估计实验

TUM Mono VO^[17]数据集提供多种不同环境的图像序列，同时保留了相机的曝光时间真值与准确标定出的响应函数系数与光晕系数（可视为真值）。为验证本算法在光度参数标定方面的性能提升，该部分采用本文算法与文[10] 算法优化的光度参数以及TUM数据集中提供的光度真值进行对比，实验结果如图 4所示。

图中红色虚线为数据集提供的真值，通过对比2种方法相对于真实值的近似程度，可以看出2种方法对于响应函数和曝光时间两种光度参数的估计效果近似，均取得了较好的估计效果。而在光晕系数的估计方面，由图 4(a)可见本算法取得了更好的拟合效果，根本原因是文[10] 方法使用光流追踪的方式确定场景点关联，而这种方式易出现错误的关联点，导致非线性优化精度下降。而本算法通过最近共视关键帧的方式进行跟踪，极好地满足了光流法的小运动假设。同时，算法后端的优化系统会对一些错误的场景点进行剔除，进一步保证了光度参数估计时，具有更加正确的约束项。可见本算法恢复出的光度参数更能准确地逼近真值，具有较高质量的估计效果。

4.1.2 光度校正实验

EuRoC数据集^[18]记录了室内环境下的传感器数据序列，序列中包含了相机采集可能会面临的光照复杂、运动模糊等情况。为更直观地验证本算法能够有效应对光度扰动，能够改善传统视觉方法对于光度恒定假设的过强依赖性，针对EuRoC数据集中的复杂光照数据序列进行了对比验证实验，使用算法估计出的光度参数对原始图像进行光度校正，对比校正前后的图像。其中典型的困难序列“V1\_03\_difficult”实验结果如图 5所示。

图 5(a)截取了序列中帧ID为145~215的一段连续图像序列，算法在连续帧上不断更新优化光晕参数、响应函数参数和曝光时间的估计值，利用这些估计出的参数对原始图像进行校正，图 5(b)为校正后的图像，从中可明显观察到(b) 组经过校正后的连续帧图像的灰度基本保持一致，利于视觉特征点的追踪、后端重投影优化和闭环检测。

由于光度参数更新环节不需额外提取特征点，而是直接使用SLAM算法前端追踪的特征，因此节省了额外的时间，大大提高了算法的效率。

4.2 半直接法定位性能优化对比

为更进一步验证本算法对于传统半直接SLAM算法（SVO）在定位精度方面的提升效果，分别选择室内、室外公开数据集对改进前后的两种算法进行对比实验，对本算法轨迹估计精度进行评估验证。

1) 室内场景实验

选择EuRoC数据集中的多项不同难易程度的序列开展综合性能测试，利用左目图像序列运行算法，同时将本算法的轨迹结果、SVO 2.0算法^[19]的轨迹估计结果与轨迹真值进行尺度对齐，实验中本算法将后端优化迭代次数的最大值设置为15，优化成功的判定阈值$ N_{\rm C} $设置为20。本项对比实验分别在2个简单数据集序列“MH_01_easy”和“MH_02_easy”，在中等难度序列“MH_03_medium”以及高难度序列“MH_04_difficult”上使用EVO（evaluation of odometry and SLAM）工具对轨迹结果进行了10次反复评估。文中以误差项的最大值为指标，来衡量SLAM算法的最大受影响程度（本实验数据集的影响情况为光照骤变）。经过计算，本算法在“medium”和“difficult”2个数据集序列上效果的提升极为明显，相较于改进前的方法，绝对轨迹误差的最大值分别降低了1.182 m和0.624 m。

由图 6可得，在光度变化较小、相机运动较为平稳的“easy”序列中，基于半直接法的SVO 2.0算法与本算法均有较好的轨迹精度。而在“medium”和“difficult”序列中，由于曝光变化较大，SVO 2.0算法得到的轨迹出现了较大的偏差，而本算法由于对图像光度进行了校正，有较高的定位精度。

2) 室外场景实验

TUM Mono VO数据集包含室内室外环境、静态动态场景的50个单目序列。选择其中的室外序列进行实验，所选的测试数据集采集相机在拍摄起始和结束的位置一致，可用于长时间轨迹估计的累积漂移评估和闭环检测效果方面的评估。

视觉里程计方法不可避免地会出现累积漂移，为验证本算法相对于半直接方法在里程计精度方面的提升效果，将本算法闭环检测关闭前后的两条运行轨迹与SVO 2.0算法得到的轨迹进行对比，图 7为在序列“sequence_29”上的实验结果。

显然，本算法由于融合了在线光度校正的位姿估计方法，其轨迹估计结果明显优于传统半直接法。而开启闭环检测功能后，本算法在数据集序列末尾（即采样相机回到初始位置时），成功进行了闭环检测，对历史地图点及轨迹进行了校正调整，取得了最佳的轨迹估计效果。

4.3 主流SLAM方法对比实验 4.3.1 轨迹估计精度对比

为更进一步验证本算法的综合性能，与当前主流SLAM方法进行对比。分别对SVO2.0^[19]、VINS^[20]、DSO^[11]、LSD^[6]、ORB-SLAM2^[4]和本算法进行精度评估实验，公平起见，上述算法的测试参数均采用算法开源时的默认值，而本算法的参数设置在不同的数据集实验中保持一致，使用绝对轨迹的均方根误差（RMSE）进行对比，结果如表 1所示，其中“$ \times $”表示跟踪失败。

经对比，本算法在绝大部分数据集序列中的表现相较于SVO 2.0算法有明显的优势，尤其在中等难度、困难数据集的测试结果中，LSD与SVO 2.0算法的误差较大，甚至出现跟踪失败导致整个算法终止运行的情况，而本算法仍能在这些数据集中保持较高的稳定性与精度；同时相较于其他主流的视觉SLAM算法，本算法同样也表现出一定优势。

4.3.2 运行时间对比

本算法在半直接SVO 2.0算法的基础上融合了光度标定，采用非线性优化方式对这些光度参数进行估计，而这无疑会导致计算成本的增加。为评估算法效率，对比本算法与其他方法的运行时间，取时间中值和平均频率进行比较，结果见表 2。

表 1和表 2的实验结果表明，本算法与SVO 2.0算法相比，而虽然时间成本增加了73.26%，但算法的精度提升为原来的230.7%，并且算法仍然以60 Hz的频率运行，时间效率很高。

4.4 实际场景测试验证

在真实场景下对本算法进行验证。选择配备Kinect v1相机和RPLIDAR-A2激光雷达的TurtleBot3机器人作为实验平台，场景选择实验楼大厅，实验平台和场景如图 8所示，场景内存在较多的光源和地面反射光，属复杂光照场景。实验过程中，使用安装在实验机器人上的相机捕获图像信息；同时，获取激光雷达的点云信息。

利用机器人上搭载的激光雷达运行Cartographer算法，并将得到的轨迹结果与本算法的轨迹估计结果进行对比，结果如图 9所示。

将2条轨迹在时间戳上进行同步，同时分别在$ X $与$ Y $方向上计算相同时序上2种轨迹间的绝对轨迹均方根误差，获得相对误差曲线，如图 10所示。

由图 9结果可知，本算法估计出的轨迹与激光SLAM方法估计出的轨迹基本吻合，而通过观察图 10两个方向上的相对误差曲线，易看出本算法结果与激光方法的结果相对误差整体偏小，最大相对误差为0.071 m。说明本算法在场景亮度不连续的情况下仍能保证SLAM系统的鲁棒性，且具有较高的定位精度。

5 结论（Conclusion）

提出了一种基于在线光度标定的半直接视觉SLAM算法。该方法将在线光度标定融入半直接SLAM方法中，在估计位姿的同时也对光度校正参数进行估计，对原始图像进行光度校正；采用基于最近共视关键帧配准的特征追踪方法和多维度的关键帧选取策略提高特征点和关键帧质量；后端优化中对基于光束平差法的高斯－牛顿优化策略进行改进；使用基于词袋模型的方法进行回环检测。实验结果表明，本算法不仅有效地提高了半直接法的轨迹估计精度与稳定性，而且相较于其他视觉SLAM算法也具有较大优势。

本算法有效提高了视觉SLAM方法在光照不均匀场景下的同时定位与建图性能。而在复杂光照场景中，本算法在极端情况下（如图像过曝、光骤变）的表现仍有进一步提升的空间。因此，下一步的工作将尝试研究建模更完整、环境鲁棒性更优的光度模型，进一步提高SLAM系统位姿估计精度。