PL-VIO: Tightly-Coupled Monocular Visual–Inertial Odometry Using Point and Line Features

Yijia He 1,2,* , Ji Zhao 3, Yue Guo 1,2, Wenhao He 1 and Kui Yuan 1

2018

摘要

To address the problem of estimating camera trajectory and to build a structural 3D map based on inertial measurements and visual observations, this paper proposes point-line visual-inertial odometry (PL-VIO), a tightly-coupled monocular visua-inertial odometry system exploiting both point and line features. Compared with point features, lines provide significantly more geometrical structure information on the environment. To obtain both computation simplicity and representational compactness of a 3D spatial line, Plücker coordinates and orthonormal representation for the line are employed. To tightly and efficiently fuse the information from inertial measurement units (IMUs) and visual sensors, we optimize the states by minimizing a cost function which combines the pre-integrated IMU error term together with the point and line re-projection error terms in a sliding window optimization framework. The experiments evaluated on public datasets demonstrate that the PL-VIO method that combines point and line features outperforms several state-of-the-art VIO systems which use point features only.

引言

松耦合和紧耦合

通过直接或间接的融合传感器的测量值，VIO有两类：松耦合和紧耦合方法。

松耦合方法通过两个估计器分别估计相对运动和融合两个估计器的估计来处理图像和IMU测量结果。紧耦合方法使用一个估计器，通过直接融合来自相机和IMU的原始测量值来找到最佳估计。

与松耦合方法相比，紧耦合方法通常更准确和鲁棒。本文提出的PL-VIO方法是一个紧耦合VIO系统。紧耦合VIO方法根据度量模型中线性化的次数进行分类。这些基于EKF的方法在更新步骤中只处理一次测量值，而批量非线性优化方法在优化步骤中多次线性化。

基于滤波和基于优化的方法

基于滤波的方法集成IMU测量来递推/预测状态，然后用视觉测量更新/修正最新的状态。由于状态向量中包含了3D路标的坐标，因此EKF的计算复杂度随路标数量的增加呈二次增长。为了解决这一问题，Mourikis和Roumeliotis提出了MSCKF，该滤波器将路标坐标从状态向量中边缘化。这种方法的一个缺点是，用于更新状态的路标测量值需要移出相机的视野，这意味着不是所有当前的视觉测量值都在滤波器中使用。此外，线性化误差使滤波器的不一致。

基于优化的方法通过最小化IMU测量残差和视觉重投影残差的联合非线性代价函数来获得最优估计。因此，基于优化的方法可以在不同的点上重复状态向量的线性化，从而达到比基于滤波的方法更高的精度。

通过对帧间IMU测量值的积分计算得到IMU测量约束。然而，标准的IMU积分方法与第一帧IMU body的初始状态密切相关。当估计状态发生变化时，需要重新计算所有积分IMU测量值。Lupton和Sukkarieh 提出了IMU预积分技术，避免了这种重复积分。IMU预积分已广泛应用于基于优化的VIO。Forster等人将欧拉角替换为流形上的旋转群，重新定义了IMU预积分。Liu等提出了连续预积分方法。

虽然基于优化的方法已经取得了很高的精度，但随着越来越多的路标加入到优化中，计算变得昂贵。OKVIS使用了一种先进后出的滑动窗口方法，通过边缘化来自最老状态的测量值来限制计算量。Shen等人提出了一种双向边缘化，选择性地边缘化body状态和路标。

线SLAM

虽然VIO领域已经有了很多成功的工作，但大多数VIO系统只使用点特征作为视觉信息。然而，在无纹理环境中的点的检测和在光照变化的场景中的点的跟踪是具有挑战性的。相比之下，线段（line segments）在这些场景中是替代的方案。此外，线段比点提供更多的环境结构信息。对于纯视觉SLAM，有几种结合点和线特征来估计相机运动的方法（比如PL-SLAM）。在SLAM系统中集成直线特征的最简单方法是使用两个端点来表示直线。从不同的视角匹配一条线的端点是困难的。此外，3D空间线只有4个自由度，而两个三维端点引入了6个参数，这导致了过参数化。Bartoli和Sturm提出了直线的正交表示，在优化过程中使用一个3DOF旋转矩阵和一个1DOF旋转矩阵来更新线参数。正交表示已被用于一些立体视觉SLAM系统。对于VIO方法，Kottas和Roumeliotis仅使用线特征研究了VIO的可观性。Kong等人利用三焦几何构建了一个结合点和线特征的立体VIO系统。但是，这些工作涉及到基于滤波的VIO。在本文提出的PL-VIO方法中，将线特征集成到优化框架中，以便获得比基于滤波的方法更高的精度。

本文的贡献

为了构建结构的3D地图并获得相机的运动，本文提出了PL-VIO系统，该系统通过联合最小化IMU预积分约束以及滑动窗口中的点和线重投影误差来优化系统状态。与传统的仅使用点特征的方法相比，PL-VIO利用了额外的线特征，旨在提高光照变化环境下的鲁棒性和准确性。

主要贡献如下

PL-VIO是第一个使用点和线作为路标的、基于优化的单目VIO系统。
为了紧密而有效地融合来自视觉和惯性传感器的信息，引入了一个具有IMU预积分约束和点/线特征的滑动窗口模型。为了在优化中更紧凑地表示三维空间直线，采用了直线的正交表示。导出了所有误差项相对于IMU body状态的雅可比矩阵，有效地求解了滑动窗口优化问题。
将PL-VIO与ROVIO、OKVIS、VINS-Mono等方法，在EuRoc数据集和PennCOSYVIO数据集。

数学形式

记号

图1演示了视觉-惯性传感器和点线特征的视觉观测。记 cic_ici 为时刻 t=it=it=i 的相机坐标系， bib_ibi 是IMU坐标系， www 世界坐标系。 (⋅)c(\cdot)^c(⋅)c 表示坐标系 ccc 中的向量 (⋅)(\cdot)(⋅) 。四元数 qxy\bold{q}_{xy}qxy 用于将向量从坐标系 xxx 旋转到坐标系 yyy ，对应的旋转矩阵为 Rxy\bold{R}_{xy}Rxy 。 pxy\bold{p}_{xy}pxy 表示将向量从坐标系 xxx 平移到坐标系 yyy 的平移向量。四元数 qbc\bold{q}_{bc}qbc 和平移向量 pbc\bold{p}_{bc}pbc 是相机坐标系和body坐标系的外参，这些外参要么是数据集提供的，要么使用Kalibr标定工具箱得到的。 fj\bold{f}_jfj 和 Lj\mathcal{L}_jLj 是第j个点特征和线特征。z\bold{z}z 表示测量， zfjci\bold{z}_{\bold{f}_j}^{c_i}zfjci 是相机坐标系i中观测到的第j特征点。 zbibj\bold{z}_{b_i b_j}zbibj 表示两帧之间的预积分IMU测量。

IMU预积分

