Structure SLAM 论文阅读

一、StructVIO : Visual-inertial Odometry with Structural Regularity of Man-made Environments

I. 摘要

• 基于Atlanta World模型对人工环境规则性进行描述。
• • Atlanta World 定义为包含有几个不同方向的局部Manhattan World世界。
• • 每个局部Manhattan World可以被实时检测，且它们的方向会在新观测到来时由状态估计器不断进行修正。

介绍

1. 检测到的局部Manhattan世界无需是个真实的箱型世界，允许结构线只对准到同一个水平方向上。
2. 对准到局部Manhattan世界的水平线可以提供航向约束，保证航向误差不会在局部区域增加。
3. 即使没有检测到局部的Manhattan世界，垂直线仍然可以指明重力方向，使横滚角和俯仰角可观。

• 主要贡献
• • 提出了一个新颖的线特征最小参数化表示法，融合了Atlanta World假设和结构化特征。每个局部Manhattan世界用一个航向方向来表示，并且会被状态估计器是为一个状态变量不断修正。
• • 结构线（和主方向一致）和局部Manhattan World可以实时在线检测。即使没有检测到Manhattan World，垂直线（vertical line）依旧可以提供位姿信息观测。
• • 对状态估计器和线跟踪方法进行了改进。提出的信息积累方法能够处理长期特征跟踪的观测丢失，保证更好的特征估计；提出的多采样点的线特征跟踪方法和延迟EKF更新让线特征跟踪更加可靠。

II. 相关工作

1. 绝大多数VI-SLAM以点特征为主的原因：1. 点特征广泛存在。2. 点特征易于检测和跟踪。3. 线特征的自由度为4，点特征只有3，因此线特征更难被初始化（尤其是其方向），更难被估计（通常需要6个参数来表示，如普吕克坐标表示法）。在SLAM系统中额外添加线特征的应用，偶尔会带来更糟糕的表现。解决方法有：使用立体相机或多等几个关键帧来进行延迟初始化
2. 有许多提取灭点来改善VI- SLAM性能的工作，但这些研究中的线特征仅仅是灭点提取的过渡结果，提取完灭点后先特征即被抛弃了。因此，若是像点特征一样，把线特征加进估计器里，理应能提升性能。
3. 大多数现有方法只利用了结构特征的部分信息——要么是没有考虑它们的先验方向，要么是仅使用了先验方向而没有作为额外的观测来改善估计结果。
4. 本文所提出的方法不需要在初始化过程中提取任何灭点。而且，新的线参数化方法把每条线特征锚定在了一个局部的manhattan世界上，从而减少了线参数的一个自由度，沿着Manhattan世界方向的线特征方向也可以被修正，作为更可靠的观测被收集。

III. 结构线和Atlanta World

Atlanta World是多个具备不同水平方向 ${\varphi }_i\in [0,\pi /2)$的Manhattan Worlds的组合，如图所示。
我们在里程计起点建立 $Z$ 轴朝上（重力方向相反）的全局世界坐标系 $W$ 。

每条线被锚定在第一次被观测到的图像局部坐标系中，称这个坐标系统为“起始帧” $S$，其方向与这条直线所属的局部Manhattan World的 ${\varphi }_i$ 相同，原点设置为该帧相机的位置。该起始帧原点位置 ${}^{W}ps$， 会在滤波器中继续进行优化估计。

对于锚定在起始帧 $S$ 上的一条给定直线，我们可以找到一个旋转 ${}_L^{S}R$ 来将这条线从参数空间 $L$ 转换到 $S$ 。该条直线被对齐到 $L$ 的 $Z$ 轴上，如下图所示。
在参数空间 $L$ 中，该条线可以用其与$XY$平面的交点来表示，即 ${}^L{l}_p=(a,b,0{)}^T$。这里我们使用逆深度重新表示该条直线，即 $(\theta ,\rho ,0{)}^T$, 其中 $\rho =1/\sqrt{a^2+b^2}$ 和 $\theta =\text{atan2}(b,a)$。

