基于线流特征的视觉惯性SLAM算法

Visual Inertia SLAM Algorithm Based on Point Line Flow Features

摘要: 本文研究了一种基于点和线特征的高效视觉惯性同时定位与建图(SLAM)方法。通过短线融合、线特征均匀分布和自适应阈值提取来改进传统的线检测模型，以获得用于构建SLAM约束的高质量线特征。基于灰度不变性假设和共线性约束，提出了一种线光流跟踪方法。实验结果表明，算法提高了线特征检测与匹配的效率和定位精度。

Abstract: This paper investigates an efficient visual inertial simultaneous localization and mapping (SLAM) method based on point and line features. Improve traditional line detection models by short-term fusion, uniform distribution of line features, and adaptive threshold extraction to obtain high-quality line features for constructing SLAM constraints. A line optical flow tracking method is proposed based on the assumption of grayscale invariance and collinearity constraints. The experimental results show that the algorithm improves the efficiency and positioning accuracy of line feature detection and matching.

1. 引言

同步定位和绘图(SLAM)技术[1]在自动驾驶、无人机、机器人和增强现实中至关重要。不同传感器之间的内在特性使得优势互补的多传感器融合SLAM技术成为主导的研究方向。视觉惯性SLAM导航系统(VINS)相对于其他多传感器融合方案具有低成本、轻量化等优点，受到了研究人员的广泛关注[1]-[3]。这些基于点的VINS方法难以在一些人工场景中找到足够数量的可靠点特征，例如稀疏或重复纹理，最终导致整个系统位姿估计精度降低甚至失效[4]。人工场景中有丰富的线特征，一维线特征比零维点特征[3]包含更多的信息[5]。因此，当场景中没有足够的点特征时，线特征[6]-[10]是很好的补充。虽然线特征可以提高SLAM在挑战性环境下的精度和鲁棒性[4]，但其缺点是线特征比点特征更难跟踪，这使得基于线的SLAM方法不如基于点的SLAM方法流行。主要存在三个问题：错误匹配、遮挡[11]和实时性。重复纹理下线段的错误匹配问题严重，线段提取容易受到遮挡或外观模糊的影响。线流是打破当前SLAM中线特征瓶颈的较好方向。利用时空相干性的线流可以减少帧间的错误匹配问题。此外，没有线段描述的线流可以提高线特征跟踪的实时性。然而，这一领域的工作做得很少。由此提出了一种可以高效完成一帧检测和多帧跟踪的线光流，以解决线特征匹配中的误差匹配和耗时问题。

2. 系统概述

所提出的单目视觉惯性SLAM系统概述如图1所示。算法的基本结构类似于VINS-Mono和PL-VINS [9]。侧重于通过一种新颖的线光流跟踪算法和一种构造线特征残差的新方法来提高最先进算法的效率和精度。需要注意的是，与基于VINS-Mono的其他点线融合VINS，如PL-VIO [12]、PL-VINS、PLF-VINS相比，算法摒弃了耗时的LSD [4]和LBD [9]组合，采用了改进版的EDLines [13]检测和新的线光流跟踪方法，大大提高了运行速度。对于线特征的测量模型，构建了残差，避免了遮挡和短线匹配的情况。

Figure 1. System framework diagram

图1. 系统框架图

3. 线特征测量模型

3.1. 直线表示

一条线在空间中有四个自由度。本文使用了两种形式的线表示。plucker表示由6个参数组成，分别用于初始化、变换和投影。标准正交表示由4个参数组成，用于优化。

plucker表示：三维空间线可以表示为：

$ℒ={\left(n^{\text{T}},v^{\text{T}}\right)}^{\text{T}}$ (1)

式中，$v\in {ℝ}^3$ 表示直线方向向量，$n\in {ℝ}^3$ 表示由L和摄像机坐标系原点确定的平面π的法向量。

标准正交表示：plucker坐标虽然可以简洁地表示变换过程，但由于其过度参数化，不容易直接优化。因此，使用由四个参数组成的标准正交表示来进行优化。三维空间线L可以表示为：

$ℒ=\left(U,W\right)$ (2)

其中$U\in SO\left(3\right)$ ，$W\in SO\left(2\right)$ 。plucker坐标系到正交坐标系的方程可以表示为：

$U=\left[\begin{array}{ccc}{\varphi }_1& {\varphi }_2& {\varphi }_3\end{array}\right]=\left[\begin{array}{ccc}\frac{n}{\Vert n\Vert }& \frac{v}{\Vert v\Vert }& \frac{n\times v}{\Vert n\times v\Vert }\end{array}\right]$ (3)