一个6轴IMU，包含了3轴加速度计和3轴陀螺仪，可以测量body坐标系相对于惯性坐标系的加速度 a\bold{a}a 和角速度 ω\boldsymbol{\omega}ω 。原始的测量值 ω^\boldsymbol{\hat{\omega}}ω^ 和 a^\bold{ \hat{a}}a^ 受到bias和白噪声的影响：
ω^=ωb+bgb+ngb(1)\boldsymbol{\hat{\omega}} = \boldsymbol{\omega}^b + \bold{b}^b_{\rm g} + \bold{n}^b_{\rm g} \tag{1} ω^=ωb+bgb+ngb(1)

a^=Rbw(aw+gw)+bab+nab(2)\bold{\hat{a}} = \bold{R}_{bw} (\bold{a}^w + \bold{g}^w ) + \bold{b}^b_{a} + \bold{n}^b_{a} \tag{2} a^=Rbw(aw+gw)+bab+nab(2)

其中 bgb\bold{b}^b_{\rm g}bgb , ngb\bold{n}^b_{\rm g}ngb 分别是陀螺仪的bias和噪声， bab\bold{b}^b_{a}bab , nab\bold{n}^b_{a}nab 是加速度计的bias和白噪声。gw=[0,0,g]⊤\bold{g}^w = [0,0,{\rm g}]^\topgw=[0,0,g]⊤ 是在坐标系 www 的重量向量。运动学如下：
p˙wbt=vtw,v˙tw=atw,q˙wbt=qwbt⊗[012ωbt](3)\bold{\dot{p}}_{w b_t} = \bold{v}_t^w, \quad \bold{\dot{v}}_t^w = \bold{a}_t^w, \quad \bold{\dot{q}}_{w b_t} = \bold{q}_{w b_t} \otimes \begin{bmatrix} 0 \\ \frac{1}{2} \boldsymbol{\omega}^{b_t} \end{bmatrix} \tag{3} p˙wbt=vtw,v˙tw=atw,q˙wbt=qwbt⊗[021ωbt](3)
其中 ⊗\otimes⊗ 表示四元数的乘法。

给定 t=it=it=i 时刻的IMU body状态：pwbi\bold{p}_{w b_i}pwbi , vtw\bold{v}_t^wvtw , qwbj\bold{q}_{w b_j}qwbj ，以及一系列在时间区间 t∈[i,j]t \in [i,j]t∈[i,j] 内的 ω,a\boldsymbol{\omega},\bold{a}ω,a 值，通过(3)积分得到时刻 t=jt=jt=j 的body状态：
pwbj=pwbi+viwΔt+∬t∈[i,j](Rwbtabt−gw)δt2vjw=viw+∫t∈[i,j](Rwbtabt−gw)δtqwbj=∫t∈[i,j]qwbt⊗[012ωbt]δt(4)\bold{p}_{w b_j} = \bold{p}_{w b_i} + \bold{v}_i^w \Delta t + \iint_{t \in [i,j]}{ ( \bold{R}_{w b_t} \bold{a}^{b_t} - \bold{g}^w ) \delta t^2} \\ \bold{v}_j^w = \bold{v}_i^w + \int_{t \in [i,j]}{( \bold{R}_{w b_t} \bold{a}^{b_t} - \bold{g}^w ) \delta t} \\ \bold{q}_{w b_j} = \int_{t \in [i,j]}{ \bold{q}_{w b_t} \otimes \begin{bmatrix} 0 \\ \frac{1}{2} \boldsymbol{\omega}^{b_t} \end{bmatrix} \delta t} \tag{4} pwbj=pwbi+viwΔt+∬t∈[i,j](Rwbtabt−gw)δt2vjw=viw+∫t∈[i,j](Rwbtabt−gw)δtqwbj=∫t∈[i,j]qwbt⊗[021ωbt]δt(4)
其中 Δt\Delta tΔt 是 iii 和 jjj 之间的时间差。在等式(4)中，从坐标系 bib_ibi 开始递推body 状态。当 bib_ibi 的状态改变时，需要对所有的测量值重新积分。由于在优化过程中，body状态都会调整, 公式(4)是非常耗时的. 通过将 qwbj\bold{q}_{w b_j}qwbj 解耦成 qwbi⊗qbibt\bold{q}_{w b_i} \otimes \bold{q}_{b_i b_t}qwbi⊗qbibt , 公式(4) 可以写成:
pwbj=pwbi+viwΔt−12gwΔt2+Rwbiαbibjvjw=viw−gwΔt+Rwbiβbibjqwbj=qwbi⊗qbibj(5)\bold{p}_{w b_j} = \bold{p}_{w b_i} + \bold{v}_i^w \Delta t - \frac{1}{2} \bold{g}^w \Delta t^2 + \bold{R}_{w b_i} \boldsymbol{\alpha}_{b_i b_j} \\ \bold{v}_j^w = \bold{v}_i^w - \bold{g}^w \Delta t + \bold{R}_{w b_i} \beta_{b_i b_j} \\ \bold{q}_{w b_j} = \bold{q}_{w b_i} \otimes \bold{q}_{b_i b_j} \tag{5} pwbj=pwbi+viwΔt−21gwΔt2+Rwbiαbibjvjw=viw−gwΔt+Rwbiβbibjqwbj=qwbi⊗qbibj(5)
其中:
αbibj=∬t∈[i,j](Rbibtabt)δt2βbibj=∫t∈[i,j](Rbibtabt)δtqbibj=∫t∈[i,j]qbibt⊗[012ωbt]δt(6)\boldsymbol{\alpha}_{b_i b_j} = \iint_{t \in [i,j]}{ ( \bold{R}_{b_i b_t} \bold{a}^{b_t}) \delta t^2}\\\boldsymbol{\beta}_{b_i b_j} = \int_{t \in [i,j]}{( \bold{R}_{b_i b_t} \bold{a}^{b_t} ) \delta t}\\\bold{q}_{b_i b_j} = \int_{t \in [i,j]}{ \bold{q}_{b_i b_t} \otimes \begin{bmatrix} 0 \\ \frac{1}{2} \boldsymbol{\omega}^{b_t} \end{bmatrix} \delta t}\tag{6} αbibj=∬t∈[i,j](Rbibtabt)δt2βbibj=∫t∈[i,j](Rbibtabt)δtqbibj=∫t∈[i,j]qbibt⊗[021ωbt]δt(6)
zbibj=[αbibj,βbibj,qbibj]⊤\bold{z}_{b_i b_j} =[ \boldsymbol{\alpha}_{b_i b_j}, \boldsymbol{\beta}_{b_i b_j}, \bold{q}_{b_i b_j} ]^\topzbibj=[αbibj,βbibj,qbibj]⊤ 称为预积分测量,可以在不知道 bib_ibi 的body状态的情况下计算, 这意味着, 即使body状态改变, 也不需要重新预积分. 将这个预积分测量作为连续关键帧之间的约束因子.