该条线段在起始帧中可以通过以下公式计算：
$$\begin{array}{rl}{}_L^S{R}^L{l}_p& =a{}_L^{S}R(:,1)+b_L^{S}R(:,2)\\ & =\frac{\text{cos}\theta }{\rho }{}_L^{S}R(:,1)+\frac{\text{sin}\theta }{\rho }{}_L^{S}R(:,2)\end{array}$$
对于每个结构线，无论对齐到局部Manhattan World的哪一个轴上，旋转 ${}_L^{S}R$ 一定是下面三个矩阵之一：

$\left[\begin{array}{ccc}0& 0& 1\\ 0& 1& 0\\ -1& 0& 0\end{array}\right]$，$\left[\begin{array}{ccc}1& 0& 0\\ 0& 0& 1\\ 0& -1& 0\end{array}\right]$ 和 $\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$，分别是对齐到$X$,$Y$,$Z$轴。

从起始帧 $S$ 到世界坐标系下的变换由旋转 ${}_S^{W}R({\varphi }_i)$ 和 相机位置${}^{W}Ps$决定，其中 ${}_S^{W}R({\varphi }_i)$是绕重力方向轴的旋转（重力方向可观），即
$${}_S^{W}R({\varphi }_i)=\left[\begin{array}{ccc}\text{cos}{\varphi }_i& \text{sin}{\varphi }_i& 0\\ \text{-sin}{\varphi }_i& \text{cos}{\varphi }_i& 0\\ 0& 0& 1\end{array}\right]$$
对于竖直线段，它们的起始帧坐标轴和世界坐标系保持一致，即${}_S^{W}R=I_{3\times 3}$。我们使用${}_S^{W}R({\varphi }_0)$, $({\varphi }_0=0)$ 来表示这类起始帧。

以下部分属于本文比较关键的部分，结构线参数化和3d空间线与2d观测的关系构建。

为了获取结构线在图像中的投影，可以如下计算：
$${}^W{l}_p={}_S^{W}R({\varphi }_i){}_L^S{R}^L{l}_p+{}^W{p}_S$$and$${}^C{l}_p={{}_W^{C}R}^W{l}_p{+}^C{p}_W$$
用逆深度表示法$(\theta ,\rho ,0{)}^T$ 来取代${}^L{l}_p$，则该 交点(注意这里还是参数空间的平面交点) 在图像平面上的2D投影齐次坐标为：
$${}^C{l}_p\sim {}_W^{C}R{}_S^{W}R({\varphi }_i){}_L^{S}R\cdot r+({}_W^C{R}^W{p}_S+{}^C{p}_W)\cdot \rho$$，其中 $r=[\text{cos}\theta ,\text{sin}\theta ,0{]}^T$。经参数空间$Z$ 轴投影后的消影点齐次坐标计算如下：
$${}^{C}v\sim {}_W^{C}R{}_S^{W}R({\varphi }_i){}_L^{S}R(:,3)$$
考虑相机内参，则图像中的直线表示为：
$${}^{im}l=(K^{-T}){(}^C{l}_p\times {}^{C}v)$$

---

通过上述定义，给定逆深度表示$l=(\theta ,\rho {)}^T$，局部Manhattan World 方向 ${\varphi }_i$ ，以及该直线对齐的轴(用 ${}_L^{S}R$ 描述)等参数，我们在3D线和2D观测之间建立了联系。
直线投影可以写作这些参数的函数，
$${}^{im}l=\Pi (l,{\varphi }_i{,}_L^{S}R,{}_C^I\tau ,{}_I^W\tau )$$
其中${}_C^I\tau$ 为IMU和相机之间的相对位姿变换，可以继续被滤波器优化。

与基于点的EKF方法相比，虚线框为Struct VIO提出的新框架，包括线段检测和分类、Manhattan世界检测和融合、特征边缘化和重新三角化等。

系统概述

状态向量定义为：

