PL-SLAM Ｒeal-time monocular visual SLAM with points and lines

这篇论文是在ORBSLAM的基础上，将点特征增加为点线特征，由此引出的点线特征的SLAM。针对于线特征，又补充了线相关的误差计算、BA优化以及初始化等内容。

论文翻译

三、系统概述

整篇论文都是在ORBSLAM上做出的修改，将对线的操作补充了进去，如下图所示。

对于线特征，从输入帧中使用LSD算法进行提取，这是一个时间复杂度为O(N)的分类器，N表示输入帧中的像素数目，也就是说像素数目越多，用时越多。提取出的线特征通过关系图策略与地图中已经存在的线特征进行匹配。在后续的计算中，线会被投影到当前的图像上，以进一步寻找对应关系，如果图像包含足够多的环境信息，就会被设置为一个新的关键帧，并且将相关的特征通过三角化插入到地图中。此外，为了尽可能丢弃异常点，观测到线特征的帧数如果少于3或者在不到25%的帧中观察到的线特征也会被丢弃。
地图中线特征相关的位置信息也会使用BA优化进行优化，但是并不会用线特征去做回环检测，因为计算开销太大。

四、基于线特征的SLAM

整篇论文并没有对完整的框架做介绍，因为它是基于ORBSLAM改的，论文的四五两部分主要都是在补充加入的线特征对框架做出的改变。

A、基于线特征的重投影误差
在一般的特征点的误差计算，使用的无非重投影误差和光度误差，但是当特征从点扩大到线，就需要重新设计一些描述方法。

对应上面左侧的图，PQ表示地图中一条线的两个端点，这条线是环境中的正确位置，它在当前位姿的图像上的投影为pq上面带一个波浪线，而图像上拍摄到的PQ对应的线，则用pdqd来表示，反过来将这两个点恢复到真实场景下，对应的点是PdQd。这些都是对点的规定，除此之外用phdqhd来表示pdqd相应的齐次坐标。
在此基础上，用下面的式子来表示一条直线：

论文原话说的是用I表示归一化线系数，这里个人的理解是描述线的一部分，如果使用的是普朗克坐标，那么这个相当于向量叉乘的单位化，是表示中的一部分，这篇论文在这里是引用的论文一作的另一篇论文，所以在这里描述的不是很清楚，总之我们暂时就将I看作是一个描述直线的方法。
现在有了线的描述方法，我们就可以计算线的重投影误差：

这些参数中，I表示检测到的线的描述参数，PQ是空间点的坐标，Θ表示旋转和平移，K则表示相机的内参矩阵。可以看出线的重投影误差实际上就是两个端点的重投影误差，而端点的重投影误差也不完全是点特征中的重投影误差，这里表示的实际上是地图线在图像上投影的端点到检测到的线的距离，也就是下面图中的d1+d2。

下面问题在于，这个计算过程为什么看起来这么奇怪，这里也是个人的猜测，首先从I的表示方法看，这是一个向量叉乘然后单位化，也就是说所谓的归一化线系数本身仍然是一个向量，而向量的方向垂直于这两个向量所在的平面，也就是下图中的平面Opdqd，这个向量本身也表示一个方向向量，而好巧不巧这不就是投影点P~到pdqd的垂线方向，根据论文来看，里面的π表示的是投影点的坐标，这里没写是什么类型的坐标，不过为了能够进行乘法，我们只能看作是齐次坐标。

有了上面的推导，那么进行计算的过程，也就是求Epl的过程，从形式上来看，就是两个向量的内积，而内积的几何意义表示后向量在前向量上的投影，前向量是平面的法向量，在二维图像刚好就是垂线方向，而后向量正好是OP~，所以这表示在法向量方向上的投影，对应到2d的图上就是d1。

对于这个计算的过程，终于找到了合适的解释。将相机坐标系原点记作O的话，上面计算的的l，实际上就是平面Opdqd的法向量，计算的d1，实际上就是点p~到平面的距离：

利用图中的计算公式，n就是我们计算的l，而且我们已经对l进行了单位化，所以n的模是1，d的公式中就只剩下分子的部分，分子是原点到待测点向量与法向量点乘的绝对值，而使用齐次坐标的p~坐标，刚好可以看作这里的AP，计算过程就是求内积，计算的结果刚好就是前面图中的d1。这样去解释，就能解释清楚计算的过程了，但是为了保证乘法的合法性，投影函数的结果必须是齐次坐标，而且里面所有的坐标形式都必须用列向量的形式去表示。

在实际应用中会遇到线冲突或者误检测情况，图像平面上检测到的端点pd和qd可能和端点PQ的投影不匹配，比如像下面这样：

这种情况下，还需要一个检测线的重投影误差，定义为：

这里的I就变成了地图线的描述了，还是一个道理，这个误差表示的是图中的d1’+d2’。有了这两个误差，下面要做的就是递归优化位姿参数，让这两个误差尽可能接近。这里其实也存在一些小细节，一般来说优化都是让误差最小，而这里是让两个误差尽可能接近，我顺着引用找到了这篇论文作者的另一篇文章，找到了下面这张图：

右边上下两张图表示的是一次迭代的修改，可以看见检测到的点没有发生变化，而投影的点则发生了变化，个人猜测这里是在确定端点，特征线是个直线，没有终点，而上面的端点在不确定的情况下是可以在线上随意移动的。一般的优化通过调整位姿，让检测到的点和真实点的投影尽可能接近，而这里由于是线特征，我们如果采用同样的方法去调整端点，那么线本身就起不到任何约束作用了，所以采用这种沿着线更换点的方法，调整投影线上的位置，从而起到一个优化位姿的作用。