预积分模型(式(6))由连续时间导出，忽略了bias和噪声。实际上, IMU的测量是从离散时间得到的，需要考虑噪声。这里使用中值积分来积分IMU测量。利用离散时刻 kkk 和 k+1k+1k+1 的测量值进行IMU body的递推 :
ω^=12((ω^bk−bgbk+ngbk))+(ω^bk+1−bgbk+ngbk+1)q^bibk+1=q^bibk⊗[112ω^δt]a^=12(Rbibk(a^bk−babk+nabk)+Rbibk+1(a^bk+1−babk+1+nabk+1))α^bibk+1=α^bibk+β^bibkδt+12a^δt2β^bibk+1=β^bibk+a^δt(7)\boldsymbol{\hat{\omega}} = \frac{1}{2} ( ( \boldsymbol{\hat{\omega}}^{b_k} - \bold{b}^{b_k}_{\rm g} + \bold{n}^{b_k}_{\rm g} ) )+ ( \boldsymbol{\hat{\omega}}^{b_{k+1}} - \bold{b}^{b_k}_{\rm g} + \bold{n}^{b_{k+1}}_{\rm g} ) \\ \bold{\hat{q}}_{b_i b_{k+1}} = \bold{\hat{q}}_{b_i b_{k}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2} \boldsymbol{\hat{\omega}} \delta t \end{bmatrix} \\ \bold{\hat{a}} = \frac{1}{2} \big( \bold{R}_{b_i b_k} ( \bold{\hat{a}}^{b_k} - \bold{b}^{b_k}_a + \bold{n}^{b_k}_a ) + \bold{R}_{b_i b_{k+1}} ( \bold{\hat{a}}^{b_{k+1}} - \bold{b}^{b_{k+1}}_a + \bold{n}^{b_{k+1}}_a ) \big) \\ \boldsymbol{\hat{\alpha}}_{b_{i} b_{k+1}} = \boldsymbol{\hat{\alpha}}_{b_{i} b_{k}} + \boldsymbol{\hat{\beta}}_{b_{i} b_{k}} \delta t + \frac{1}{2} \bold{\hat{a}} \delta t^2 \\ \boldsymbol{\hat{\beta}}_{b_{i} b_{k+1}} = \boldsymbol{\hat{\beta}}_{b_{i} b_{k}} + \bold{\hat{a}} \delta t\tag{7} ω^=21((ω^bk−bgbk+ngbk))+(ω^bk+1−bgbk+ngbk+1)q^bibk+1=q^bibk⊗[121ω^δt]a^=21(Rbibk(a^bk−babk+nabk)+Rbibk+1(a^bk+1−babk+1+nabk+1))α^bibk+1=α^bibk+β^bibkδt+21a^δt2β^bibk+1=β^bibk+a^δt(7)

开始时, k=ik=ik=i , 有 qbibj=[0,0,0,1]⊤\bold{q}_{b_i b_j} =[0,0,0,1]^\topqbibj=[0,0,0,1]⊤ , αbibi\boldsymbol{\alpha}_{b_{i} b_{i}}αbibi, βbibi\boldsymbol{\beta}_{b_{i} b_{i}}βbibi 设置为0. 公式(7)中, 为了高效地计算预积分, 设置bias在两帧之间是常量:
babk=babk+1,bgbk=bgbk+1,k∈[i,j−1](8)\bold{b}^{b_{k}}_a = \bold{b}^{b_{k+1}}_a, \quad \bold{b}^{b_{k}}_{\rm g} = \bold{b}^{b_{k+1}}_{\rm g}, \quad k \in [i, j-1]\tag{8} babk=babk+1,bgbk=bgbk+1,k∈[i,j−1](8)
在实际中, 由于bias改变的很慢. 因此将bias建模为随机游走噪声:
babk+1=babk+nbaδt,bgbk+1=bgbk+nbgδt(9)\bold{b}^{b_{k+1}}_a = \bold{b}^{b_{k}}_a + \bold{n}_{\bold{b}_a} \delta t, \quad \bold{b}^{b_{k+1}}_{\rm g} = \bold{b}^{b_{k}}_{\rm g} + \bold{n}_{\bold{b}_{\rm g}} \delta t\tag{9} babk+1=babk+nbaδt,bgbk+1=bgbk+nbgδt(9)
其中高斯白噪声定义为 nba∈N(0,σba2)\bold{n}_{\bold{b}_a} \in \mathcal{N}(0, \sigma^2_{b_a})nba∈N(0,σba2) , nbg∈N(0,σbg2)\bold{n}_{\bold{b}_g} \in \mathcal{N}(0, \sigma^2_{b_g})nbg∈N(0,σbg2) . bias通过小的增量来更新预积分, 使用一阶近似来更新 q^bibj\bold{\hat{q}}_{b_{i} b_{j}}q^bibj , α^bibj\boldsymbol{\hat{\alpha}}_{b_{i} b_{j}}α^bibj , β^bibj\boldsymbol{\hat{\beta}}_{b_{i} b_{j}}β^bibj .
α^bibj←α^bibj+Jbaiαδbabi+Jbgiαδbgbiβ^bibj←β^bibj+Jbaiβδbabi+Jbgiβδbgbiq^bibj←q^bibj⊗[112Jbgiqδbgbi](10)\boldsymbol{\hat{\alpha}}_{b_{i} b_{j}} \leftarrow \boldsymbol{\hat{\alpha}}_{b_{i} b_{j}} + \bold{J}^{\alpha}_{b^i_a} \delta \bold{b}^{b_i}_a + \bold{J}^{\alpha}_{b^i_{\rm g}} \delta \bold{b}^{b_i}_{\rm g}\\\boldsymbol{\hat{\beta}}_{b_{i} b_{j}} \leftarrow \boldsymbol{\hat{\beta}}_{b_{i} b_{j}} + \bold{J}^{\beta}_{b^i_a} \delta \bold{b}^{b_i}_a + \bold{J}^{\beta}_{b^i_{\rm g}} \delta \bold{b}^{b_i}_{\rm g}\\\bold{\hat{q}}_{b_{i} b_{j}} \leftarrow \bold{\hat{q}}_{b_{i} b_{j}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2} \bold{J}^{q}_{b^i_{\rm g}} \delta \bold{b}^{b_i}_{\rm g} \end{bmatrix}\tag{10} α^bibj←α^bibj+Jbaiαδbabi+Jbgiαδbgbiβ^bibj←β^bibj+Jbaiβδbabi+Jbgiβδbgbiq^bibj←q^bibj⊗[121Jbgiqδbgbi](10)
其中 Jbaiα=∂αbibj∂δbabi\bold{J}^{\alpha}_{b^i_a} = \frac{\partial \boldsymbol{\alpha}_{b_i b_j} }{\partial \delta \bold{b}^{b_i}_a}Jbaiα=∂δbabi∂αbibj , Jbgiα=∂αbibj∂δbgbi\bold{J}^{\alpha}_{b^i_{\rm g}} = \frac{\partial \boldsymbol{\alpha}_{b_i b_j} }{\partial \delta \bold{b}^{b_i}_{\rm g}}Jbgiα=∂δbgbi∂αbibj , Jbaiβ=∂βbibj∂δbabi\bold{J}^{\beta}_{b^i_a} = \frac{\partial \boldsymbol{\beta}_{b_i b_j} }{\partial \delta \bold{b}^{b_i}_a}Jbaiβ=∂δbabi∂βbibj , Jbgiβ=∂βbibj∂δbgbi\bold{J}^{\beta}_{b^i_{\rm g}} = \frac{\partial \boldsymbol{\beta}_{b_i b_j} }{\partial \delta \bold{b}^{b_i}_{\rm g}}Jbgiβ=∂δbgbi∂βbibj , Jbgiq=∂qbibj∂δbgbi\bold{J}^{q}_{b^i_{\rm g}}=\frac{\partial \bold{q}_{b_i b_j} }{\partial \delta \bold{b}^{b_i}_{\rm g}}Jbgiq=∂δbgbi∂qbibj 是预积分测量相对于时刻 iii 的bias的雅可比矩阵. 这可以通过误差状态变换矩阵推导出来, 见附录A. 预积分测量的协方差的计算也是在附录A.