• • 其中 $x_{I_k}$为当前的IMU状态向量，包括位姿、速度和陀螺仪加速度计的偏差等。
• • 采用Atlanta世界模型，本方法实时检测每个局部的manhattan世界并将其方向 ${\varphi }_i$ 加入状态向量中并不断优化，共计$N$个。
• • ${}_{I_i}^w\tau$为历史IMU位姿，共计$M$个。当超出窗口阈值时则从状态向量中排除。
• • 所有图像特征，包括点、结构线，都没有包含在状态向量中。会在滤波器外被单独估计且仅用来推导IMU的几何约束。

1. 动力学模型
   与传统EKF基本一致。
2. 测量模型
   包括点特征和线特征，本文只描述结构线的测量模型。为了推导结构线的测量模型，首先需要计算出给定结构线在图像平面的投影。
   假设结构线在第 $k$ 帧图像平面的投影为 ${}^{im}l=(l_1,l_2,l_3{)}^T$ 和对应观测线段的两个端点为 $s_a,s_b$ (齐次坐标)。采用有符号距离来描述观测预测线投影和观测线特征的相似度如下：
   
   (本质上是求两个观测直线端点到预测投影直线的距离所组成的向量)
   通过线性化线特征参数的最新估计，残差 $r_k$ 近似为：
   
   通过堆叠所有可见帧的两侧，可以得到单条结构线的测量方程如下：
   
   然后将残差 $z$ 投影到 $H_l$ 的左零空间来得到一个新的残差 $z^{(0)}$如下：
   
   写成简单形式为：
   
   由此，将结构线本身从状态估计中解耦，从而显著减少了状态变量的维度。
   注意，与点特征不同的是，结构线的测量模型有一个与给定manhattan世界方向相关联的新成分。意味着这个manhattan世界的水平方向可以通过滤波器估计，从而允许我们使用多个箱型世界来建模复杂环境。

由于解析形式过于复杂、难以计算，本文采用数值微分来计算所有的Jacobian矩阵。

3. EKF更新
   有两种EKF更新的触发方式：a) 一条结构线不再被跟踪到：所有的该结构线的量测将被用来进行EKF更新。考虑到偶然的跟踪失败，我们不会立即触发EKF更新，而是进行等待直到跟踪器无法恢复一定数量的帧。这种延迟更新策略显著改善了线跟踪。 b) 位姿数量超出窗口阈值。这种情况下，我们从第二旧帧开始，选择1/3的位姿状态，并且使用所有的对应点特征和线特征来构建量测方程。
4. 状态管理
   同样被两种事件触发：a) 新帧到来：状态增广和协方差矩阵增广。b) 检测到新的manhattan世界：状态增广和协方差矩阵增广。
   注意，由于manhattan世界的方向受多种因素影响，尽管考虑到所有误差因子可以获取准确的对应协方差，但是在实践中采取简单形式：$P_k^{x\varphi }=0$, $P_k^{\varphi }={\sigma }_{\varphi }^2$，其中 ${\sigma }_{\varphi }=5^o$。

关键实现

1. 直线检测和跟踪

• • LSD直线检测
• • 3D-2D匹配来跟踪直线量测。优点是利用了相机运动去预测结构线的可能位置，减少了搜索范围，同时也方便处理偶然的跟踪丢失。对每条结构线，除了之前提到的几何参数，同样引入一个变量 $r=(r_s,r_e{)}^T$ 来表示3d空间中结构线的范围。结构线的两个端点在参数空间的坐标为：${}^L{l}_s=(a,b,r_s{)}^T$ 和 ${}^L{l}_s=(a,b,r_e{)}^T$ 。当新帧图片到来时，结构线将基于IMU预积分预测的相机位姿投影到图片中。下一步就是在新图像中搜索与投影相近的线段，可以通过检查位置和方向差别来实现。我们将会得到一系列的备选线段。然后，将尝试在候选线段中辨别出真实的线段关联（line correspondence）。不仅考虑线段的中点，我们将同时考虑线段上的多个点来提升跟踪的鲁棒性。
• • 我们在结构线的两端点之间上均匀采样为：$r_s,r^{(1)},r^{(2)},...,r^{(K-2)},r_e$ ，对每个采样点，我们保存它在最新一帧的投影附近的图片块，然后通过ZNCC图像匹配方法来在候选线段中搜索它的相关点，如下图所示。最终我们选取相关点最多的线段。