$W=\left[\begin{array}{cc}\cos \left(\theta \right)& -\sin \left(\theta \right)\\ \sin \left(\theta \right)& \cos \left(\theta \right)\end{array}\right]=\frac{1}{\sqrt{{\Vert n\Vert }^2+{\Vert v\Vert }^2}}\left[\begin{array}{cc}\Vert n\Vert & -\Vert v\Vert \\ \Vert v\Vert & \Vert n\Vert \end{array}\right]$ (4)

因此，标准正交表示也可以表示为：

$\mathcal{O}=\left(\begin{array}{cccc}{\varphi }_1& {\varphi }_2& {\varphi }_3& \theta \end{array}\right).$ (5)

3.2. 观测模型

在先前的研究中，如PL-VIO和PL-VINS，线重投影误差是由观测线端点与重投影线端点之间的距离来构建的。残差的大小取决于线端点的精度。然而，在不同视角下，空间中的线特征在相机图像平面中观察到的线的长度是不一致的。此外，当线受到其他扰动(如遮挡)时，线特征检测方法可能会将长线划分为几个短线段。这些会导致端点位置发生显著变化，进而影响残差值。因此，提出了一种简洁的方法来构建独立于线段端点的残差。

定义平面$\tilde{\pi }$ 和平面$\pi$ ，在相机坐标系的原点$C_i$ 和归一化平面上的观测线之间形成平面$\tilde{\pi }$ ，在摄像机坐标系的原点$C_i$ 与归一化平面上的投影线之间形成平面$\pi$ 。直线重投影误差可以定义为：

$r\left(z_{ℒ}^{c_i},\chi \right)=1-\frac{{\tilde{n}}^{c_i}\cdot n^{c_i}}{{\tilde{n}}^{c_i}{n}^{c_i}}$ (6)

其中${\tilde{n}}^{c_i}$ 和$n^{c_i}$ i分别表示平面$\tilde{\pi }$ 和平面$\pi$ 的法向量，$\chi$ 为滑动窗口中的完整状态变量，$z_{ℒ}^{c_i}$ 表示第i个相机帧观察到的线特征L。对应的雅可比矩阵$J_n$ 可由链式法则得到：

$J_n=\frac{\partial r_n}{\partial n^{c_i}}\frac{\partial n^{c_i}}{\partial {ℒ}^{c_i}}\left[\begin{array}{cc}\frac{\partial {ℒ}^{c_i}}{\partial \delta x^i}& \frac{\partial {ℒ}^{c_i}}{\partial {ℒ}^w}\frac{\partial {ℒ}^w}{\partial \delta \mathcal{O}}\end{array}\right]$ (7)

$\frac{\partial r_n}{\partial n^{c_i}}={\left[\begin{array}{ccc}\frac{\partial r_n}{\partial n_{\text{1}}^{c_i}}& \frac{\partial r_n}{\partial n_{\text{2}}^{c_i}}& \frac{\partial r_n}{\partial n_{\text{3}}^{c_i}}\end{array}\right]}^{\text{T}}=[\begin{array}{c}{n}_1^{c_i}\frac{{\tilde{n}}^{c_i}\cdot n^{c_i}}{{\Vert {\tilde{n}}^{c_i}\Vert }^{\frac{3}{2}}{\Vert n^{c_i}\Vert }^{\frac{3}{2}}}-\frac{{\tilde{n}}_1^{c_i}}{\Vert {\tilde{n}}^{c_i}\Vert \Vert n^{c_i}\Vert }\\ n_2^{c_i}\frac{{\tilde{n}}^{c_i}\cdot n^{c_i}}{{\Vert {\tilde{n}}^{c_i}\Vert }^{\frac{3}{2}}{\Vert n^{c_i}\Vert }^{\frac{3}{2}}}-\frac{{\tilde{n}}_2^{c_i}}{\Vert {\tilde{n}}^{c_i}\Vert \Vert n^{c_i}\Vert }\\ n_3^{c_i}\frac{{\tilde{n}}^{c_i}\cdot n^{c_i}}{{\Vert {\tilde{n}}^{c_i}\Vert }^{\frac{3}{2}}{\Vert n^{c_i}\Vert }^{\frac{3}{2}}}-\frac{{\tilde{n}}_3^{c_i}}{\Vert {\tilde{n}}^{c_i}\Vert \Vert n^{c_i}\Vert }\end{array}]$(8)

4. 线特征检测与跟踪模型

4.1. 改进的EDLines检测方法

选择EDLines作为线段检测的基本算法，该算法具有丰富的可调参数。然而，原始EDLines检测到的片段在空间分布上并不适合SLAM约束。因此，提出了一种改进的EDLines检测方法。对三个部分做了改进分别是：短线融合、线特征均匀分布和自适应阈值提取。

短线融合：由于部分遮挡经常将一条线分割成碎片，图像噪声可能导致线端点[11]不稳定，在这种情况下，EDLines可能会将一条完整的线提取成几条短线。这可能会使线流跟踪和优化变得不稳定，所以需要合并短线。判断标准：两条线共线，两条线端点之间的距离足够小。