附录A

误差状态递推

预积分的协方差

线的几何表示

一条空间直线有4个自由度, 因此最少需要4个参数来表示. 这里将3D空间中的直线视为无限直线，并对一条3D直线采用两种参数化方法。(1) Plücker线坐标由6个参数组成，用于变换和投影。(2) 由4个参数组成的标准正交表示法由于优化, 由于其紧凑性。

Plücker线坐标

如图2a所示. 一个空间直线 L\mathcal{L}L 在Plücker线坐标中表示为 L=(n⊤,d⊤)⊤∈R\mathcal{L} = (\bold{n}^\top,\bold{d}^\top)^\top \in \mathbb{R}L=(n⊤,d⊤)⊤∈R , 其中 d∈R3\bold{d} \in \mathbb{R}^3d∈R3 是线的方向向量, n∈R3\bold{n} \in \mathbb{R}^3n∈R3 是由线和坐标原点确定的平面点法向量. Plücker坐标是过参数的, 因为在向量 n\bold{n}n 和 d\bold{d}d 之间有一个隐式的约束 n⊤d=0\bold{n}^\top \bold{d} = 0n⊤d=0 . 因此 Plücker坐标无法在无约束优化时使用. 尽管如此, 用一个法向量和一个方向向量表示一条三维直线，从两个视图进行几何三角化很简单，而且对直线几何变换建模也很方便。

对于线几何变换, 给定变换矩阵 Tcw=[Rcwpcw01]\bold{T}_{cw} = \begin{bmatrix} \bold{R}_{cw} & \bold{p}_{cw} \\ \bold{0} & \bold{1} \end{bmatrix}Tcw=[Rcw0pcw1] 表示世界坐标系 www 到相机坐标系 ccc 的变换, 可以对 Plücker坐标进行变换:
Lc=[ncdc]=TcwLw=[Rcw[pcw]×Rcw0Rcw]Lw(11)\mathcal{L}^{c} = \begin{bmatrix} \bold{n}^{c} \\ \bold{d}^{c} \end{bmatrix} = \mathcal{T}_{cw} \mathcal{L}_{w}= \begin{bmatrix} \bold{R}_{cw} & [\bold{p}_{cw}]_{\times} \bold{R}_{cw} \\ \bold{0} & \bold{R}_{cw} \end{bmatrix} \mathcal{L}^{w}\tag{11} Lc=[ncdc]=TcwLw=[Rcw0[pcw]×RcwRcw]Lw(11)
其中 [⋅]×[\cdot]_{\times}[⋅]× 表示3D向量的反对称矩阵, Tcw\mathcal{T}_{cw}Tcw 是用于将直线从坐标系 www 变换到 ccc 的变换矩阵.

当在两个不同的相机中观察到一个新的线路标时，可以很容易地计算Plücker坐标。如图2b所示, 空间直线 L\mathcal{L}L 在相机 c1c_1c1 和 c2c_2c2 的投影为 zLc1\bold{z}^{c_1}_{\mathcal{L}}zLc1 和 zLc2\bold{z}^{c_2}_{\mathcal{L}}zLc2 . 线段 zLc1\bold{z}^{c_1}_{\mathcal{L}}zLc1 在归一化图像平面上可以由两个端点表示 sc1=[us,vs,1]⊤\bold{s}^{c_1}=[u_s,v_s,1]^\topsc1=[us,vs,1]⊤ 和 ec1=[ue,ve,1]⊤\bold{e}^{c_1}=[u_e,v_e,1]^\topec1=[ue,ve,1]⊤ . 三个不共线的点, 包括线段的两个端点和坐标系原点 C=[x0,y0,z0]⊤\bold{C} = [x_0,y_0,z_0]^\topC=[x0,y0,z0]⊤ 共同确定一个3D空间中的一个平面 π=[πx,πy,πz,πw]⊤\boldsymbol{\pi} =[\pi_x,\pi_y,\pi_z,\pi_w]^\topπ=[πx,πy,πz,πw]⊤ :
πx(x−x0)+πy(y−y0)+πz(z−z0)=0(12)\pi_x (x-x_0)+\pi_y (y-y_0)+\pi_z (z-z_0)=0\tag{12} πx(x−x0)+πy(y−y0)+πz(z−z0)=0(12)
其中
[πxπyπz]=[sc1]×ec1,πw=πxx0+πyy0+πzz0(13)\begin{bmatrix} \pi_x \\ \pi_y \\ \pi_z \end{bmatrix}= [\bold{s}^{c_1}]_{\times} \bold{e}^{c_1}, \ \pi_w = \pi_x x_0 + \pi_y y_0 + \pi_z z_0 \tag{13} ⎣⎡πxπyπz⎦⎤=[sc1]×ec1, πw=πxx0+πyy0+πzz0(13)
给定在相机坐标系上的两个平面 π1\boldsymbol{\pi}_1π1 和 π2\boldsymbol{\pi}_2π2 , 对偶Plücker矩阵 L∗\bold{L}^*L∗ 计算如下:
L∗=[[d]×n−n⊤0]=π1π2⊤−π2π1⊤∈R4×4(14)\bold{L}^* = \begin{bmatrix} [\bold{d}]_{\times} & \bold{n} \\ -\bold{n}^\top & 0 \end{bmatrix}= \boldsymbol{\pi}_1 \boldsymbol{\pi}_2^\top - \boldsymbol{\pi}_2 \boldsymbol{\pi}_1^\top\in \mathbb{R}^{4\times4}\tag{14} L∗=[[d]×−n⊤n0]=π1π2⊤−π2π1⊤∈R4×4(14)
在相机坐标系 c1c_{1}c1 中的Plücker坐标可以很容易从对偶Plücker矩阵 L∗\bold{L}^*L∗ 中得到.

正交表示(orthonormal representation)