2. 结构线段识别

• • 对于每个结构线段，我们试图找到它们位于哪个局部Manhattan World下，并且分类到不同的方向。
• • 首先，计算某个Manhattan World三个方向的所有消影点。则 $Z$ 轴上的消影点为：
    $$v_z=K{}_W^{C}R[001{]}^T$$同理，对于$X$ 和 $Y$ 方向，计算如下：
    $$\begin{array}{rl}{v}_x^{{\varphi }_i}& =K{}_W^{C}R[\text{cos}{\varphi }_i,\text{-sin}{\varphi }_i,0{]}^T\\ v_y^{{\varphi }_i}& =K{}_W^{C}R[\text{sin}{\varphi }_i,\text{-cos}{\varphi }_i,0{]}^T\end{array}$$ 注意到只有水平方向的消影点才会依赖局部Manhattan World的方向 ${\varphi }_i$ 。因此，即使没有检测到Manhattan World，我们也可以识别出竖直线。
• • 为了从所有的检测线段中识别出结构线，从每个消影点引一条向线段 $S$ 中点的射线，然后检测该射线和线段 $S$ 的一致性。对每条线段都进行所有消影点的测试。一条线段若是与消影点其中一条一致，则判断为结构线。与该线段相关的Manhattan World就可以以对应的消影点确定下来。
• • 当存在多个一致的消影点时选取一致性最高的那个。不属于任何消影点的直线段将从状态估计器中排除。注意即使没有检测出manhattan世界，竖直方向的线段仍然可以被识别出来。

3. 结构线初始化

• • 选择最具信息量的线段用来初始化：1）长度最长的。2）与已初始化线段距离较远的。
• • 初始化一个新结构线 $l=(\theta ,\rho )$ 的关键是找到角度参数 $\theta$ , 因为逆深度值可以设定为一个预设值。初始化第一步是为结构线建立一个起始帧。
• • 对所有竖直方向的结构线，令起始帧与世界坐标系轴对齐，或者建立一个虚拟的Manhattan World坐标系让${\varphi }_0=0$。
• • 对水平方向的结构线，起始帧选择为与局部Manhattan World坐标一致，${\varphi }_i$。
• • 角度参数 $\theta$ 由相机光心到局部参数空间的XY平面上的线的方向决定。该方向可以近似为相机光心到线段 $s$ 中点的方向。$m$ 为 $s$ 中点的齐次坐标，其相机平面的逆投影射线为 $K^{-1}m$，通过下式可以进一步转化到局部参数空间
    $${}^{L}m={}_S^{L}R{}_W^{S}R({\varphi }_i){}_C^{W}R{K}^{-1}m$$其中对于竖直方向的结构线，前两个旋转皆为单位阵，简化为$${}^{L}m={}_C^{W}R{K}^{-1}m$$角度参数 $\theta$ 就可以通过局部参数空间的水平方向来确定。让 ${}^{L}m=(m_x,m_y,m_z{)}^T$, 则角度参数为 ${\theta }_0=\text{atan2}(m_y,m_x)$，逆深度都初始化为 ${\rho }_0$。
• • 将该初始化过程表示如下：
    
    其不确定性可以通过如下协方差矩阵设定：
    
    通常 ${\sigma }_{{\theta }_0}$ 是一个逆投影回来的微小值（相对于线段检测的平均误差2～4个像素）。Manhattan世界方向和当前相机位姿的不确定性可以从滤波器获得。逆深度的不确定性被人工设置为一个较大值 5 ，来覆盖从 0.2米到正无穷的范围。

4. 用先验知识进行结构线三角化

