SLAM--PL-SLAM中基于线特征的初始化方法（LSD算法，LBD描述子，计算R和t）

线特征的初始化

ORB_SLAM2的东西差不多讲完了，接下来就讲讲其他开源代码的一些有趣的地方。最近看了以ORB为基础的点线特征的开源代码PL-SLAM（ 翻译后论文），其中他利用线特征进行初始化，并在误差构建和PNP上做出了改动，PNP选用了EPNPL。其中为了让我的系统初始化更加鲁棒，我可能会用到线特征的初始化，所以去详细研究了一下具体的算法。具体的算法步骤如下：
1.通过LSD算法提取线特征
2.通过LBD算法获取线的描述子进行匹配
3.通过三帧之间的匹配关系来计算R
4.通过三焦点张量法来计算t
下面我们来一步一步看。
https://blog.csdn.net/u013341645/article/details/78668316

直线段检测算法—LSD：a Line Segment Detector

首先先来看看如何提取线特征。在直线检测上，其实之前有提到过一种算法----霍夫变换。但他的时间复杂度太高了，这里用的是LSD算法，据说可以在O(n)的时间复杂度能提取到亚像素极的直线。

我们知道，检测图像中的直线其实就是寻找图像中梯度变化较大的像素。LSD的目标在于检测图像中局部的直的轮廓，这也是我们称之为直线分割的原因。轮廓是图像中的某些特殊区域，在这些区域，图像的灰度从黑到白或者从白到黑的剧烈变化。因此，梯度和level-line是两个重要的概念，如下图所示：

算法首先计算每个像素的level-line angle以构成一个level-line 场。该场被分割为连通的若干个部分，它们方向近似相同并且在容忍度τ内，这样可以得到一系列regions，这些 regions被称为 line support regions（支持域）。如下图所示：

每一个line support region其实就是一组像素，它也是直线段（line segment）的候选。同时，对于这个line support region，我们可以观察它的最小外接矩形。直观上来讲，当一组像素构成的区域，特别细长时，那么这组像素更加可能是直线段。line support region的一个主惯性轴作为矩形的主方向，矩形的大小选择为覆盖整个区域。

矩形中的像素的level-line angle与最小外接矩形的主方向的角度差在容忍（tolerance）τ内的话，那么这个点被称作"aligned point"（同性点）。通过统计最小外接矩形内的所有像素数n和其内的alinedg points个数k，用来判定这个line support region是否是一个直线段。判定的准则使用的是a contrario approach and the Helmholtz principle。

算法的具体流程：
输入：灰度图

输出：一系列的直线分割结果。

1.以 s=0.8的尺度对输入图像进行高斯核采样。

2.计算每一个点的梯度值以及梯度方向（level-line orientation），其中gx和gy分别为水平和垂直方向梯度。

3.根据梯度值对所有点进行伪排序（pseudo-ordered），建立状态列表，所有点设置为UNUSED。

设置图像梯度强度范围到[0, 1023]大于1023的梯度强度，强制设置为1023,创建1024个链表，遍历整个梯度图，根据梯度强度，相同梯度值像素坐标放入同一张链表中,将1024个链表，按从大到小顺序，合成一张大链表(首部为1023链表，尾部为0)

4.将梯度值小于ρ的点状态表中相应位置设置为USED。

5.取出链表头部存储图像坐标位置，作为种子像素，

do：
a.以seed为起点，根据梯度角方向相似(搜索周围UNUSED并且方向在阈值[ -τ, τ]范围内的点)，进行区域扩散。(每扩散一个像素，将该像素坐标从链表中删除，并且做标记(状态改为USED)，之后新的区域扩散，无法再扩散到在像素)。
b.将扩散区域进行矩形拟合,R。
c.判断同性点（aligned pt）密度是否满足阈值D，若不满足，截断（cut）R变为多个矩形框，直至满足。
d.计算NFA(拟合矩形精度误差)。
e.改变R使NFA的值更小直至NFA <= ε ，R加入输出列表;如果改变了还不满足或者矩形区域太小，舍去。

6.继续回到第5步，从链表中，找到下一个种子点，从剩下图像进行区域扩散，至到遍历完全图，得到所有检测到的直线。

具体每一步的意义和步骤大家可以参考这篇文章.，讲的非常详细。

描述子—LBD：Line detection and description

提取到了直线，现在如何来做匹配呢？那就需要给与直线一个ID，通过ID来进行匹配，ORB中用的是brief描述子，那这里用的就是LBD描述子。
1.尺度空间中提取线段
为了克服在线检测碎片问题和提高大尺度变化的性能，我们的检测框架采用由对原始图像进行高斯下采样（ with a set of 尺度因子and 高斯模糊 ）的尺度空间金字塔组成（N层图像）。我们首先用Edline算法每层的尺度空间产生一组线。每条直线都有一个方向，它是由从直线左侧到右侧的大部分边缘像素点的梯度构成的。然后通过在尺度空间中找到对应的直线来组织它们。在尺度空间中所有的线，他们被分配一个唯一的ID，如果在图像中他们相关相同的事件，将其存储在一个称为LineVecs的向量中（即图像的同一区域具有相同的方向）。线检测方法 不同于Wang et al.[5] ,通过重新组织从尺度空间提取的所有的线段形成LineVecs，从而降低了图匹配问题的维数。最终提取的结果是一组LineVecs如下图：