由于空间直线仅有4DOF, 因此在优化时正交表示 (U,W)∈SO(3)×SO(2)(\bold{U},\bold{W}) \in SO(3) \times SO(2)(U,W)∈SO(3)×SO(2) 比Plücker坐标更合适. 正交表示和Plücker坐标可以相互转换，这意味着我们可以在一个SLAM系统中同时采用这两种表示来实现不同的目的。如图2a所示，在3D线上定义了一个坐标系。归一化法向量和归一化方向向量是坐标系的两个轴。第三个轴是由另外两个轴向量的交点决定的。定义线坐标和相机坐标系的旋转矩阵为 U\bold{U}U :
U=R(ψ)=[n∥n∥d∥d∥n×d∥n×d∥](15)\bold{U} = \bold{R}(\boldsymbol{\psi}) = \begin{bmatrix} \frac{\bold{n}}{\Vert \bold{n} \Vert} & \frac{\bold{d}}{\Vert \bold{d} \Vert} & \frac{\bold{n \times d}}{\Vert \bold{n \times d} \Vert} \end{bmatrix}\tag{15} U=R(ψ)=[∥n∥n∥d∥d∥n×d∥n×d](15)
其中 ψ=[ψ1,ψ2,ψ3]⊤\boldsymbol{\psi} = [\psi_1,\psi_2,\psi_3]^\topψ=[ψ1,ψ2,ψ3]⊤ 由围绕着相机坐标系的x, y ,z轴的旋转角组成. Plücker坐标和 U\bold{U}U 的关系为:
[nd]=[n∥n∥d∥d∥n×d∥n×d∥][∥n∥00∥d∥00](16)\begin{bmatrix} \bold{n} & \bold{d} \end{bmatrix} =\begin{bmatrix} \frac{\bold{n}}{\Vert \bold{n} \Vert} & \frac{\bold{d}}{\Vert \bold{d} \Vert} & \frac{\bold{n \times d}}{\Vert \bold{n \times d} \Vert} \end{bmatrix}\begin{bmatrix} \Vert \bold{n} \Vert & 0 \\ 0 & \Vert \bold{d} \Vert \\ 0 & 0 \end{bmatrix}\tag{16} [nd]=[∥n∥n∥d∥d∥n×d∥n×d]⎣⎡∥n∥000∥d∥0⎦⎤(16)
由于上式中 (∥n∥,∥d∥)(\Vert \bold{n} \Vert , \Vert \bold{d} \Vert)(∥n∥,∥d∥) 的联合仅有1DOF, 可以使用三角函数来表示它:
W=[cos⁡(ϕ)−sin⁡(ϕ)sin⁡(ϕ)cos⁡(ϕ)]=1(∥n∥2+∥d∥2)[∥n∥−∥d∥∥d∥∥n∥](17)\bold{W} = \begin{bmatrix} \cos(\phi) & -\sin(\phi) \\ \sin(\phi) & \cos(\phi) \end{bmatrix} = \frac{1}{\sqrt{ ( \Vert \bold{n} \Vert^2 + \Vert \bold{d} \Vert^2 ) }} \begin{bmatrix} \Vert \bold{n} \Vert & -\Vert \bold{d} \Vert \\ \Vert \bold{d} \Vert & \Vert \bold{n} \Vert \end{bmatrix}\tag{17} W=[cos(ϕ)sin(ϕ)−sin(ϕ)cos(ϕ)]=(∥n∥2+∥d∥2)1[∥n∥∥d∥−∥d∥∥n∥](17)
其中 ϕ\phiϕ 是旋转角. 由于坐标原点到3D直线的距离为 d=∥n∥∥d∥d=\frac{\Vert \bold{n} \Vert}{\Vert \bold{d} \Vert}d=∥d∥∥n∥ , 所以 W\bold{W}W 包含了距离d的信息. 根据 U\bold{U}U 和 W\bold{W}W 的定义, 空间直线的4DoF包括来自旋转矩阵的3DoF，它将线坐标转换到相机坐标系，还有来自距离d的1DoF。使用 O=[ψ,ϕ]⊤\mathcal{O} = [\boldsymbol{\psi},\phi]^\topO=[ψ,ϕ]⊤ 作为在优化时3D直线的最小表示.

一旦3D直线使用正交表示优化之后, 对应的Plücker坐标计算如下:
L′=[w1u1T,w2u2T]T=1(∥n∥2+∥d∥2)L\mathcal{L}' = [w_1 \bold{u}^T_1 , w_2 \bold{u}^T_2]^T = \frac{1}{\sqrt{ ( \Vert \bold{n} \Vert^2 + \Vert \bold{d} \Vert^2 ) }} \mathcal{L} L′=[w1u1T,w2u2T]T=(∥n∥2+∥d∥2)1L
其中 ui\bold{u}_iui 是矩阵 U\bold{U}U 的第 iii 行, w1=cos⁡(ϕ)w_1 = \cos(\phi)w1=cos(ϕ) , w2=sin⁡(ϕ)w_2 = \sin(\phi)w2=sin(ϕ) . 在 L\mathcal{L}L 和 L′\mathcal{L}'L′ 还有一个尺度因子, 但是它们都表示同一个3D直线.

紧耦合视觉惯性融合

在视觉SLAM中，BA通过最小化图像平面的重投影误差来优化相机位姿和三维地图点。BA的非线性优化可以看做一个因子图，如图3a所示: 圆形节点是需要优化的相机位姿或3D路标; 带有方框的边表示视觉测量作为节点之间的约束。对于视觉惯性融合，也可以使用紧耦合的基于图的框架来优化视觉惯性系统的所有状态变量。如图3b所示，因子图中不仅包含视觉测量值，还将预积分的IMU测量值作为边，约束连续的IMU body状态。

滑动窗口的形式

为了提高计算效率和计算精度，这里采用滑动窗口公式进行因子图优化。滑动窗口在i时刻的完整状态变量定义为:

其中 xi\bold{x}_ixi 表示IMU body在世界坐标系下的位置,速度,方向, 以及在IMU body坐标系下的bias. 下标 n,m,o是body状态,点路标,线路标的开始索引. N是在滑动窗口中关键帧的数量. M和O分别为滑动窗口中所有关键帧观测到的点路标和线路标的数量。仅使用一个逆深度 λk\lambda_kλk 来参数化第 k 个点路标. Ol\mathcal{O}_lOl 是第 lll 个直线特征点正交表示.

通过最小化所有测量残差的代价函数之和来优化滑动窗口中的所有状态变量:

其中 rb(zbibi+1,χ)\bold{r}_b(\bold{z}_{b_{i}b_{i+1}},\boldsymbol{\chi})rb(zbibi+1,χ) 是body状态 xi\bold{x}_ixi 和 xi+1\bold{x}_{i+1}xi+1 之间的IMU测量值残差. BBB 是滑动窗口内IMU的预积分的集合. rf(zfjci,χ)\bold{r}_f(\bold{z}^{c_i}_{\bold{f}_j},\boldsymbol{\chi})rf(zfjci,χ) 是点特征的重投影误差, rl(zLici,χ)\bold{r}_l (\bold{z}^{c_i}_{\mathcal{L}_i},\boldsymbol{\chi})rl(zLici,χ) 是线特征点重投影误差. F和L是当前帧观测到的点特征的集合和线特征的集合. {rp,Jp}\{\bold{r}_p,\bold{J}_p \}{rp,Jp} 是先验信息, 这是在边缘化某一帧后得到的, Jp\bold{J}_pJp 是先验雅可比矩阵, 从之前的优化中得到的Hessian矩阵中得来的. ρ\rhoρ 是Cauchy鲁棒函数用于抑制外点.