• 每次状态更新后都会进行三角化来更新所有线的参数，将通过最小化所有该线段的可见帧的重投影均方误差和来完成。若仅使用状态内的可见帧来进行三角化，则可能因为较小的运动视差而导致不准确的三角化结果；若使用全部可见帧来进行三角化，则会显著增加计算消耗，且滑窗外的较老的位姿估计也可能导致更大的误差。
• 为了解决上述问题，我们将为每条结构线维护一个先验分布 $N(l_0,{\Sigma }_0)$ (线参数和协方差），来保存初始的先验分布，或者是从历史观测中推导出的先验分布。
• 总的目标函数为：
  
  即使初始值偏离真实值较远（由于逆深度参数化引入的高线性化），该非线性最小二乘问题也可以用高斯牛顿法在3-5个迭代中求解。

5. 处理长时间跟踪的线的观测丢失

• 与滑动窗口估计器类似，本文提出估计器存在的问题是：特征被跟踪的时间可能长于状态估计器中存储的图像帧的总时间。在现存的滑动窗口估计器中，这些滑窗外的观测会被简单的抛弃，因此可能导致线参数的不准确估计。在[27]中，作者将这些特征加进滤波器状态向量中（类似经典EKF-based框架）。缺点就是需要严格控制特征点的数量，以控制状态向量维度不会过高。
• 本文提出一种新方法，将这些丢失的观测转化成几何信息的先验分布，来辅助未来的更新。称这个过程为信息积累（information accumulation）。
• $D$ 为将要从滑动窗口中移除的一系列位姿，$l_0^{old}$ 和 ${\Sigma }_0^{old}$ 为旧先验分布。当 $D$ 帧集合从滑动窗口中移除的时候，先验分布将进行更新以保留被移除观测的几何约束信息。
• 新的均值 $l_0^{new}$ 将通过最小化目标函数计算如下：
  
  与前一个目标函数不同，该公式的最小化求解仅作用于丢失的观测。 使 $\Lambda \delta l=Y$ 为最后一次高斯牛顿法迭代求解出的正规方程。在下一次三角化之前，将新的协方差更新为：${\Sigma }_0^{new}={\Lambda }^{-1}$
• 每条结构线都会锚定在状态向量中的其中一个相机位姿（起始帧）。若该起始帧从状态向量中移除，我们需要首先改变其起始帧为状态向量中存在的某帧，并更新相应的线参数和协方差。

6. 检测与合并Manhattan世界

• Manhattan世界的检测要通过聚类平行线来提取消影点，然后借助消影点来确定三轴方向。若IMU可用，则这个过程可以变得更简单，因为加速度计提供了可观的垂直重力方向。
• 只要垂直方向被确定下来，就会开启Manhattan世界检测。水平地面的消影线（世界坐标系下的XY平面）计算如下：
  (法向量)
• 然后，使用 1-line的 ransac算法来检测可能的manhattan世界，步骤如下：
• • 随机选取一条未被识别成结构线的线段，将其延伸与水平消影线 $l_{\infty }$ 相较于一个消影点 $v_x$，我们假设它就是该可能的manhattan世界的x轴方向的投影。因为垂直方向已知，我们可以进一步得到manhattan世界方向 $\varphi$ 和 $v_y$。
• • 获取与 $v_x$ 和 $v_y$ 方向一致的线段的数量。
• • 重复上述步骤直到迭代最大次数。
• 最终，若最大一致线段超出阈值，则manhattan世界被确定。
• 让 ${\varphi }^{\ast }$ 为检测出的manhattan世界的方向，若与现存的manhattan世界方向偏差大于 $5^o$，该manhattan世界方向 ${\varphi }^{\ast }$ 将被加进状态向量中，协方差也如前文所述进行更新。
• 在EKF更新后，两个manhattan世界的方向偏差偶尔会小于阈值。这种情况下，我们将合并两个manhattan世界（移除最新的那个并且更新协方差，相应的结构线也会被移动到剩余的那个manhattan世界）。

7. 外点排除
   在EKF更新前将通过卡方检验来检测外点：
   
   没有通过卡方检验的结构线将被排除。在EKF更新后，我们将重新三角化所有的结构线，然后进一步检查重投影误差。重投影误差大于一定阈值（本文为4个像素）的将被抛弃。

二、Leveraging Structural Information to Improve Point Line Visual-Inertial Odometry（2021）

摘要

• 和线特征一样，结构线特征同样能帮助改善基于点特征的视觉惯性里程计系统。本文提出了一个基于点和线的VIO系统，分为结构线和非结构线。
• 与 4 参数的正交表示法不同，我们只使用了两个参数来最小化结构线和非结构线的表示方法。
• 我们设计了一种基于采样点的直线匹配策略，来改善直线匹配的效率和成功率。

I. 引言

• 在一些纹理缺失和照明挑战的环境里，基于点的VIO会产生位姿估计失败。
• 线特征可以分为两类：结构线和非结构线。非结构线的处理不受环境限制，更具通用性和鲁棒性，但因为不像结构线一样具备有效的方向约束信息，导致收敛速度较慢。
• 结构性的方向取决于对应的消影点方向，可以提供高效的几何约束来改善SLAM系统性能。
• 点线VIO：
• • PL-VIO，将直线特征加进基于点的VIO系统中。用普吕克坐标来表示直线，用最小4参数正交表示法来进行优化（因为普吕克表示法过参数化了）。
• • Trifo- VIO 使用卡尔曼滤波来讲直线特征加入VIO。用直线的正交向量所表示的几何约束来构造误差约束。
• 结构线VIO：
• • Camposeco 讲消影点加进VIO系统中，使用消影点的全局约束信息来修正航向误差。然而该系统仅仅把结构线作为中间结果，没有完全利用结构线的几何约束信息来修正平移。
• • Zou 提出了一种结构线的新参数化方法并且将其加入VIO。但是并没有利用上非结构性特征。
• 本文提出了一种紧耦合的单目VIO系统 PLS-VIO （点、结构线、非结构线）。为了减少线优化过程的计算消耗，对非结构线采用了不同的参数化表示法。我们详细描述了对所有直线特征的分类、匹配和初始化。主要贡献如下：
• • 为了更好的利用结构线信息，设计了一种线段分类策略将直线分为结构线和非结构线。
• • 为了高效的匹配和表示线特征，设计了一种基于点特征的2d-2d和2d-3d的线特征匹配算法来改善匹配的速度和精度。而且，与4参数正交表示法相比，我们引入一种2参数表示方法来改善线估计效率。
• • 提供了一系列实验来验证提出算法的有效性。

II. 系统概述

本文提出的方法基于VINS-Mono，我们加入了非结构线和结构线并构造了相应的约束。

• 前端为IMU和图像的预处理，包括IMU预积分、点特征检测、线特征检测和垂直线段分类等。
  -后端将主要介绍非结构线和结构线的处理。
• • 我们将垂直线（对其到重力方向的）和非垂直线加入后端，而X Y方向的线特征和非结构线特征将进一步分类。
• • 然后，使用两种不同的线段匹配策略，为了代码处理效率和线段数据存储的简化，我们将线段特征的匹配放进了后端。
• • 然后，初始化直线来获取3D的线段路标。
• • 最后，IMU的位姿和地图中的3D路标将通过最小化残差（先验残差、IMU残差、点重投影误差、线重投影误差）来进行优化。

III. 结构线和非结构线

将分别介绍点和线的参数化表示法、线分类方法、匹配方法和初始化方法。

A. 路标表示

• 点：逆深度表示法
• 非结构线：如图所示
  
  平面 $\pi$ 由两个3d空间的端点 $s^{\prime }$ 和 $e^{\prime }$ 和光心 $O$ 组成。3D线可以表示为 $L={[}^c{\text{n}}^T,{}^c{\text{v}}^T]$，其中 ${}^c\text{n}$ 为 $\pi$ 的法向量，${}^c\text{v}$ 为 $L$ 的方向向量。由于3D线 $L$ 在平面 $\pi$ 上，我们可以使用 $\pi$ 平面上的局部坐标系统 { P } \{P\} {P} 来表示 $L$。为了简化 $L$ 的表示方法，让 { P } \{P\} {P} 的原点为 $s^{\prime }$ ，让 y 轴方向对准到从 $O$ 到 $s^{\prime }$ 引出的一条射线，让z轴方向与 ${}^c\text{n}$ 平行。由于坐标轴的正交性，x轴与y轴和z轴垂直。 从光心 $O$ 到 $s^{\prime }$ 的距离为 $d$ 。
  为了减少线参数的数量，我们提出了一种压缩参数化方法，仅两个参数：$\theta$ 和 $\rho =1/d$。其中 $\theta$ 为局部坐标系 { P } \{P\} {P} 中的线段方向 ${}^c{\text{v}}^{\prime }$ 和 { P } \{P\} {P} 的x轴的方向角。其中 ${}^c{\text{v}}^{\prime }={\text{R}}_C^P{}^c\text{v}$，其中 ${\text{R}}_C^P$ 为相机坐标系相对于局部坐标系 { P } \{P\} {P} 的旋转矩阵。
• 结构线：原封不动的用了和StructVIO一样的2参数表示法，同样采用了Atlanda假设。

B. 结构线和非结构线分类 （没有创新）
我们使用LSD来检测图像中线段并从中分类出结构线段，剩下的即为非结构线段。我们使用消影点来识别结构线。例如，为了分类z方向的结构线，我们从消影点 ${\text{v}}_z$ 引一条到线段 $S$ 的中点的射线，当该射线和线段 $S$ 的角度偏差 $A_{err}$ 和 距离 $D_{err}$ 小于阈值的时候，认为属于z轴方向的结构线。

C. 2D-2D 和 2D-3D的线段匹配
为了提高线段匹配的稳定性和精度，我们联合两种跟踪策略，即 帧到帧(frame-to-frame)跟踪和 帧到地图(frame-to-map)跟踪。我们使用 帧到帧跟踪方法来跟踪新检测到的直线。若匹配到的线段数过少，将继续执行帧到地图的方法来增加匹配线。

1. 帧到帧线跟踪方法：采样前一帧的所有直线，得到一系列采样点 p i ∈ { P _ s a m p l e } p_i \in \{P \_{sample}\} pi​∈{P_sample}，然后使用对极搜索方法（epipolar searching method）来找到当前帧对应的候选匹配点。ZMSSD（zero-mean sum of squared differences) 模版用来计算两个点的匹配分数。我们选择得分最高的候选点作为被跟踪点，得到 p i ′ ∈ { P _ t r a c k e d } p^{\prime}_i \in \{P \_{tracked}\} pi′​∈{P_tracked}。若被跟踪点到当前帧直线的距离若小于阈值 $m\_th$ (5个像素)，则判定有效。当有效点的数量大于前一帧直线的采样点的0.8倍，则认为该线为最佳匹配。
2. 帧到地图线跟踪方法：使用ZNCC（Zero-normalized-cross-correlation）匹配方法。对一条3D线，我们得到该条线对应的最新的历史观测帧 $F_i$ ，然后使用ZNCC方法匹配当前帧和 $F_i$ 的线段。由于相机的快速运动和场景遮挡，匹配到的线段长度可能非常不同以至于影响匹配精度。我们使用对极几何约束来确定线段的采样范围去辅助ZNCC匹配：即计算出 $F_i$ 和当前帧的本征矩阵 $\text{E}$ 后，$F_i$ 的线段端点将被投影到当前帧，通过将当前帧的匹配线与两条极线相交来确定相应的采样范围，以此来提升ZNCC匹配的成功率。

