Structure SLAM 相关论文阅读（一）：消影点/消失点/灭点检测提取

一、Visual-Inertial Odometry of Smartphone under Manhattan World

1. 摘要

1. 由三个正交消影点构建正交基，以此作为结构特征.
   2. 提出了一种基于已知主方向的快速结构特征提取方法。

2. 介绍

1. 使用点特征的重投影约束和结构特征的全局观测约束，考虑了结构特征之间的正交性。
2. 为了减轻运算功耗，优化了结构特征提取的过程。
3. 主要贡献
   • 可以实时运行在手机上的快速结构特征提取方法，使用穷举消影点假设来获取最优解。
   • 提出了一种直接参数化三个消影点为一个正交基的方法，定义为一个结构特征。使用四元数避免奇异性，旋转流型的切面空间对结构特征进行更新。
   • 提出了一个紧耦合的基于优化的单目视觉惯性视觉里程计，同时使用点特征和结构特征作为观测。

3. 相关工作

• 消影点提取: 传统方法首先提取图像线特征再进行聚类。现在的消影点提取方法可分为两类：

  • 穷举法

    • 2-Line Exhaustive Searching for Real-Time Vanishing Point Estimation in Manhattan World. 他们列出所有的消影点假设，通过极坐标参数化来加快消影点假设的搜索，进而提升实时性（详见二）。

  • 基于RANSAC ：初始的消影点通过定义一个最小解集（两条线段）来进行估计，迭代的随机抽样来产生效应点假设，选择最优解, 速度更快，精度降低。

    • Non-iterative approach for fast and accurate vanishing point detection. 使用J-linkage方法在没有任何先验信息的情况下提取消影点。
    • 3-line RANSAC for orthogonal vanishing point detection. 在已知地平面正交向量的情况下，用一条线来求解消影点（详见三）。
    • Using vanishing points to improve visual-inertial odometry. 通过加速度重力方向来加速RANSAC的线聚类过程（详见四）。

  
  结构性

  • • StructSLAM 没有直接使用消影点作为观测约束，而是用对应的参数化后的结构线（所在平面和与平面的交叉点）来约束。
  • • PL-SLAM 使用消影点观测约束来校准相对和绝对旋转
  • • Global SFM 使用平均旋转来进一步优化绝对旋转。

4. 方法: 结构特征检测和匹配

• 结构特征提取的本质是识别图像中的线段，然后计算三个正交主方向，就得到了相机坐标系下的结构特征表示为
  $${}^C{\text{q}}^{VPs}=\text{q}([{\text{d}}_0{\text{d}}_1{\text{d}}_2{]}_{3\times 3})$$
• 使用了EDline来提取线段。
• 结构特征初始化完成前，联合使用RANSAC和穷举法来进行消影点的提取(穷举部分详见二)。
• 结构特征初始化完成后，进行基于已知竖直方向的消影点提取(详见三)。

• 1. 结构特征的全局状态初始化

• 理想状态下，每个结构特征${}^W{\text{q}}^{VPs}={}^W{\text{q}}^B\otimes {}^B{\text{q}}^C\otimes {}^C{\text{q}}^{VPs}$在世界坐标系下的量测应该相同。由于噪声的存在，使用球面插值( spherical interpolation)来获取${}^W{\bar{\text{q}}}^{VPs}$。若${}^W{\bar{\text{q}}}^{VPs}$和世界系下的结构特征量测，在每个主方向的角度误差小于阈值，则认为是内点。若内点比例大于0.8，则认为初始化成功。

• 2. 结构特征提取

• 结构特征的全局状态初始化完成以后，进行基于已知竖直主方向的结构特征提取。流程如图，
  

  • 图 a ：初始化中获取的垂直方向${\text{VP}}_{init}$投影到图像平面获取${\text{VP}}_{img}$，并将线段进行分类。属于竖直方向的的线段用来重新计算当前帧的竖直方向的消影点${\text{VP}}_1$。
  • 图 b ：生成在${\text{VP}}_1$的正交平面的单位采样${\text{VP}}_2$和${\text{VP}}_3$。
  • 图 c ：所有线段组合所生成的消影点假设的极坐标表示。
  • 图 d ：权重最大的假设被视为全局最优的消影点组合。