使用Levenberg Marquard算法来解决非线性优化问题(20)。最优状态估计 χ\boldsymbol{\chi}χ 可由初始估计 χ0\boldsymbol{\chi}_0χ0 迭代更新为:
χt+1′=χt⊗δχ\boldsymbol{\chi}'_{t+1} = \boldsymbol{\chi}_{t} \otimes \delta \boldsymbol{\chi} χt+1′=χt⊗δχ
⊗\otimes⊗ 算子使用增量 δχ\delta \chiδχ 来更新参数. 对于位置,速度,bias, 逆深度, 更新算子的定义为:
p′=p+δp,v′=v+δv,λ′=λ+δλ,b′=b+δb(22)\bold{p}' = \bold{p} + \delta \bold{p},\bold{v}' = \bold{v} + \delta \bold{v}, \lambda'=\lambda + \delta \lambda , \bold{b}' = \bold{b} + \delta \bold{b}\tag{22} p′=p+δp,v′=v+δv,λ′=λ+δλ,b′=b+δb(22)
对于状态 q\bold{q}q 的更新要更复杂一些. 四元数中使用了4个参数来表示3DOF旋转，所以它是过参数化的。旋转的增量应该是三维的。使用正切空间(tangent space)上的扰动 δθ∈R3\delta \boldsymbol{\theta} \in \mathbb{R}^3δθ∈R3 作为旋转增量。因此，旋转 q\bold{q}q 可以通过四元数乘法来更新
q′=q⊗δq,δq≈[112δθ](23)\bold{q}' = \bold{q} \otimes \delta \bold{q}, \ \delta \bold{q} \approx \begin{bmatrix} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta} \end{bmatrix}\tag{23} q′=q⊗δq, δq≈[121δθ](23)
旋转矩阵的形式为:
R′≈R(I+[δθ]×)(24)\bold{R}' \approx \bold{R} ( \bold{I} + [\delta \boldsymbol{\theta}]_{\times} ) \tag{24} R′≈R(I+[δθ]×)(24)
其中 I\bold{I}I 是3x3单位阵. 类似的, 可以更新正交表示:
U′≈U(I+[δψ]×)W′≈W(I[0−δϕδϕ0])(25)\bold{U}' \approx \bold{U} (\bold{I} + [\delta \boldsymbol{\psi}]_{\times}) \\\bold{W}' \approx \bold{W} \Big( \bold{I} \begin{bmatrix} 0 & -\delta \phi \\ \delta \phi & 0 \end{bmatrix} \Big) \tag{25} U′≈U(I+[δψ]×)W′≈W(I[0δϕ−δϕ0])(25)
正交表示第增量为 δO=[[δψ]×,δϕ]⊤\delta \mathcal{O}=[ [\delta \boldsymbol{\psi}]_{\times},\ \delta \phi ]^\topδO=[[δψ]×, δϕ]⊤ . 最后,增量 δχ\delta \chiδχ 定义为:

在每次迭代时, 增量 δχ\delta \chiδχ 可以通过公式(20)来求解:
(Hp+Hb+Hf+Hl)δχ=(bp+bb+bf+bl)(27)(\bold{H}_p + \bold{H}_b + \bold{H}_f + \bold{H}_l) \delta \chi = (\bold{b}_p + \bold{b}_b + \bold{b}_f + \bold{b}_l) \tag{27} (Hp+Hb+Hf+Hl)δχ=(bp+bb+bf+bl)(27)
其中 Hp\bold{H}_pHp , Hb\bold{H}_bHb , Hf\bold{H}_fHf , Hl\bold{H}_lHl 分别是先验残差,IMU测量残差,点重投影误差,线重投影误差的Hessian矩阵. 对于残差 r(⋅)\bold{r}_{(\cdot)}r(⋅) , 有 H(⋅)=J(⋅)⊤Σ(⋅)−1J(⋅)\bold{H}_{(\cdot)}=\bold{J}_{(\cdot)}^\top \bold{\Sigma}_{(\cdot)}^{-1} \bold{J}_{(\cdot)}H(⋅)=J(⋅)⊤Σ(⋅)−1J(⋅) , b(⋅)=J(⋅)⊤Σ(⋅)−1r(⋅)\bold{b}_{(\cdot)}=\bold{J}_{(\cdot)}^\top \bold{\Sigma}_{(\cdot)}^{-1} \bold{r}_{(\cdot)}b(⋅)=J(⋅)⊤Σ(⋅)−1r(⋅) , 其中 J(⋅)\bold{J}_{(\cdot)}J(⋅) 是残差向量 r(⋅)\bold{r}_{(\cdot)}r(⋅) 相对于 δχ\delta \chiδχ 的雅可比矩阵, Σ(⋅)\bold{\Sigma}_{(\cdot)}Σ(⋅) 是测量的协方差矩阵.

IMU测量模型

由于由式(10)计算的预积分IMU测量值是两个连续关键帧 bi\bold{b}_ibi 和 bj\bold{b}_jbj 之间的约束因子，因此可以定义IMU测量残差为:

其中 [⋅]xyz[\cdot]_{xyz}[⋅]xyz 抽取四元数的的XYZ部分,用于估计3D旋转误差.

在非线性优化过程中，IMU测量残差相对于body状态 xi\bold{x}_ixi 和 xj\bold{x}_jxj 的雅可比矩阵:

其中 [q]L[\bold{q}]_L[q]L 和 [q]R[\bold{q}]_R[q]R 是左乘和右乘四元数矩阵. ⌊⋅⌋3×3\lfloor \cdot \rfloor _{3\times3}⌊⋅⌋3×3 用于抽取(.)的右下块矩阵. 雅可比矩阵的计算为:
Jbgirθ=∂rθ∂δbgbi=⌊−[qwbj−1⊗qwbi⊗qbibj]L⌋3×3Jbgiq\bold{J}_{b^i_{\rm g}}^{\bold{r}_\theta} = \frac{\partial \bold{r}_\theta}{ \partial \delta \bold{b}^{b_i}_{\rm g} } =\big \lfloor -[ \bold{q}^{-1}_{w b_j} \otimes \bold{q}_{w b_i} \otimes \bold{q}_{b_i b_j} ]_{L} \big \rfloor _{3\times3} \bold{J}^{q}_{b^i_{\rm g}} Jbgirθ=∂δbgbi∂rθ=⌊−[qwbj−1⊗qwbi⊗qbibj]L⌋3×3Jbgiq

点特征测量模型

对于点特征，3D点的重投影误差为投影点与观测点之间的图像距离。这里在归一化的图像平面上处理的点和线特征测量。给定在 cjc_jcj 上的第k个点特征测量 zfkcj=[ufkcj,vfkcj,1]⊤\bold{z}^{c_j}_{\bold{f}_k}=[u^{c_j}_{\bold{f}_k},v^{c_j}_{\bold{f}_k},1]^\topzfkcj=[ufkcj,vfkcj,1]⊤ ,重投影误差定义为:

其中 zfci=[ufkci,vfkci,1]⊤\bold{z}^{c_i}_{\bold{f}}=[u^{c_i}_{\bold{f}_k},v^{c_i}_{\bold{f}_k},1]^\topzfci=[ufkci,vfkci,1]⊤ 是相机第一次观测到该路标, 且落在相机坐标系 cic_ici , 且该特征点逆深度 λk\lambda_kλk 也定义在 cic_ici .

为了最小化点重投影误差(30)，我们需要优化坐标系 bib_ibi 、bjb_jbj 的旋转和位置，以及特征逆深度反 λ\lambdaλ 。用链式法则可以得到相应的雅可比矩阵:

其中 fbi\bold{f}^{b_i}fbi 是在第i个IMU body坐标系的3D点向量. 定义点测量的协方差矩阵 Σfkci\Sigma^{c_i}_{\bold{f}_k}Σfkci 为一个2x2的对角矩阵, 这假设检测到的点特征在图像平面的垂直和水平方向上都有像素噪声。

线特征测量模型

直线测量的重投影误差定义为端点到投影直线的距离。对于针孔相机, 一个3D空间直线 L=[n,d]⊤\mathcal{L}=[\bold{n},\bold{d}]^\topL=[n,d]⊤ 可以投影到图像平面:
l=[l1l2l3]=Knc=[fy000fx0−fycx−fxcyfxfy]nc(33)\bold{l} = \begin{bmatrix} l_1 \\ l_2 \\ l_3 \end{bmatrix} = \mathcal{K} \bold{n}^c = \begin{bmatrix} f_y & 0 & 0 \\ 0 & f_x & 0 \\ -f_y c_x & -f_x c_y & f_x f_y \end{bmatrix} \bold{n}^c \tag{33} l=⎣⎡l1l2l3⎦⎤=Knc=⎣⎡fy0−fycx0fx−fxcy00fxfy⎦⎤nc(33)
其中 K\mathcal{K}K 是线特征点投影矩阵. 当投影一条直线到归一化平面时, K\mathcal{K}K 是单位矩阵. 从上面的投影方程中, 3D直线投影的线段坐标只与法向量n\bold{n}n有关。

给定一条3D直线 Llw\mathcal{L}^{w}_{l}Llw ,其在世界坐标系中的正交表示为 Ol\mathcal{O}_lOl ,首先使用(11)将其变换到相机坐标系 cic_ici 下. 然后, 将其投影到图像平面得到投影到直线 llci\bold{l}^{c_i}_{l}llci .在帧i中的重投影误差为:
rl(zLlci,χ)=[d(slci,llci)d(elci,llci)](34)\bold{r}_l (\bold{z}^{c_i}_{\mathcal{L}_l}, \chi) = \begin{bmatrix} d(\bold{s}^{c_i}_{l} ,\bold{l}^{c_i}_{l}) \\ d(\bold{e}^{c_i}_{l} ,\bold{l}^{c_i}_{l}) \end{bmatrix} \tag{34} rl(zLlci,χ)=[d(slci,llci)d(elci,llci)](34)
其中 d(s,l)d(\bold{s},\bold{l})d(s,l) 表示从端点 s\bold{s}s 到投影直线 l\bold{l}l 的距离函数.
d(s,l)=s⊤ll12+l22(35)d(\bold{s},\bold{l}) = \frac{\bold{s}^{\top} \bold{l}}{ \sqrt{l^2_1 + l^2_2} } \tag{35} d(s,l)=l12+l22s⊤l(35)
第i个body 状态和第l个直线参数可以通过最小化线重投影误差 rl(zLlci)\bold{r}_l (\bold{z}^{c_i}_{\mathcal{L}_l})rl(zLlci) 得到. 对应的雅可比矩阵可由链式法则得到:

推导的细节在附录B中提供。与点测量协方差矩阵相似，线测量协方差矩阵 ΣLlci\Sigma^{c_i}_{\mathcal{L}_l}ΣLlci 是通过假设线段的端点有像素噪声来定义的。

附录B

点线特征的单目VIO

如图4所示，提出的PL-VIO系统有两个主要模块:前端和后端。前端模块用于对IMU和相机的测量数据进行预处理。后端模块用于估计和优化body状态。我们将在下面的小节中介绍细节。

前端

前端从相机和IMU的原始测量数据中提取信息。每一次新的IMU测量都会通过递推更新body状态，并将最新的body状态作为滑动窗口优化的初始值。此外，在优化过程中对新的IMU测量值进行了预积分，以约束后续IMU body状态。

在图像处理中，点特征和线特征分别在两个线程中检测。当有新帧出现时，点特征利用KLT光流算法从前一帧跟踪到新帧. 然后，使用带有基本矩阵测试的RANSAC框架来去除outliers。如果外点剔除后跟踪到的点特征数量小于阈值，则加入由FAST检测器检测到的新的角点特征。

线段特征用LSD检测器检测新一帧的线段，用基于外观的描述子LBD与前一帧线段进行匹配。使用几何约束来去除线匹配的outliers. 比如, 两个匹配的线的中点的距离应该不超过阈值 δdistth\delta^{th}_{dist}δdistth 像素, 角度误差应该不超过 δangleth\delta^{th}_{angle}δangleth 度. 经过特征检测和匹配后，将线段的点特征和端点投影到归一化图像平面上。此外，如果跟踪的点特征平均视差大于阈值，则选择一帧作为新的关键帧。

后端

在后端线程中，首先对点和线进行三角化，建立重投影残差。为了获得良好的路标估计，利用所有观测值来估计点特征的逆深度。对于线的三角化，只选择滑动窗口中空间距离最远的两个帧来初始化Plücker坐标。

在我们得到地图中点/线的初始估计和由IMU测量值预测的IMU body状态后，使用第3节中描述的滑动窗口优化来寻找最优状态。为了限制状态向量 χ\boldsymbol{\chi}χ 的大小,采用一个双向边缘化策略来从滑动窗口中去除状态. 当次新帧(第二新的帧) xn+N−1\bold{x}_{n+N-1}xn+N−1 是一个关键帧时, 将最老帧 xnx_nxn 及其测量边缘化掉. 否则，如果次新帧不是关键帧，则丢弃这一帧的视觉测量值，并在预积分测量中保留其IMU测量值。在被边缘化的测量的基础上获得新的先验信息，保留被移除状态的约束信息。

最后，剔除无效的地图点和线。如果一个点的逆深度为负，则从地图中删除这个点。如果一条线的重投影残差超过一个阈值，它将从地图上删除。

实现细节

为了启动VIO系统，采用视觉-惯性对齐方法来恢复IMU初始状态的尺度、重力向量、初始bias和速度。滑动窗口有N = 10个关键帧。每一帧最多有150个点特征和150条线段。线匹配的阈值设置为 δdistth=60\delta^{th}_{dist}=60δdistth=60 像素， δangleth=30∘\delta^{th}_{angle}=30 ^{\circ}δangleth=30∘ 。