D. 结构线和非结构线的初始化
线段初始化的稳定性和精度显著影响位姿估计。对2参数表示的结构线和非结构线，我们使用不同的初始化方法来确定合理的初始值。

1. 非结构线初始化。归一化平面的线段可以用两个端点来表示，和光心一起即可确定一个平面 $\pi$ ，给定相机帧 $c_1$ 和 $c_2$ 的两个平面 ${\pi }_1$ 和 ${\pi }_2$ ，对偶普吕克矩阵可以计算如下：
   
   我们从对偶普吕克矩阵中获取对应的普吕克坐标 。我们把相机坐标系下的 ${}^c\text{v}$ 转换到局部坐标系 { P } \{P\} {P} 下， ${}^c{\text{v}}^{\prime }=R_C^P{}^c\text{v}$。因此，我们可以初始化 $\theta$ 为局部坐标系x轴方向和 ${}^c{\text{v}}^{\prime }$ 的方向偏差，而逆深度 $\rho$ 通常初始化为 ${\rho }_0=0.2$。

2. 结构线初始化（与StructVIO略有不同）。同样要首先计算出普吕克坐标 ${}^{c}L={[}^c{\text{n}}^T,{}^c{\text{v}}^T]$。然后我们使用线段修建（line trimming) 来得到线段端点在世界坐标系下的3D表示 ${}^{w}L=[{\text{s}}^{wT},{\text{e}}^{wT}{]}^T$。为了获取 ${}^{w}L$ 和XY 平面在世界坐标系系统下的交点，我们将 ${}^l\text{p}$ 转换到世界坐标下：
   
   然后我们将 ${}^{w}L$ 与平面 ${}^w\text{p}$ 相交来获取交叉点 ${}^w{\text{l}}_p$, 并且将 ${}^w{\text{l}}_p$ 重新转化到参数空间来获取 ${}^l{\text{l}}_p$, 如下：
   
   从而获得structVIO里的参数化表示方法。