B、基于点线特征的BA优化
在一般的SLAM中，BA优化的部分都是依据点来对位姿进行优化，在增加了线特征之后，BA优化的策略也需要发生改变。
用Xj表示地图中的第j个点，那么对于第i个关键帧，点的投影关系为：

那么这个点的重投影误差可以表示为：

这是对于一般的点来说的误差表示方法，在一般的BA优化中也确实是这么做的。在此基础上，我们用PjQj来表示地图上的第j个线特征的端点，对应的投影关系为：

那么用线的重投影误差表示线特征的误差为：

论文在这里说这两个式子实际上就是前面使用过的线的重投影误差，这里不知道是不是出了点差错，如果按照前面的公式，这里的内参矩阵的逆矩阵实际上是多余的，因为这不符合整个误差的流程，pijh是第j个地图点在第i个关键帧上检测的位置的齐次坐标形式，左乘K逆，相当于利用内参矩阵将点恢复为空间点，暂且不提深度信息哪里来的，就这个空间点和投影直线就不在一个平面上，完全说不过去。
也许是这里笔误了，抛开不谈的话，这两个值表示的检测点到投影直线的距离，这里相当于使用了端点去估计这条线，也就是选取了端点作为代表，利用这种思路，统一的误差函数可以表示为：

其中ρ是赫柏核函数，三个Ω是协方差矩阵。以此作为BA优化的函数，从而对位姿进行优化。

C、全局重定位
这一部分论文写的很少，一般的SLAM通常使用pnp来进行重定位，使用RANSAC来去除异常点，在ORBSLAM中使用的是Epnp，为了使线特征也可以参与到重定位中，论文使用了新出的EpnpL，这个方法依然是这个论文作者提出的，具体的方法写在另一篇论文里。EPnPL对于线特征冲突和误检测很鲁棒。这通过两步过程来实现，首先使得检测到的线的重投影误差最小，然后估计线的端点PdQd。然后，这些点顺着这条线移动来匹配3D线段的端点PQ的投影点。一旦建立了这些匹配，相机位姿就可以估计了。

拿这个图来说，最小化检测线的重投影误差，就是让蓝色线尽可能贴近绿色线，假设经过第一步两条线重合了，就变成了左侧图的样子，之后就沿着这条重合的线，去让投影点和检测点对齐，从而就可以估计出相机的位姿。

四、特征线地图初始化

也许是本人太笨，这篇论文越往后看越看不懂。这部分主要介绍的是将线特征融入到地图的初始化中，一般的SLAM的初始化都是使用至少两帧的点，通过计算单应矩阵或者基本矩阵，从而初始化地图和位姿参数，但是在纹理较少的场景中，点特征效果不好，这时候就可以使用下面介绍的线特征方法去进行初始化。

使用线的初始化需要假设连续两个相机位姿之间小的且连续的旋转，这样使得从第一个相机视图到第二个相机视图的旋转和从第二个相机视图到第三个相机视图的旋转相同。

也就是在这张图中，O1O2O3这三个位姿的旋转变化是相同的，那么可以假设旋转R，则可以用这一个旋转去表示三个旋转的变化：

这里对于小的旋转R，我们将R近似为：

对于这三个位姿下的观察结果，原文认为线系数I1I2I3任意两个叉乘的结果与直线PQ平行，同时与第三个向量正交，经过旋转放入适当的坐标系下应该有：

对于这个式子，未知量只有一个R，而R里面有三个变量r1r2r3，所以需要三条匹配的线去解这个式子，最多可以得到八个解，之后再通过三焦点张量去解平移，最后评估可能的八个解，让上式最小的为最优解。
平移旋转的计算

延伸-PLPSLAM

PLSLAM使用的是点和线的特征进行的SLAM系统，在查论文的过程中无意中发现了一个PLPSLAM，使用了点线面的特征进行SLAM系统，在这里顺便记录一下。论文是中国民航大学的一篇中文论文，最令我好奇的是题目中的面特征，论文本身用的公式说实话挺抽象的，各种复杂的公式计算，也不能说是完全看懂了，只能说是连蒙带猜。
个人感觉这里使用的面特征本身并不能严格算面特征，论文中对面特征并没有做出很清楚的定义，只提到了位于同一个平面上的线特征应该满足同一个单应矩阵模型，而根据SLAM14讲，单应矩阵是用于点共面情况下的，所以是不是就是说，线特征满足单应矩阵，相当于线上的点全部符合单应矩阵，所以线就是共面的，而利用同在一个面上的线去计算单应矩阵然后恢复出位姿变化，就可以进行SLAM的过程。
论文里面说提取到的线特征可以用RANSAC根据不同平面的单应矩阵模型进行处理，相当于按照所在平面进行了一个分组，然后利用分组后共面的线特征作为面的描述。
除此之外，在线的3维重建那里还提到了反向投影平面的概念，如果是点特征，那么在不知道深度信息的情况下，其空间位置应该是一条射线，这条线反向投影直线，同理，对于线特征，在不知道深度信息的情况下，其空间位置是一个空间上的二维图形区域，也可以看作是一个平面，那么两个位姿下匹配线特征的反向投影平面的交线，应该就是线特征在空间中的位置所在的直线，利用端点应该就可以恢复相机坐标系下的线段的3d位置。
所以如果个人没有理解错的话，这个所谓的面特征，本身上是点线特征的一种特殊利用，利用共面的线计算单应矩阵从而恢复位姿变化。