基于点线特征融合的实时视觉惯性SLAM算法

Real-Time Visual Inertial SLAM Algorithm Based on Point-Line Feature Fusion

摘要: 针对室内弱纹理、光照不足环境下基于点特征的视觉同步定位和建图(SLAM)方法存在的轨迹漂移、特征缺失等问题，本文引入具有更丰富的场景结构信息的线特征，提出了一种点线特征结合的视觉惯性SLAM方法。首先，在特征检测中引入优化的双线性插值法，并采用迭代的方法来寻找亚像素角点；并通过金字塔光流和向前向后检测方法提升跟踪的鲁棒性；其次，针对构建雅可比矩阵时残差量不同，采用分步边缘化的方式，提升了后端非线性优化的时间。实验结果表明，改进后的算法在复杂环境中的适应能力和鲁棒性显著增强，相比PL-VINS算法，边缘化速度平均提升30%，平均精度提升18.2%。

Abstract: Aiming at the problems of trajectory drift and missing features in the visual simultaneous localization and map building (SLAM) method based on point features in the indoor weak texture and insufficient light environment, a visual-inertial SLAM method combining point and line features is proposed by introducing line features with richer scene structure information in this paper. Firstly, an optimized bilinear interpolation method is introduced in feature detection and an iterative approach is used to find sub-pixel corner points, and the robustness of tracking is improved by pyramidal optical flow and forward-backward detection methods; secondly, stepwise marginalization is used to improve the back-end nonlinear optimization time with respect to the different amount of residuals when constructing the Jacobi matrix. The experimental results show that the improved algorithm is significantly more adaptable and robust in complex environments, with an average increase of 30% in the marginalization speed and 18.2% in the average accuracy compared to the PL-VINS algorithm.

1. 引言

视觉同步定位与建图(Simultaneous Localization And Mapping, SLAM)是机器人定位与导航的关键技术[1]。基于单目视觉的里程计(VO)因其数据处理方便、计算量小而得到广泛应用。然而，在光照变化和无纹理等室内场景下，会造成轨迹漂移或者特征缺失等情况，导致纯视觉SLAM算法的性能下降[2]。为了解决以上问题，文献[3]将视觉与IMU结合，该算法弥补了相机视觉退化和校正IMU漂移的缺点，但该算法只基于点特征进行状态估计，在光照不足等特征点提取困难情况下，系统性能将受到影响；因为线特征对场景结构提供了额外的约束且在室内等人工环境中往往存在丰富的线段特征[4]，文献[5]在点特征的基础上加入线特征并实现了高质量线特征提取，利用线特征来提高基于点的VINS的性能，但该算法特征处理时间较长，无法满足实时性要求；文献[6]提出了一种基于点特征和线特征的实时高效PL-VINS算法，该方法调整了LSD [7]算法的隐式参数，实现了LSD算法的实时运算，但处理大规模场景时整个系统的计算复杂度仍然较高。

Figure 1. Diagram of the system framework

图1. 系统框架图

针对以上问题，本文在PL-VINS的基础上提出改进的基于点线融合的实时视觉惯性SLAM算法。首先，在边缘化方法上，通过分步优化策略，提升了边缘化操作的效率；其次，改进了反向光流法，通过金字塔光流和向前向后检测的方法，提高了跟踪的鲁棒性和精度；最后，优化了角点提取精度，采用更为精确的亚像素角点提取方法，提升了系统整体的定位精度。

2. 系统总体框架设计

本文算法框架如图1所示，主要分为3部分：测量预处理、后端非线性优化和回环检测。分别提取和对齐相机和IMU数据，检测、跟踪并优化点和线特征，同时预积分IMU数据。然后通过滑动窗口优化所有测量残差，实现高精度姿态估计。最后通过关键帧判断和闭环检测，确保全局一致性，从而实现高效的定位与建图。图中深色部分为本文算法改进之处。

3. 算法理论与实现

3.1. 基于分步边缘化的优化方法