---

• 本文里所提出的方法，是将参数空间的XY平面转换到世界坐标系下，再与世界坐标系下用普吕克表示的直线相交，求出交点坐标，然后再将该交点转换回参数空间获得参数化表示。
• 而StructVIO里所提出的方法，是直接将相机坐标系下观测到的线段中点视作参数空间直线与XY平面的交点在相机坐标系下的表示，因此将该交点直接从世界坐标系转化到参数空间。

因此可以说，本质区别就在于这个参数空间的交点在世界坐标系下坐标的计算方法不同，前者理论上更精确，但后者处理上更方便。

---

IV. 点线结合的VIO

A. VIO 系统组成

其中 $r_{f_k}^{c_i}$ 为点的重投影误差，$r_{L_l}^{c_i}$ 和 $r_{L_s}^{c_i}$ 分别为非结构线和结构线的重投影误差。$\rho (\cdot )$ 为cauchy 鲁棒核函数，用来抑制外点。

B. 点特征测量模型（与VINS-Mono一致）
C. 非结构线测量模型
局部坐标系下的线段方向向量可以计算如：${}^d\text{v}=[cos\theta ,sin\theta ,0{]}^T$ ，然后将${}^d\text{v}$从局部坐标系转换到相机坐标系获得 ${}^c\text{v}$，即