如图1所示，每个Linevec可能包括尺度空间的多条线。为描述一个LineVec局部外观，对于其中的每条线，我们会在被提取的线段的每层图中产生一种线描述符。

2.条带（Band）来表示线的支持域
在octave image给出了线段，描述符将从线段支持域（LSR）计算。该支持区域被划分为一组条带{B1；B2；…；Bm}，每个条带都是LSR的子区域并且他们之间的平行的，条代数m的和每个条带的宽度w，条带的长度等于线段的长度，如下图当m =5 ，w=3的LSR例子。

类似于 MSLD[11],引入两个方向构成a local 2D coordinate frame（局部二维坐标系），用于区分具有相反梯度方向的平行线并使描述符旋转不变。根据dl线的方向为 ，正交方向（垂直方向）dT 被定义为顺时针垂直的方向。该线的中点被选为这个局部坐标系的原点。LSR中的每个像素的梯度投影到这个局部框架:

其中的g：image frame中的像素梯度， g`：local frame中的像素梯度。

此方法描述子区域的优点：首先，它对dl方向的小的位置变化更有鲁棒性，因为在这种情况下，带内图像的大部分内容保持不变，只有带边界略有变化。注意，这个特性很重要，因为一般来说，由于线路端点不稳定，线的位置精度在方向中dl要比dT方向低。第二，它的计算效率更高，因为每个条带之间在dl方向没有重叠，高斯权重直接应用于每一行，而不是每个像素。

3.构造条带描述符

每条条带的描述子BDj：通过其最近的两相邻行的条带Bj-1 ; Bj+1来计算 。特别是，对于在顶部和底部的条带带B1和Bm，在计算B1和Bm的描述子时行在LSR之外不会被考虑在内。（所以下面n的取值会把j=1||m单独出来）

BDj的计算（其实就是描述的上下左右四个方向）：

k：表示在条带Bj的第k行，我们累积的像素的梯度在这行；
为高斯权重。

条带描述矩阵the band description matrix (BDM)：

BDj由BDMj矩阵的均值向量Mj and标准方差 Sj 得到：

LBD的均值部分和标准方差部分由于其大小不同，分别进行规范化处理。此外，为减少非线性光照变化的影响，对LBD每个维度的进行抑制，使它小于一个阈值（经验：0.4的是一个很好的值）。最后，我们重新规范约束向量得到单元的LBD。

计算旋转矩阵

得到了匹配的线段之后就用论文中的方式来计算R和t了。

现在考虑上图的建立，其中由端点P, Q定义的线段投影到三个相机图像帧中。用{p1,q1},{p2,q2},{p3,q3}作为在每个视图中线段端点的投影点，用I1,I2,I3∈R3表示从投影的端点计算而来的直线参数。
假设连续两个相机位姿之间小的且连续的旋转，这样使得从第一个相机视图到第二个相机视图的旋转和从第二个相机视图到第三个相机视图的旋转相同。在这个假设之下，我们可以表示这三个相机旋转为：R1=RT,R2=I,R3=R，I是3×3的单位矩阵。
注意，线系数Ii,i={1,2,3}也代表了向量的参数，该向量垂直于由投影中心Oi和投影点pi,qi形成的平面。这样的两个向量Ii的叉乘将和直线P,Q平行，同时与第三个矢量正交，所有这些矢量经过适当的旋转并放入到公共参考系中。这个约束可以写为：

另外，对于小的旋转，我们近似 R为:

对于这个参数化，有三条匹配线，我们将会有三个含有三个未知数 r1,r2,r3的二次方程。最多可以得到8个解。

三焦点张量计算t

对每个可能的旋转矩阵，我们可以通过三焦张量方程来获得 t1,t3。假设 t2=0。我们评估8个可能解，然后获取使得约束方程最小的那个解。

如上图所示，图像中三条对应直线的反向投影必相交于空间的一条三维直线。记三条直线为l1,l2,l3，相机对应的位姿矩阵P=[I|0],P=[A|a1],P=[B|b4]。有如下关系：

由于三个平面相交于一个直线，三平面组成的矩阵，满足秩为2的约束。

展开该矩阵可得：

由秩为2，即可写成：

根据第二行可知：

其中k为尺度因子，将上式代入第一行：

从而l的第i个坐标可以写成：

记：

则对应直线有如下关系：

三个矩阵的集合{T1,T2,T3}组成三焦点张量的矩阵表示形式。三焦点张量由3个3x3的矩阵组成，由27个元素，消去尺度因子及射影坐标系的影响，三焦点张量只有18个自由的。

在论文中，作者说道：在使用三焦点张量方程求解平移分量时，获得足够的独立约束，我们需要两个额外的线对应关系，因此，我们算法所需的线特征匹配数量是5。

这就是线特征初始化的步骤。