• 2.1 首先，如图a所示，将已知的主方向投影到图像平面获取${\text{VP}}_{init}$，然后找到所有属于该主方向的线段集合 { l v } \{\textbf{l}_v\} {lv​}，定义角度偏差如图所示
  若偏差小于6 degree就认为属于该主方向。然后构建一个最小二乘问题来求解${\text{VP}}_1$ : ${\text{L}}^T{\text{VP}}_1={\text{0}}_{n\times 1}$。因此，已知的竖直主方向只提供了用来分类线段的初始值。

• 2.2 然后，以 n 度为间隔，生成对应 ( 360/n )的消影点单位采样${\text{VP}}_{2}s$。原文中的n=0.5.

• 2.3 最后，${\text{VP}}_3$通过${\text{VP}}_1$和${\text{VP}}_2$叉乘获得。

• 为了快速计算全局最优的消影点假设，把图像中所有线段组合的交点投影到极坐标系下（这块本论文表述的不够清楚，详细参见二）：
  $$\begin{gathered} \mathbf{\theta }=\text{acos}({\text{P}}_x/norm(\text{P})) \\ \mathbf{\lambda }=\text{atan2}({\text{P}}_x,{\text{P}}_y)+\pi \end{gathered}$$
  权重定义为 ${\text{l}}_1^{response}\times {\text{l}}_2^{response}$, response是图像线段的比例。意味着被更多线段生成的消影点假设获得了一个更高的权重。构建极坐标后，使用一个高斯平滑滤波器来减少测量噪声的影响。最终，我们只需要将所有的${\text{VP}}_2$和${\text{VP}}_3$假设投影到极坐标上，然后选择权重最大的作为最终的最优消影点。

• 3. 结构特征匹配
  因为结构特征由三个正交消影点组成，因此特征匹配问题就是消影点方向的匹配问题。因为竖直方向已知，只需要再匹配一个主方向就行。首先，对于连续帧 $\mathrm{I}$ 和 $\mathrm{J}$ ，使用${}^W{\text{q}}^{C_I}$和${}^W{\text{q}}^{C_J}$来将其对应的结构特征测量转换到世界坐标系下。当 $\mathrm{I}$ 和 $\mathrm{J}$ 的主方向偏差小于6 degree时，认为well matched。因为当位姿误差较大时匹配也会失败，因此当上述方法失败时，我们使用预积分的相对旋转${}^{C_I}{\text{q}}^{C_J}$把 $\mathrm{I}$ 的结构特征量测转换到 $\mathrm{J}$ 下来进行匹配。

---

二、2-Line Exhaustive Searching for Real-Time Vanishing Point Estimation in Manhattan World（有代码）

可以结合另一篇代码解读博客进行理解
摘要
提出了一种使用2条线段来获取第一个消影点${\text{v}}_1$，然后在${\text{v}}_1$的等效球面上的外圈上获取第二个消影点${\text{v}}_2$的假设，再通过叉乘计算第三个消影点。优点在于：

• 2-line 模型更鲁棒
• 给定外点比率，通过内点线段构建的2-line模型的概率可以计算出来，意味着可以决定迭代次数，因此消影点的估计可以用一种非常简单的穷举法来进行，而不是用传统的RANSAC法。
• 构建的极坐标网格可以作为查询表，用来快速验证假设。