其中 ${\text{R}}_P^C$ 的列由局部坐标系 { P } \{P\} {P} 的x，y，z轴组成。
相机坐标系下，线段的其中一个端点 $\text{s}$ 可以计算如下：

其中 $\text{y}$ 为局部坐标系的y轴。
为了获得一条直线在归一化图像坐标系的投影，需要将第一个观测关键帧下的端点 $\text{s}$ 和 方向 ${}^c\text{v}$ 转换到目标帧上去：

我们就可以得到在目标归一化图像平面下的线段方程：

归一化平面的线段测量 ${\text{z}}_{L_l}^{m_i}$ 包含两个端点的测量 ${\text{s}}_l^{m_i}$ 和 ${\text{e}}_l^{m_i}$，则线的重投影误差为：

其中 $d(\text{s},\text{l})$ 是端点 $\text{s}$ 到 投影直线 $\text{l}$ 的距离：

D. 结构线的测量模型
结构线的残差是将参数空间中的线参数转移到起始帧再转移到目标帧，然后构建重投影误差。

其中 ${\text{R}}_W^C$ 为从世界坐标系到目标坐标系的旋转矩阵， ${}^l{\text{l}}_p=[a,b,0{]}^T$，其中 a,b可以用 参数 $\theta$ 和 $\rho$ 来表示。
在参数空间中的结构线的方向向量表示为 ${}^l\text{v}=[0,0,1{]}^T$ ，将其变换到相机坐标系下：

则可以进一步得到目标帧下的直线方程：

与非结构线类似，结构线的残差同样定义为观测线段端点到投影直线的距离。

---