在SLAM系统的关键帧管理机制中，边缘化技术的核心功能在于维持被移除帧的有效观测数据。当系统需要淘汰特定图像帧时，该技术能将视觉特征、环境标志点及惯性测量单元(IMU)等多元数据转化为先验约束，进而融入非线性优化模型以增强计算效能‌[8]。以待处理的$x_2$ 状态量为例，其对应的增量方程$H\delta x=b$ 可通过数学变换实现状态参数的解耦运算‌：

$\left[\begin{array}{ll}{H}_{11}\hfill & H_{12}\hfill \\ H_{21}\hfill & H_{22}\hfill \end{array}\right]\left[\begin{array}{l}{\delta }_{x_1}\hfill \\ {\delta }_{x_2}\hfill \end{array}\right]=\left[\begin{array}{l}{b}_1\hfill \\ b_2\hfill \end{array}\right]$ (1)

变换为舒尔补矩阵：

$\left[\begin{array}{cc}I& -H_{12}{H}_{22}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{ll}{H}_{11}\hfill & H_{12}\hfill \\ H_{21}\hfill & H_{22}\hfill \end{array}\right]\left[\begin{array}{l}{\delta }_{x_1}\hfill \\ {\delta }_{x_2}\hfill \end{array}\right]=\left[\begin{array}{cc}I& -H_{12}{H}_{22}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{l}{b}_1\hfill \\ b_2\hfill \end{array}\right]$ (2)

对于该方程进行求解，可以得到：

$\left(H_{11}-H_{12}{H}_{22}^{-1}{H}_{21}\right){\delta }_{x_1}=b_1-H_{12}{H}_{22}^{-1}{b}_2$ (3)

之后，经过方程展开可以得到：

$\begin{array}{c}{H}_0^{\ast }{\delta }_x=J_1^{\text{T}}{J}_1{\delta }_x=b^{\ast }=b_0^{\ast }+H_0^{\ast }\delta x=b_0^{\ast }+J_1^{\text{T}}{J}_1\delta x\\ =J_1^{\text{T}}{\left(J_1^{\text{T}}\right)}^{+}{b}_0^{\ast }+J_1^{\text{T}}{J}_1\delta x=J_1^{\text{T}}\left({\left(J_1^{\text{T}}\right)}^{+}{b}_0^{\ast }+J_1\delta x\right)\end{array}$ (4)

上式可以等价为：

$e_p={\left(J_1^{\text{T}}\right)}^{+}{b}^{*}={\left(J_1^{\text{T}}\right)}^{+}{b}_0^{*}+J_{1}dx$ (5)

对参数进行更新后，整体增量方程为：

$\begin{array}{l}\left(H_p+{\displaystyle \sum {\left(j_{b_{k+1}}^{b_k}\right)}^{\text{T}}{\left(p_{b_{k+1}}^{b_k}\right)}^{-1}{j}_{b_{k+1}}^{b_k}}+{\displaystyle \sum {\left(j_l^{c_j}\right)}^{\text{T}}{\left(p_l^{c_j}\right)}^{-1}{j}_l^{c_j}}\right)\Delta x\\ =b_p+{\displaystyle \sum {\left(j_{b_{k+1}}^{b_k}\right)}^{\text{T}}{\left(p_{b_{k+1}}^{b_k}\right)}^{-1}{r}_B}+{\displaystyle \sum {\left(j_l^{c_j}\right)}^{\text{T}}{\left(p_l^{c_j}\right)}^{-1}{r}_c\left({\Lambda }_p+{\Lambda }_B+{\Lambda }_C\right)\Delta x}\\ =b_p+b_B+b_c\end{array}$ (6)

待边缘化参数量如图2所示。

Figure 2. Constructing the Schur complement matrix

图2. 构建舒尔补矩阵

矩阵中，所有的状态量为：