线特征均匀分布：特征在空间中的分布越均匀，特征匹配就越能准确地表示空间中的几何关系。在这种方法中，在当前图像中检测到的线的位置添加了一个宽的掩码，不在掩码上提取新的线。

自适应阈值提取：在上述两种策略之后，对提取的线条进行计数。假设提取的行数小于事先设定的阈值。在纹理较弱、灰度变化较小的情况下，通过降低EDLines的梯度阈值参数，再次进行线特征提取，获得足够的线特征。

4.2. 光流跟踪

线流模型基于灰度不变性理论。在时刻t位于$\left(u,v\right)$ 的像素在时刻$t+\text{d}t$ 移动到$\left(u+\text{d}u,v+\text{d}v\right)$ 。可以将这个像素的灰度值定义为：

$I\left(u+\text{d}u,v+\text{d}v,t+\text{d}t\right)=I\left(u,v,t\right)$ (9)

式(1)左侧的泰勒展开式为：

$\begin{array}{c}I\left(u+\text{d}u,v+\text{d}v,t+\text{d}t\right)\approx I\left(u,v,t\right)+\frac{\partial I}{\partial u}\text{d}u+\frac{\partial I}{\partial v}\text{d}v+\frac{\partial I}{\partial t}\text{d}t.\end{array}$ (10)

合并式(1)和式(2)：

$\frac{\partial I}{\partial u}\frac{\text{d}u}{\text{d}t}+\frac{\partial I}{\partial v}\frac{\text{d}v}{\text{d}t}=-\frac{\partial I}{\partial t}$ (11)

其中$\frac{\partial I}{\partial u}$ 和$\frac{\partial I}{\partial v}$ ，记为$I_u$ 和$I_v$ ，分别表示该点图像在u和v方向上的灰度梯度。$\frac{\partial I}{\partial t}$ 记为$I_t$ ，表示图像灰度值随时间的变化量。化简为

$\left[\begin{array}{cc}{I}_u& I_v\end{array}\right]\left[\begin{array}{c}\frac{\text{d}u}{\text{d}t}\\ \frac{\text{d}v}{\text{d}t}\end{array}\right]=-I_t$ (12)

式(4)在图像中的任何像素点上都有效，因此它在线段上成立，而直线上的点也必须满足共线性约束。定义这条线上的点的集合：

$l=\left\{\left(u_1,v_1\right),\left(u_2,v_2\right),\cdots ,\left(u_n,v_n\right),\cdots ,\left(u_m,v_m\right)\right\}$ (13)

其中$\left(u_1,v_1\right)$ 和$\left(u_m,v_m\right)$ 分别表示直线的起点和终点。$l_n$ 表示$\left(u_1,v_1\right)$ 和$\left(u_m,v_m\right)$ 之间的欧几里得距离。$l^{\prime }$ 表示l的下一帧位置，连续两帧之间的直线对应点之间的关系为：