由于视觉惯性系统只有4个不可观测的自由度(yaw方向和绝对位置)，对6 DOF的优化方法通过自动沿roll和pitch方向前进来最小化代价函数，从而给roll和pitch方向引入虚假信息。在滑动窗口优化之后，我们通过沿roll和pitch方向的增量将其旋转回来，重置body状态。所有的数值优化是解决使用Levenberg Marquardt方法在Ceres求解程序库。线检测和匹配代码由OpenCV3提供。

实验结果

使用两个公共基准数据集评估了PL-VIO:EuRoc MAV Dataset和PennCOSYVIO Dataset。将PL-VIO方法的精度与目前最先进的3种单目VIO方法进行了比较，ROVIO, OKVIS，VINS-Mono。ROVIO是一种基于EKF的VIO的紧耦合形式。在更新步骤中，它直接利用图像的强度误差来寻找最优状态。OKVIS是一种基于点特征的滑动窗口优化算法，可用于单目或双目模式。VINS-Mono是一个完整的VIO-SLAM系统，利用点特征在滑动窗口中优化IMU状态，并执行回环检测。

所有实验均在Intel Core i7-6700HQ CPU、2.60 GHz、16gb RAM和ROS Kinetic上进行。

EuRoc MAV数据集

EuRoc微型飞行器(MAV)数据集由MAV在两个室内场景中收集，其中包含来自全局快门相机以20FPS的双目图像和同步IMU测量以200hz的数据。每个数据集提供VICON运动捕捉系统给出的ground-truth轨迹。数据集中还提供了所有的外参和内参。这里只使用了左边相机的图像。

线特征的主要优点是它们提供了与环境相关的重要几何结构信息。图5展示了PL-VIO根据MH_05_difficult序列构建的重建图。图5a-d中的4幅图像是由在大厅中飞行的MAV拍摄的，展示了房间的结构。如图5d所示，弱光照场景下的线段检测比点特征检测具有更强的鲁棒性。从重建的三维地图中可以看出，环境的几何形状是用线段来描述的，因此可以从地图中提取语义信息。这对机器人导航很有用。

为了进行定量评估，将PL-VIO与三种最先进的单目视觉惯性方法进行了比较: 在单目模式下ROVIO,OKVIS，以及vins-mono不带回环闭合。为了进行公平的比较，使用了这些比较方法的作者提供的默认参数。选择绝对位姿误差(APE)作为评价指标，直接比较了估计的轨迹误差与gt之间的差异。使用evo评估轨迹.表1给出了沿所有轨迹平移和旋转的均方根误差(RMSE)，并给出了其直方图，如图6所示。

表1显示了联合优化点和线的PL-VIO，它为旋转提供了8个序列的最佳性能，V1_01_easy除外。除V1_01_easy、V1_02_medium和V1_03_difficult外，PL-VIO对6个序列的平移效果最好。表1的结果表明，将线特征集成到VIO中可以提高运动估计的准确性。为了直观地展示结果，图7给出了PL-VIO和VINS-Mono估计的几种轨迹热图。颜色越红，翻译误差越大。通过对三种轨迹的比较，我们得出具有直线特征的PL-VIO在相机快速旋转运动时比vins-mono的误差小。此外，我们发现这些快速旋转的序列会导致视觉方向的较大变化，光照条件对点特征的跟踪尤其具有挑战性。如图8所示，成功匹配27对点(包括10个离群值)，成功匹配33行，全部匹配正确。

表2列出了不同方法的计算时间。基于滤波的ROVIO算法计算效率最高，精度最低。PL-VIO系统是最耗时的方法，但其计算时间主要受行检测和匹配步骤的限制。在5.3节中，以V1_03_difficult序列独立估计了PL-VIO中不同模块的计算时间，发现PL-VIO系统的计算效率直接取决于线的检测和匹配。

PennCOSYVIO Dataset

略

计算时间

最后，评估了以V1_02_medium序列运行PL-VIO的平均执行时间，因为这个图像序列是从一个典型的室内场景收集的。表5显示了每个块的执行时间。线检测与匹配是效率方面的瓶颈，前端运行频率为11hz。目前最先进的线检测和匹配方法，如LSD和LBD的结合，在VIO/SLAM系统中并不令人满意。PL-VIO是独立于线特征检测和匹配的，因此提高它们的效率超出了本文的范围。后端的边缘化是另一个耗时的部分。边缘化的效率低下是由于在边缘化特征时进行了填充，使得Hessian矩阵变得不那么稀疏。这个问题可以通过在执行边缘化以维护稀疏Hessian矩阵时丢弃一些特性来解决。

PL-VIO论文阅读相关推荐

一种GPS辅助的多方位相机的VIO——Slam论文阅读
"A GPS-aided Omnidirectional Visual-Inertial State Estimator in Ubiquitous Environments "论 ...
Learning Multiview 3D point Cloud Registration论文阅读笔记
Learning multiview 3D point cloud registration Abstract 提出了一种全新的,端到端的,可学习的多视角三维点云配准算法. 多视角配准往往需要两个阶段 ...
SLAM 论文阅读和分类整理
前言:以前读论文,都是靠脑子硬记,哪个实验室,谁,哪一年在什么会议上发了一篇关于什么的论文.当需要回溯的时候,每篇论文能给出个大概,不具体,找起来也麻烦,以后就在这个 List 里分类整理已经读过的论 ...
Feature Selective Anchor-Free Module for Single-Shot Object Detection论文阅读翻译 - 2019CVPR
Feature Selective Anchor-Free Module for Single-Shot Object Detection论文阅读翻译文章目录 Feature Selective A ...
Transformer系列论文阅读
这是博主在五一期间对Transformer几篇相关论文阅读的小笔记和总结也借鉴参考了很多大佬的优秀文章,链接贴在文章下方,推荐大家前去阅读该文章只是简单叙述几个Transformer模型的基本框架 ...
论文阅读笔记 | 目标检测算法——SAPD算法
如有错误,恳请指出. 文章目录 1. Introduction 2. Soft Anchor-Point Detector 2.1 Detection Formulation with Anchor ...
论文阅读笔记 | 目标检测算法——FSAF算法
如有错误,恳请指出文章目录 1. Introduction 2. FSAF Module 2.1 Network Architecture 2.2 Ground-truth and Loss 2.2 ...
Structure SLAM 论文阅读
论文阅读整理笔记一.StructVIO : Visual-inertial Odometry with Structural Regularity of Man-made Environments ...
【每日论文阅读】Collaborative Visual Inertial SLAM for Multiple Smart Phones
文章目录论文阅读:Collaborative Visual Inertial SLAM for Multiple Smart Phones 摘要主要贡献算法逻辑算法结果论文阅读:Collab ...
[软件工程程序修复论文阅读]基于代码感知机器翻译的程序修复
本文约2871字,预计阅读时长6分钟. 原文标题为CURE: Code-Aware Neural Machine Translation for Automatic Program Repair 论文 ...

PL-VIO论文阅读