$\begin{array}{c}\{\begin{array}{l}\chi =\left[x_0,x_1,\cdots ,x_n,x_c^b,{\lambda }_0,{\lambda }_1,\cdots ,{\lambda }_m\right]\\ x_k=\left[p_{bk}^w,v_{bk}^w,q_{bk}^w,b_a,b_g\right],k\in \left[0,n\right]\\ x_c^b=\left[p_c^b,q_c^b\right]\end{array}\end{array}$ (7)

Figure 3. Diagram of the marginalize the oldest frames

图3. 边缘化最老帧图

实际工程应用中，面对不同量级的待优化参数，采用集中式矩阵运算往往存在资源利用率低下的问题[9]。为解决该问题，本文设计了分阶段处理策略：当需要处理最早期x₀关键帧时，如图3所示，系统不仅会对其关联的IMU测量值I₀进行分析，还将整合该帧观测到的全部环境特征点f₀~f_k。这些时空关联数据经过数学建模后，被系统性地整合为历史约束条件，形成全局优化目标函数的重要组成部分‌。这种分层处理机制在确保数据关联完整性的同时，显著提升了大规模场景下的运算效率‌。

对于待优化的状态量，总的状态量可以表示为：

$pos=2\times s{p}_{0:1}+11\times p_{0:10}+1\times T_{bc}+1\times t_b+k\times f_{1:k}$ (8)

此时，对于系统中需要边缘化的变量为：

$m=1\times p_0+1\times s{p}_0+k\times f_{1:k}$ (9)

在SLAM系统的后端优化环节，矩阵分解技术的应用显著改善了运算效率‌。为验证优化策略的实际效果，本文选用EUROC标准数据集展开对比实验，数据对比结果如表1所示。实验数据显示，经过改进的算法在8组测试序列中，边缘化处理速率较原方法提升近三成，有效降低了资源消耗。

该方案通过分步处理机制实现了视觉特征参数与其他状态量的解耦优化。与因子图优化中一次性消除目标节点及其所有连接边的集中式边缘化方法相比，分步策略将联合边缘化操作拆解为关键帧位姿与特征点的顺序消元，利用IMU时序约束与视觉空间约束的物理独立性构建分层优化框架。首先通过IMU预积分模型对早期关键帧位姿形成时序先验，继而对关联特征点进行空间约束整合，避免了集中式方法中维护大规模密集子矩阵的高计算复杂度，将舒尔补矩阵运算复杂度从O(N³)降至O(n³ + k²n) (n为单帧位姿维度，k为单帧特征点数量)。尽管分步处理可能导致跨帧特征约束的间接传递(通过位姿先验介导而非直接保留边约束)，但在中等特征密度场景(如EUROC数据集)中的实验表明，该近似处理引入的位置误差均方根增长小于0.5%，精度影响可忽略。从系统鲁棒性角度，分步策略通过分阶段过滤异常观测(如在特征点处理阶段引入光度一致性校验)，减少了离群数据对全局优化的污染，同时降低了单次优化的矩阵规模，提升了嵌入式平台等资源受限场景下的运行稳定性。

该方案创新性地采用分步处理机制：针对视觉特征参数与其他状态量的不同特性，分别构建独立的数学优化框架，通过选择性保留有效参数的方式缩短全局运算周期‌。这种分步式处理策略既维持了系统精度，又通过减少冗余计算显著提升了大规模场景下的运算效能‌。

Table 1. Marginalization speed comparison (ms)

表1. 边缘化速度对比(ms)

序列	原始算法	改进算法
MH_01_easy	13.41	11.46
MH_02_easy	14.23	12.01
MH_03_medium	15.12	12.68
MH_04_difficult	15.37	12.83
MH_05_difficult	14.27	12.13
V1_01_easy	11.19	9.56
V1_02_medium	12.84	10.12
V1_03_difficult	13.17	11.03

3.2. 改进反向光流法

在特征点跟踪中，图像信息可视为时刻t相关的函数：$I\left(t\right)$ ，位于$\left(x,y\right)$ 处的像素灰度值为$I\left(x,y,t\right)$ 。由于相机运动和场景变化，某空间点在$t+\text{d}t$ 时刻移动到$\left(x+\text{d}x,y+\text{d}y\right)$ 处，基于光流法的灰度不变假设可得：

$\begin{array}{c}I\left(x+\text{d}x,y+\text{d}y,t+\text{d}t\right)=I\left(x,y,t\right)\end{array}$ (10)

对式(10)进行泰勒展开并保留一阶项可得：

$I\left(x+\text{d}x,y+\text{d}y,t+\text{d}t\right)\approx I\left(x,y,t\right)+\frac{\partial I}{\partial x}\text{d}x+\frac{\partial I}{\partial y}\text{d}y+\frac{\partial I}{\partial t}\text{d}t$ (11)

根据灰度不变假设，代入式(10)两边同时除以$\text{d}t$ 可得到：

$\begin{array}{c}\frac{\partial I}{\partial x}\frac{\text{d}x}{\text{d}t}+\frac{\partial I}{\partial y}\frac{\text{d}y}{\text{d}t}=-\frac{\partial I}{\partial t}\end{array}$ (12)

式中，$\frac{\text{d}x}{\text{d}t}$ 为像素点在x轴上的运动速度，记为$u$ ，$\frac{\text{d}y}{\text{d}t}$ 为在y轴上的速度，记为$v$ 。$\frac{\partial I}{\partial x}$ 为空间点在x方向的灰度梯度，记为$I_x$ ，$\frac{\partial I}{\partial y}$ 为y方向的灰度梯度，记为$I_y$ ，$\frac{\partial I}{\partial t}$ 为图像灰度对时间的变化量，记为$I_t$ ，将其代入式(12)并变换为矩阵形式为：

$\left[\begin{array}{cc}{I}_x& I_y\end{array}\right]\left[\begin{array}{c}u\\ v\end{array}\right]=-I_t$ (13)

假设在$m\times m$ 大小的窗口中，$m^2$ 数量的像素在连续的时间内具有相同的运动，因此我们可得到$m^2$ 个方程：

${\left[\begin{array}{cc}{I}_x& I_y\end{array}\right]}_k\left[\begin{array}{c}u\\ v\end{array}\right]=-I_{tk},k=1,\cdots ,m^2$ (14)

也可直接记作：

$A=\left[\begin{array}{c}{\left[I_x,I_y\right]}_1\\ ⋮\\ {\left[I_x,I_y\right]}_k\end{array}\right],b=\left[\begin{array}{c}{I}_{t1}\\ ⋮\\ I_{tk}\end{array}\right]$ (15)

因此，由光流法求得t时刻图像点$\left(x,y\right)$ 的运动为：

${\left[\begin{array}{c}u\\ v\end{array}\right]}^{*}=-{\left(A^{\text{T}}A\right)}^{-1}{A}^{\text{T}}b$ (16)

传统LK光流仅利用前向映射($t\to t+1$ )进行运动估计，易受局部极小值和灰度突变影响。本文通过构建双向可逆约束，将前向光流估计($v_t\to t+1$ )与后向光流估计($v_t+1\to t$ )联立优化，形成闭环校验体系。具体而言，前向过程通过式(13)~(16)计算特征点从t到t + 1帧的偏移量$v_f$ ，后向过程则以t + 1帧特征点为起点，反向计算回到t帧的偏移量$v_b$ 。根据光流可逆性原理，理想情况下$x_b+v_f-v_b=x_t$ ，即双向偏移量应满足$v_f=v_b$ 。通过定义双向误差函数，并将其融入原始LK光流的目标函数(式(17))，形成包含前向灰度约束与后向可逆约束的联合优化模型。该机制通过双向信息耦合，有效抑制了单方向跟踪误差的累积。

当相机运动过快时，单层光流计算容易引发局部极值问题。为解决此问题，可采用金字塔光流方法，通过逐层缩放图像，从顶层到底层依次应用光流计算，并将每层结果作为下一层的初始变换[10]，如图4所示。

Figure 4. Feature pyramid model

图4. 特征金字塔模型

Figure 5. Bidirectional reversibility model view

图5. 双向可逆性模型视图

在LK光流跟踪方法中，由于迭代不收敛或收敛错误，可能导致跟踪失败且难以恢复。为解决该问题，本文引入了前后向检测机制，通过误差校验每一帧的跟踪结果。光流的双向可逆性是特征点跟踪的重要约束，假设特征点的时间运动可逆[11]。如图5所示，改进后的LK算法利用前一帧的特征点预测当前帧的位置，并反向估计前一帧的位置。理论上，这两个位置应完全一致，但由于误差会存在偏差。本文通过定义前向和后向偏移量，并融合两者的目标函数，构建新的优化目标，进而得到更精确和鲁棒的位置偏移量。

如图5所示，以相邻帧灰度强度$I_t\left(X\right)$ 与$I_{t+1}\left(X\right)$ 为输入数据，构建双向光流约束目标函数：

$\begin{array}{c}ϵ\left(d_{\text{offset\_prev}},d_{\text{offset\_back}}\right)={\displaystyle \sum }_{x=x_0-w_x}^{x_0+w_w}{\displaystyle \sum }_{y=y_0-w_y}^{y_0+w_y}{\left(I_t(X)-I_{t+1}\left(X+d_{\text{offset\_prev}}\right)\right)}^2\\ +{\displaystyle \sum }_{x=x_0-w_x}^{x_0+w_w}{\displaystyle \sum }_{y=y_0-w_y}^{y_0+w_y}{\left(I_{t+1}\left(X+d_{\text{offset\_prev}}\right)-I_t\left(X+d_{\text{offset\_back}}\right)\right)}^2\end{array}$ (17)

对$d_{\text{offset\_prev}}$ 和$d_{\text{offset\_back}}$ 分别求偏导数，得到式(18)，并令其等于0。

$\begin{array}{c}\frac{\partial ϵ\left(d_{\text{offset\_prev}},d_{\text{offset\_back}}\right)}{\partial d_{\text{offset\_prev}}}=-2{\displaystyle \sum }_{x=x_0-w_x}^{x_0+w_x}{\displaystyle \sum }_{y=y_0-w_y}^{y_0+w_y}\left(I_t-I_{t+1}\left(X+d_{\text{offset\_prev}}\right)\right)\cdot \nabla I_{t+1}\\ +{\displaystyle \sum }_{x=x_0-w_x}^{x_0+w_x}{\displaystyle \sum }_{y=y_0-w_y}^{y_0+w_y}\left(I_{t+1}\left(X+d_{\text{offset\_prev}}\right)-I_t\left(X+d_{\text{offset\_back}}\right)\right)\cdot \nabla I_t\\ =0\end{array}$ (18)

$\frac{\partial ϵ\left(d_{\text{offset\_prev}},d_{\text{offset\_back}}\right)}{\partial d_{\text{offset\_back}}}=-2{\displaystyle \sum }_{x=x_0-w_x}^{x_0+w_x}{\displaystyle \sum }_{y=y_0-w_y}^{y_0+w_y}\left(I_{t+1}\left(X+d_{\text{offset\_prev}}\right)-I_t\left(X+d_{\text{offset\_back}}\right)\right)\cdot \nabla I_t=0$ (19)

将式(18)和(19)进行一阶泰勒级数展开，可以求出$I_{t+1}\left(X+d_{\text{offset\_prev}}\right)$ 和$I_{t+1}\left(X+d_{\text{offset\_back}}\right)$ 的近似值为：

$I_{t+1}\left(X+d_{\text{offset\_prev}}\right)\approx I_{t+1}\left(X\right)+\nabla I_{t+1}\cdot d_{\text{offset\_prev}}$ (20)

$I_t\left(X+d_{\text{offset\_back}}\right)\approx I_t\left(X\right)+\nabla I_t\cdot d_{\text{offset\_back}}$ (21)

将式(20)和(21)代入式(17)中，可以得到：

$\begin{array}{l}{\displaystyle \sum }_{x=x_0-w_x}^{x_0+w_x}{\displaystyle \sum }_{y=y_0-w_y}^{y_0+w_y}\left(I_t\left(X\right)-I_{t+1}\left(X\right)-\nabla I_{t+1}\cdot d_{\text{offset\_prev}}\right)\nabla I_{t+1}\\ -{\displaystyle \sum }_{x=x_0-w_x}^{x_0+w_x}{\displaystyle \sum }_{y=y_0-w_y}^{y_0+w_y}\left(I_{t+1}\left(X\right)+\nabla I_{t+1}\cdot d_{\text{offset\_back}}\right)\nabla I_{t+1}=0\end{array}$ (22)

${\displaystyle \sum }_{x=x_0-w_x}^{x_0+w_x}{\displaystyle \sum }_{y=y_0-w_y}^{y_0+w_y}\left(I_{t+1}\left(X\right)+\nabla I_{t+1}\cdot d_{\text{offset\_prev}}-I_t\left(X\right)-\nabla I_t\cdot d_{\text{offset\_back}}\right)\cdot \nabla I_t=0$ (23)

联立方程(22)和(23)求出$d_{\text{offset\_back}}$ 的值为：

$d_{\text{offset\_back}}=\left(I_t\left(X\right)-I_{t+1}\left(X\right)-\nabla I_{t+1}\cdot d_{\text{offset\_prev}}\right)\cdot {\left(\nabla I_{t+1}^{\text{T}}\nabla I_t\right)}^{-1}$ (24)

根据该公式可计算特征点的后向偏移量$d_{\text{offset\_back}}$ 。

引入反向跟踪机制后，可通过后向偏移量优化前向偏移量，从而提升估计精度。若points_prev和points_back之间偏移量$d_{\text{offset\_back}}\approx 0$ 的话，则表明特征点跟踪结果准确可靠；反之，如果$d_{\text{offset\_back}}>d_{\text{threshold}}$ ，则说明跟踪失败。为提高计算结果的精度，可将此类特征点剔除。

本文分别采用传统LK光流法与改进后的LK光流算法对FAST、ORB和Shi-Tomasi三种常见的特征点检测方法提取的特征点进行跟踪。实验数据选自EuRoC数据集中的图像序列，以验证所提算法的有效性与精度，具体对比结果如表2所示。

Table 2. Optical flow tracking comparison

表2. 光流跟踪对比

算法	检测时间(s)	特征点提取数目	特征点跟踪率	前后向算法修正前	前后向算法修正后	实际位移(cm)
FAST	1.20	580	0.78	13.8	14.6	15
ORB	1.50	460	0.76	13.7	14.8	15
Shi-Tomasi	2.30	520	0.82	13.5	14.4	15

3.3. 优化角点提取精度

在对角点进行提取时，需要获取该点的像素灰度值，简单的单线性插值方法会造成较大的偶然性，所以在图像求解像素点灰度值时多使用双线性插值的方法[12]，如图6所示。

Figure 6. Schematic diagram of the bilinear interpolation method

图6. 双线性插值法示意图

基于P点邻近四邻域像素的已知灰度信息，采用双线性插值方法可计算目标点像素值：

$\begin{array}{c}\{\begin{array}{c}f\left(R_1\right)\approx \frac{x_2-x}{x_2-x_1}f\left(Q_{11}\right)+\frac{x-x_1}{x_2-x_1}f\left(Q_{21}\right)\\ f\left(R_2\right)\approx \frac{x_2-x}{x_2-x_1}f\left(Q_{12}\right)+\frac{x-x_1}{x_2-x_1}f\left(Q_{22}\right)\end{array} \end{array}$ (25)

$f\left(P\right)\approx \frac{y_2-y}{y_2-y_1}f\left(R_1\right)+\frac{y-y_1}{y_2-y_1}f\left(R_2\right)$ (26)

式中，$R_1=\left(x,y_1\right)$ ，$R_2=\left(x,y_2\right)$ ，f为该点的像素灰度值。首先在x轴方向上进行两次插值，得到$R_1$ 和$R_2$ 的像素值，然后在y轴方向上，根据$R_1$ 、$R_2$ 两点的像素值插值得到P点的像素值。

传统双线性插值方法的不足体现在其固定沿水平与垂直方向的插值路径，与图像边缘实际走向可能存在偏差：当插值点处于斜向边缘(即边缘方向与坐标轴不平行)时，按固定方向的分步插值会横穿不同灰度区域，导致插值结果出现高频伪影，表现为边缘模糊或虚假纹理。本文使用的边缘自适应插值方法，通过检测局部边缘的方向，将插值路径调整为沿边缘切线方向进行加权计算。具体来说，首先分别计算水平和垂直方向的插值结果，然后根据边缘倾斜角度动态分配权重——当边缘接近水平时，侧重水平方向的插值结果；当边缘倾斜时，增加沿边缘切线方向的插值权重。这种自适应调整机制能够有效避免插值过程中跨越边缘两侧的灰度突变区域，从而在保留边缘锐利度的同时，减少插值误差。

该方法的各向同性插值策略在物体边缘与坐标轴非对齐时(如图7所示)，因局部梯度方向与插值轴向失配，导致强度场出现阶跃式跳变。这种方向敏感性误差主要体现在插值路径跨越不同材质边界时产生伪影以及斜向边缘区域的均方误差较轴向边缘增加1.8~2.5倍的问题。

Figure 7. Schematic diagram of the interpolation method

图7. 插值法示意图

本文采用基于边缘的双线性插值算法[11]，首先分别在x轴和y轴方向上求得A点和B的像素值，如下：

$\begin{array}{c}\{\begin{array}{c}{Q}_a=\frac{x_{12}-x_A}{x_{12}-x_{11}}{Q}_{11}+\frac{x_A-x_{11}}{x_{12}-x_{11}}{Q}_{12}\\ Q_b=\frac{y_{21}-y_B}{y_{21}-y_{11}}{Q}_{11}+\frac{y_B-y_{11}}{y_{21}-y_{11}}{Q}_{21}\end{array}\end{array}$ (27)

然后，沿A、B两点的直线方向进行插值，得到P点的像素值为：

$I\left(P\right)=W_{Pa}{Q}_a+W_{Pb}{Q}_b$ (28)

式中，$W_{Pa}$ 和$W_{Pb}$ 分别是线段PA和PB的权重，使用此方法求出的P点像素值，比直接在x轴方向和y轴方向做插值更准确，并且可以避免因为像素值突变带来的边缘上的像素模糊，使得最终获取的点的像素灰度值更加准确。

Table 3. Comparison of sub-pixel coordinates after iteration of original coordinates

表3. 原始坐标迭代后亚像素坐标对比

原始坐标	亚像素坐标
(108, 529)	(108.1253, 529.412)
(184, 449)	(184.4311, 448.8032)
(121, 529)	(121.092, 528.7131)
(160, 391)	(160.2131, 391.412)
(180, 471)	(180.3143, 471.042)
(139, 529)	(138.6245, 529.751)
(108, 529)	(108.1253, 529.412)

传统的角点检测算法通常输出整数坐标，意味着角点位置与像素位置完全重合。然而，角点的真实位置是连续分布的，而相机成像的像素则是离散的，两者总是有偏差。如果需要基于图像进行几何测量，偏差会带来一定的精度损失。本文使用基于迭代的方法寻找亚像素角点[13]，并在Lena图像上进行了测试。原始坐标与迭代后亚像素坐标对比如表3所示。

4. 实验与结果分析

算法运行的平台配置为Inter i7-7500U处理器，主频为2.70 GHz，内存8 G，未使用GPU加速，系统为Ubuntu 18.04，Ros版本为Melodic。

4.1. 数据集介绍

为验证本文算法的有效性，采用开源数据集EuRoc进行实验测试。该数据集由无人机搭载双目相机和IMU传感器采集，涵盖普通房间和厂房中不同光线和纹理等复杂环境。数据集序列提供了轨迹的真实值用以量化轨迹误差，且根据无人机的速度和图片质量，序列分为简单、中等、难三个等级[14]。

4.2. SLAM中的测试效果验证

Figure 8. Comparison of estimated and benchmark trajectories for the two algorithms

图8. 两种算法的估计轨迹和基准轨迹比较

本文以EuRoC中的MH_01_easy、MH_03_medium、MH_04_difficult和MH_05_difficult作为实验数据集，测试了PL-VINS以及本文改进后的系统。使用EVO工具将时间戳与真实值对齐并计算绝对轨迹误差(absolute pose error, APE)，使用均方根误差(root mean square error, RMSE)对系统性能进行评估[15]。图8为PL-VINS和本文算法运行的轨迹与基准轨迹的对比，图中，虚线“groundtruth”代表的是基准轨迹，蓝色实线“before”代表PL-VINS的轨迹，绿色实线“later”表示本文改进算法的轨迹。

由图8(a)~(d)可以看出，本文改进后的方法和原方法的轨迹都非常接近于真实轨迹，图8(a)和图8(c)中，当突然转向和加速时，能明显看出本文方法的轨迹更接近于真实轨迹，定位精度要优于PL-VINS。

Figure 9. RMSE plot of the trajectory of the algorithm in this paper

图9. 本文算法运行轨迹RMSE图

图9为本文算法分别在MH_01_easy、MH_03_medium、MH_04_difficult和MH_05_difficult数据集下的实验轨迹与真实轨迹之间的APE误差热力图，虚线是参考的真实轨迹，实线是本文算法运行得出的轨迹。从图中可以看出，本文算法的稳定性较强，整体轨迹误差持续控制在较低范围。且当相机运动方向突然加速或转向，或光线变化较大时，系统误差相对来说会增大，但仍能稳定控制在较小范围。系统整体可以较好地完成位姿精准估计的任务。

表4给出了以APE为指标计算的PL-VINS和本文改进算法的RMSE，定量分析了跟踪精度。从表中可以看出，本文算法在精度上均优于PL-VINS，RMSE显著降低，平均降低18.2%；在MH_01_easy、MH_03_medium、MH_04_difficult和MH_05_difficult序列中，与原始算法相比，本文算法的RMSE分别降低了29.8%、9.7%、10.9%、25.7%。在运动速度更快、场景模糊及纹理稀疏的difficult等级序列中，本文算法的RMSE仍显著降低，进一步说明本文方法对复杂场景的适应性更强，具有较强的鲁棒性。

Table 4. Comparison of localization accuracy of different datasets (m)

表4. 不同数据集定位精度对比(m)

序列	PL-VINS	本文算法
MH_01_easy	0.173772	0.121970
MH_03_medium	0.259535	0.234449
MH_04_difficult	0.350490	0.312435
MH_05_difficult	0.369991	0.274969

5. 结论

本文提出了一种基于改进PL-VINS的点线特征融合的实时视觉惯性SLAM算法，分别优化了前端特征点提取、特征点匹配和后端非线性优化部分。前端通过迭代模型优化特征点提取，在原有角点的基础上找到更接近真实世界坐标的点，提高了整体算法的精度；利用反向光流剔除杂点，并采用前后帧双向检测提高匹配精度；在后端非线性优化中，针对雅可比矩阵残差不同造成的计算损耗，采用分步优化，减少了计算时间。实验结果表明：本文算法相比于PL-VINS，边缘化速度平均提升30%，不同难度的数据集上平均定位精度提升18.2%，且在光照不足、弱纹理的复杂环境下，适应能力、鲁棒性更强。