$\{\begin{array}{l}{u^{\prime }}_n=u_n+g_1+{l^{\prime }}_n\cos \left(\alpha +g_3\right)-{l^{\prime }}_n\cos \alpha \\ {v^{\prime }}_n=v_n+g_2+{l^{\prime }}_n\sin \left(\alpha +g_3\right)-{l^{\prime }}_n\sin \alpha \end{array}$ (14)

其中$g_1,g_2,g_3$ 分别表示起始点的位置变化和围绕起始点$\left(u_1,v_1\right)$ 的旋转变化。在两个连续的帧之间，$g_3$ 是一个微小的变化，$l_{\text{n}}\approx {l^{\prime }}_{\text{n}}$ 。因此，(6)可以简化为：

$\{\begin{array}{l}{u^{\prime }}_n=u_n+g_1-g_3\left(v_n-v_1\right),\\ {v^{\prime }}_n=v_n+g_2+g_3\left(u_n-u_1\right).\end{array}$ (15)

由(7)，我们可以得到：

$\{\begin{array}{l}\frac{\text{d}u}{\text{d}t}=\frac{\text{d}\left({u^{\prime }}_n-u_n\right)}{\text{d}t}=\frac{\text{d}{g}_1}{\text{d}t}-\left(v_n-v_1\right)\frac{\text{d}{g}_3}{\text{d}t},\\ \frac{\text{d}v}{\text{d}t}=\frac{\text{d}\left({v^{\prime }}_n-v_n\right)}{\text{d}t}=\frac{\text{d}{g}_2}{\text{d}t}+\left(u_n-u_1\right)\frac{\text{d}{g}_3}{\text{d}t}.\end{array}$ (16)

由式(8)，定义$\frac{\text{d}{g}_1}{\text{d}t},\frac{\text{d}{g}_2}{\text{d}t}，\frac{\text{d}{g}_3}{\text{d}t}$ 为$G_1,G_2,G_3$ ，由此公式(4)可表示为

$\left[\begin{array}{cc}{I}_u& I_v\end{array}\right]\left[\begin{array}{ccc}1& 0& -\left(v_n-v_1\right)\\ 0& 1& \left(u_n-u_1\right)\end{array}\right]\left[\begin{array}{c}{G}_1\\ G_2\\ G_3\end{array}\right]=-I_t.$ (17)

直线上的每个点$\left(u_n,v_n\right)$ 都满足上述方程。为了提高线流跟踪能力，我们将Lucas-Kanade特征跟踪器的pyra-midal实现扩展到线流。丢弃两个端点附近的一个小直线段来构建线流跟踪的约束，并在跟踪完成后通过判断端点灰度梯度是否与线段一致来扩展它。通过迭代的收敛性来判断每条线跟踪的质量。将跟踪成功的线加入到优化中。

5. 实验

5.1. 线流分析

运行时间：我们测量了EuRoC、TUM VI和KAIST VIO数据集上的线特征检测和跟踪运行时间，如图2所示。实验结果表明，与PL-VINS相比，MED和LOFT模块在线条特征检测和匹配方面都有显著提高。为了进行更全面的比较，我们评估了算法在EuRoC数据集完整序列上的每个线程的平均运行时间，如表1所示。在所有线程上，VINS-Fusion略快于VINS-Mono。由于在优化中加入了线特征残差，因VINS-Mono相比，PL-VINS、E-PL-VINS、PL-F-VINS和EPLF-VINS在线程3上的时间增加。对于螺纹2，E-PL-VINS、PL-F-VINS和EPLF-VINS的速度分别比PL-VINS提高28.7%、18.7%和32.9%。

5.2. 定量比较

实验测量EuRoC数据集的完整序列，该数据集由机器大厅和Vicon室的11个序列组成，具有结构信息。表1给出了绝对轨迹误差(ATE)的均方根误差(RMSE)。在无闭环约束条件下，EPLF-VINS在平均平移值上比VINS-Mono提高35.83%，比PL-VINS提高28.54%。PL-F-VINS显示出一定程度的精度提高。在闭环约束条件下，EPLF-VINS比VINS-Mono精度提高32.27%，比PL-VINS精度提高13.61%。实验结果表明，与PL-VINS、E-PL-VINS和PL-F-VINS相比，我们的系统线特征测量模型进一步提高了测量精度。

5.3. 定性评价

TUM VI基准是一个序列，围绕大学建筑的中央大厅散步，包含丰富的线条特征。与PL-VINS进行了线特征检测与跟踪的可视化实验，如图3(a)所示。绿色提取线条，蓝色追踪图3(a)中的线在空间上的分布比红线更为合理。合理的线特征检测和残差测量模型使得后端映射更加精确和准确，如图3(b)所示。

Figure 2. Runtimes of line feature detection and tracking.

图2. 线特征检测和跟踪的运行时间

Table 1. RMSE ATE [m] comparison results

表1. RMSE ATE [m]比较结果

	MH_01	MH_02	MH_03	MH_04	MH_05
VINS-Mono	0.2023	0.1879	0.2281	0.3697	0.2968
PL-VINS	0.2159	0.1933	0.2177	0.2471	0.3254
E-PL-VINS	0.2245	0.1968	0.2195	0.2476	0.3248
PL-F-VINS	0.2059	0.1812	0.1624	0.1622	0.2185
EPLF-VINS	0.1404	0.0879	0.1137	0.1826	0.1817

Figure 3. Experimental results on the TUM VI benchmark dataset. (a) The red line in the middle represents the line detection and matching visualization of PL-VINS. (a) The green line (detection line) and blue line (tracking line) on the lower side represent the results of our algorithm. (a) Comparison results of line feature extraction and tracking with PL-VINS. (b) PL-VINS line chart. (c) EPLF-VINS line chart

图3. TUM VI基准数据集上的实验结果。(a)中的红线表示PL-VINS的线检测和匹配可视化。(a)下侧的绿线(检测线)和蓝线(追踪线)表示我们的算法结果。(a)与PL-VINS的线特征提取与跟踪对比结果。(b) PL-VINS线形图。(c) EPLF-VINS线形图

6. 结论

我们提出了一种简洁高效的线流检测和跟踪算法，该算法通过改进的线特征提取策略提高了线特征的质量。线流跟踪器比传统的LBD匹配算法更有效。此外，提出了一种新的线特征测量模型来表示残差，以消除线端点对残差估计的影响。我们使用EuRoC、TUM VI和KAIST VIO数据集提供定量评估和可视化实验。与最先进的算法相比，我们的算法计算效率更高，得到的结果也更准确。