方法
1. 极坐标网格构建
等效球体
等效球是个单位球体，其$X$轴和$Y$轴与图像平面的$X$和$Y$轴平行，$Z$轴指向图像平面的光心。因此，给定光心$(x_0,y_0)$和焦距 $f$，可以把像素$(x,y)$转换到等效球体坐标系上：
$$\{\begin{array}{l}X=x-x_0\\ Y=y-y_0\\ Z=f\end{array}$$
然后，转化成经纬度模式
$$\{\begin{array}{l}\varphi =\text{acos}(Z/\sqrt{X^2+Y^2+Z^2})\\ \lambda =\text{atan2}(X,Y)+\pi \end{array}$$
极坐标网格
$\varphi$ 和 $\lambda$ 的范围分别为 $[0,\pi /2]$ 和 $[0,2\pi ]$。因此，极坐标网格$\mathcal{G}$ 可以通过以下步骤来构建：

1. 以$1^{\circ }$ 为间隔，把$\mathcal{G}$ 初始化为$90\times 360$ 的零空间，i.e. $\mathcal{G}(i,j)=0$ for $i=1,2,...,90$ 和 $j=1,2,...,360$。
2. 对每个图像上的线段对 $l_1$和 $l_2$, 计算出交叉点 $\text{p}$ 和相应的经纬度 $(\varphi ,\lambda )$。
3. 更新网格数值：
   $$\mathcal{G}({\varphi }_{deg},{\lambda }_{deg})=\mathcal{G}({\varphi }_{deg},{\lambda }_{deg})+\Vert {\text{l}}_1\Vert \times \Vert {\text{l}}_2\Vert \times \text{sin}(2\theta )$$
   ${\varphi }_{deg}$和${\lambda }_{deg}$是$(\varphi ,\lambda )$ 对应的边界，$\theta$ 是小的那个角度。此公式的设计目的是为了给予那些长度更长、方向偏差中等的线段组合更高权重。
4. 用一个$3\times 3$的高斯平滑滤波器对极坐标网格进行平滑。

极坐标网格构建的结果就是，生成了一个记录有【线段对每个网格单元的响应】的 $90\times 360$ 的网格，一旦构建完成，之后就可以用来查询搜索，因此可以加快实际应用的实时性。

2. 假设生成
现今的假设生成方法通常使用多线最小解集(multi-line minimal solution set, multi-line MSS)法。正如 Wildenauer and Allan(Robust camera self-calibration from monocular images of manhattan worlds)指出，multi-line MSS的概率在没有线段数量的先验信息的情况下难以直接计算。因此，大多数情况下RANSAC的迭代次数都是凭经验决定的。我们提出了一种使用 2-line MSS 来生成第一个消影点的假设的方法，然后通过穷举采样获取第二个消影点，再通过叉乘计算第三个。过程如图所示，
第一个消影点是两条线段的交点，第二个消影点通过在v1的正交平面外圈上进行均匀采样，第三个叉乘获得。

第一个消影点

• 给定一张由$N$个无外点线段的图像，对应三个消影点的线段数量分别为$n_1$，$n_2$ 和 $n_3$。则随机选择两条线段来构建2-line MSS， 这两条线段属于同一个消影点的概率为：
  $$\begin{array}{rl}P& =(C_{n1}^2+C_{n2}^2+C_{n3}^2)/C_N^2\\ & \approx (n_1^2+n_2^2+n_3^2)/N^2\in [1/3,1]\end{array}$$
  当$n_1=n_2=n_3=N/3$的时候概率最小。
  然后，当给定外点比率为0.5时（即一半的线段没有对应的消影点），则 $P$ 的最小值变为$1/3\times 0.5^2=1/12$。
  在置信度为0.999时，获得至少有一个inner 2-line MSS所需要的迭代次数为：
  $$\begin{array}{rl}\#Its& =\text{log}(1-0.9999)/\text{log}(1-P)\\ & =105\end{array}$$
  因此，对于第一个消影点${\text{v}}_1$，我们迭代的随机抽选2个线段并计算它们的交叉点，然后把点投影成一个等效球体上的单位向量。

第二个消影点

• 考虑到正交约束，第二个消影点必定在${\text{v}}_1$的正交平面外圈上。因此，我们在圈上均匀采样360个假设，步骤如下：
  1. 经度$[0,2\pi ]$被均匀分割成360块，每个都对应着消影点${\text{v}}_2$的一种假设的经度。
  2. 第$ith$个假设的经纬度可以计算如下(四个方程四个未知数)：
     $$\{\begin{array}{ll}{X}_2& =\text{sin}(\varphi )\times \text{sin}(\lambda )\\ Y_2& =\text{sin}(\varphi )\times \text{cos}(\lambda )\\ Z_2& =\text{cos}(\varphi )\end{array}$$
     和（正交性质）
     $$X_1{X}_2+Y_1{Y}_2+Z_1{Z}_2=0$$
     最终，根据$\varphi$和$\lambda$的值，可以计算第二个消影点${\text{v}}_2=(X_2,Y_2,Z_2)$。

第三个消影点：${\text{v}}_3={\text{v}}_1\times {\text{v}}_2$ 直接计算得到。

3. 假设验证

最后一步是假设验证，目的是为了在37800（360*105）个假设中找到最大线段响应（response）的一个假设。对每个假设计算线段响应过于耗时，因此本文提出的查询表可以将计算复杂度降低到$\mathcal{O}(n)$。计算步骤如下：

• 首先，对每个假设，计算每个消影点对应的经纬度$({\lambda }_1,{\varphi }_1)$、$({\lambda }_2,{\varphi }_2)$和$({\lambda }_3,{\varphi }_3)$。
• 然后进行极坐标网格的搜索，并且设置 $\mathcal{G}([{\varphi }_1\times \frac{180}{\pi },{\lambda }_1\times \frac{180}{\pi }])$ + $\mathcal{G}([{\varphi }_2\times \frac{180}{\pi },{\lambda }_2\times \frac{180}{\pi }])$ + $\mathcal{G}([{\varphi }_3\times \frac{180}{\pi },{\lambda }_3\times \frac{180}{\pi }])$ (求和) 为该假设的线段组合响应。
• 最后，响应最大的假设被选为最佳消影点估计。

讨论
本文提出方法具备以下性质：鲁棒、实时、全局最优

• 鲁棒：应用的2-line MSS仅需要属于同一消影点的2条线段，因此适用于不同的场景（1个、2个或3个消影点）
• 实时：最为耗时的假设验证问题转化为极坐标网格查询，复杂度仅有 $\mathcal{O}(n)$。极坐标网格的构建、假设生成同样耗时很低。值得一提的是，极坐标网格构建（polar grid building）、假设生成（hypotheses generation）和假设验证（hypotheses validation）是并行进行。
• 全局最优：基于RANSAC的方法通常会有局部最小值的问题。而提出的方法因为采用了穷举策略，因此可以达到全局最优。第二个消影点的均匀紧凑采样会生成足够的假设（$1^{\circ }$精度)。最终，对37800个假设的穷举搜索可以达到全局最优。

---

三、3-line RANSAC for orthogonal vanishing point detection

1. 摘要

• 提出了3-line RANSAC方法(估计旋转的最小采样为3）
• 提出了基于已知地平面的1-line 方法

2. 介绍（略）

3. 方法

1. 球面表示法
   当内参矩阵已知时，高斯球面表示法是一种常用的表示不同相机获取的图像（针孔、鱼眼等）的方式。世界坐标系下的一条线段投影到等效球上的外圈上，用对应的正交向量 $n_i$ 来表示，世界系下平行的两条直线在等效球体上的两个外圈交点位于两个对极点上，如图所示，它们对应的消影点计算得到：$v=n_i\times n_j$。
2. 线段提取（generalized polygonal approximation），略
3. 3-line RANSAC
   RANSAC的基本原理为：随机采样数据，模型估计并计数其内点来验证模型，重复多次最终输出内点最多的模型。本文将消影点的正交性直接应用到模型估计过程中。第一个重要的问题是，随机抽取的样本个数，因为RANSAC的迭代次数会随着抽取样本个数指数级增长。因此这就涉及到最小采样的问题(minimal solution).
   旋转具备三个自由度且每条线提供一个约束。因此旋转估计的最小采样为3。本文方法如下图-左所示。
   

• • 前两条线（红色），交汇于一个消影点${\text{v}}_1$ (和 它的对极点 ${\text{v}}_1^{\prime }$) 。${\text{v}}_1$ 对应的虚线外圈是定义了所有可能的消影点集合。
• • 第三条线（绿色）与上述集合相交于第二个消影点${\text{v}}_2$（和它的对极点${\text{v}}_2^{\prime }$）。
• • 第三个消影点通过叉乘计算获得。

  和其他传统的基于点的极线方法一样，这个过程可以通过优先挑选那些满足几何配置（geometric configurations)的样本来进行加速。

4. 1-line RANSAC
   地平面可以通过天空、地面分割获取来作为已知的先验知识。（或者重力加速度来判别）。然后就可以进行1-line RANSAC如上图-右所示。
5. 内点的定义
   得到1-line或3-line方法的消影点$\text{VPs}$，下一步的RANSAC用来计数内点。当一条线 $l$ 与消影点 $\text{v}$ 的距离小于一定阈值就认为是内点。距离的计算可以是球面的地理距离: i.e. $|\text{asin}(n\cdot v)|$，其中 $n$ 为线段 $l$ 的正交平面外圈。另一种是在图像平面计算距离，如下图所示。

四、Using Vanishing Points to Improve Visual-Inertial Odometry

1. 摘要：提出了一种基于空间平行线在图像交叉点的误差约束。
2. 介绍
   通过识别人工环境中的消影点，为VIO估计提供了全局的绕重力轴的旋转观测。
   贡献：

• • 借助惯性辅助的RANSAC实现高效鲁棒的线段分类，然后就能通过最小二乘对消影点进行refine。
• • 消影点辅助的VIO系统，为了低端设备应用的实时性而使用了延迟更新策略。

3. 方法：惯性辅助的消影点检测
   步骤如下：

• • 检测线段，给出两个端点射线。
• • 利用当前垂直的方向去计算消影线(VL)，如下图所示。注意当前的垂直方向估计仅用来找到消影点(VP)，在最终的消影点估计中当前的状态估计没有被使用。消影线被定义为平行平面（平行于地面）和无穷平面的交线(换种说法就是，几何上，消影线由平行于景物平面并过摄像机中心的一张平面与图像平面的交线获得，可通过两平面法向量的叉积计算获得)。
    
    线段被分为平行线段 $S_{||}$ (绿)和垂直线段 $S_{\top }$(蓝)，借助重力方向可以跟踪VL(虚线)(注意这里的垂直和平行不是相对于地面)
• • 使用RANSAC把线段分类。对于票选最高的消影点(绿点所示)，平行线段集合将包含了所有指向该消影点的线段。第二个垂直线段集合中包含的线段将与消影线交叉在正交于该消影点的点上(蓝点所示）。

  A. 直线检测：LSD
  B. 用来线段分类点1-line RANSAC

  • 给定视觉惯性系统的重力方向（假设外参已知），我们以一种高效的方式来找到消影点。为了能将线段分类成 $S_{||}$和$S_{\top }$ ，采用RANSAC方法。对于所有的归一化后的线段，我们随机生成消影点假设，检查对应的线段集合尺寸来进行验证。这种方法得出的票选最高(most-voted)的消影点是比较粗糙的估计，但更重要的是得出了两种内点集合。
  • 通常为了生成一个消影点假设，我们需要至少两条线段的交叉。但是，若我们已经提前知道了其对应的消影线VL，则消影点必定位于消影线上。因此，将任意一条线段与消影线交叉就可获得一个消影点，如上图所示。我们定义地理距离来判断内点：
    $$d_{u_k,\text{v}}=|\text{atan}(\frac{|{\text{u}}_k\times \text{v}|}{{\text{u}}_k\cdot \text{v}})|$$
    其中，$\text{v}$ 为消影点假设，$d_{u_k}$ 为弧度距离，${\text{u}}_k$ 是线段集合的第 $k$ 个元素。这个距离可以直接用来判断出交叉于同一个消影点的线段($S_{||}$）和交叉于与当前消影点假设正交的点的线段（$S_{\top }$），残差设置为 $\text{min}(d_{u_k},|\pi /2-d_{u_k}|)$ ，若小于阈值则判定为一个内点，然后加入对应的集合中去。

  C. 最小二乘解
  一旦我们找到票选最高的消影点假设，我们就可以得到与该消影点对应的平行线线段集合$S_{||}$和正交线段集合$S_{\top }$。接着，我们基于这两个线段集合对该消影点进行refine，我们提出了一个最小化代价函数的最小二乘方案如下：
  $$E(\text{v})=\sum _{i\in S_{||}}(\text{v}\cdot {\text{u}}_i{)}^2+\sum _{j\in x{S}_{\top }}((\text{v}\times {\text{a}}_C)\cdot {\text{u}}_j{)}^2$$
  其中满足$||\text{v}||=1$，$\text{v}$ 是微调后的消影点，且${\text{a}}_C=\text{C}{(}_I^{c}q)\cdot \frac{{\text{a}}_m}{||{\text{a}}_m||}$，${\text{a}}_m$ 为测量加速度。注意这里没有继续使用地理距离，而是简单的使用了点积。这是因为我们不再需要保证平行和正交的法向量用同一种方法来表示。后续的优化求解用拉格朗日乘子法来强制 $||\text{v}||=1$。实现过程中用线段长度进行了加权。

---

新建了一个点线SLAM/结构化SLAM交流群，可以私信我加入，欢迎新人同学讨论和行业大佬指导