【论文阅读】PL-VIO: Tightly-Coupled Monocular Visual–Inertial Odometry Using Point and Line Features

这篇文章是中科大的一篇关于点线SLAM的文章，主要是做了一个点线结合的SLAM框架，结合视觉和VIO，但是貌似不包含回环检测的部分。

一、公式约定

论文用右下角标i表示时间，用c表示相机坐标系，用b表示IMU的自身坐标系，用w表示世界坐标系，并且使用四元数来表示旋转的过程，用f和l分别表示点特征和线特征，其右下角标表示地图中的序列。

二、IMU预积分

由于使用了IMU参与SLAM的过程，所以需要对IMU的信息引入一个预积分，从而减小计算时间开销，论文画了一些笔墨来介绍预积分，这里不再赘述，可以直接查看之前VINS的博客。在这篇论文中，IMU的预积分公式为：

在优化过程，出于计算高效性的考虑，论文假设帧间的偏移bias是恒定的，即：

三、线段的地图表示

在这一部分，主要是讨论了线段的表示方法，对于一条空间中的3D线段，最直接的方法就是用高中学过的直线方程，这种表示方法有四个自由度，在这篇论文中，作者使用了普朗克坐标和正交化坐标两种坐标表示方法。

对于普朗克坐标，可以参考之前的博客，这里作者认为，使用普朗克坐标虽然会因为六自由度产生冗余，不能直接用于优化。

但是普朗克坐标在计算位姿变换以及三角化的过程会十分方便，根据论文给出的公式，可以直接使用变换矩阵来进行变换，而不需要复杂的过程：

另外，在三角化的时候，可以直接使用延伸产生平面交线的方法来进行，也就是说用匹配的线段和相机光心组成的平面，延伸产生一条交线，这条交线就是线段的3D信息，并且空间直线的参数可以通过对偶普朗克矩阵很快地解出来：

需要注意的是，在这篇文章中，线段的空间表示是没有端点的，也就是说是以一条直线的形式进行保存的。这样做确实会简单一些，因为不用考虑端点的准确性造成的影响，对比之前看到LF-SLAM，那篇文章中是用取平均值的方法来维护空间线段的端点，个人理解主要是因为线流的存在，让线段长度差别不大，从而恢复空间信息之后的可靠性也会相对高一些，而对于这篇文章，并没有对线段长度或者端点做维护，所以其可靠性也会差一些，而且端点的空间信息并不太影响整体效果，所以就直接用了空间直线的方法。

回到坐标系上，前面提到了由于自由度冗余，普朗克坐标不能用在优化上，解决方法就是换用正交坐标系：

所谓正交坐标系，个人感觉就是用线段做了一个坐标系，这个坐标系的三个周分别是直线的方向向量、法向量以及这两个向量叉乘出来的向量，基于此，旋转矩阵可以定义为：

这个坐标系可以有与普朗克坐标进行转换：

由于式子中的模都是1自由度的，所以可以换用三角函数来表示：

由于距离可以用两个模除法来表示，所以在优化过程中，我们完全可以使用四自由度的量来表示：

虽然公式的理解上还不是很到位，但是个人感觉采用两种坐标系来表示，主要就是想利用普朗克坐标的方便但是又不想去在计算上费太多时间，于是就用两套坐标系来表示，优化时用正交坐标系，三角化的时候用普朗克坐标系来表示，二者之间需要的时候可以相互转换。

四、紧耦合的视觉惯性融合

这一部分中论文使用了VINS一样的滑动窗口机制来进行局部BA优化，用图的方法表示为：

其中节点为路标和待优化的位姿，边表示观测关系。

使用滑动窗口，首先需要明确待优化的量，这里待优化的量为：

残差的表达公式为：

和VINS一样，残差公式是由好几部分组合而成的，第一部分和VINS一样是一个边缘化之后的先验信息，边缘化的策略会放在后面。第二部分是IMU的测量残差，第三部分则是点线的重投影误差。

在IMU的测量残差的部分，由于使用了预积分，这里IMU的残差可以表示为：

点的重投影误差就比较好理解了：

线段的重投影误差，论文的计算方法是用2D平面是观测的线段的端点，向投影直线做垂线，垂线距离的和即为线段的重投影误差，这主要是因为在前面线段的三角化的过程中，空间保存的是直线而不是线段，所以对于投影回2D的直线，没有一个对应的端点。
对于空间的一条线，其投影结果为：

残差的表示方法为：

五、基于点线特征的单目视觉惯性里程计

论文基于上面的部分，提出了一个SLAM框架，从流程图可以看出，PL-SVO只包含前端和后端两部分。

对于前端的部分，点特征的跟踪用光流法，之后用RANSAC来剔除异常点，如果点的数目太少，就用FAST角点来补充特征点。线段的部分则使用LSD进行提取，并使用LBD描述子来进行匹配，同时用几何关系来进行了约束，论文举了两个例子：中点距离和角度偏差，如果这两项超过阈值，就认为不匹配。此外，如果两帧之间的平均视差超过阈值，就将新的帧选为新的关键帧。

后端的部分使用了与VINS类似的滑动窗口机制，选用窗口中最远的两帧进行线的三角化。同时为了限制窗口的大小，论文也引用了边缘化的策略，如果次新帧为关键帧，就边缘化掉窗口里最旧的一帧，如果不是关键帧，就丢弃这一帧的视觉信息，但保留IMU信息，这里应该也是为了保证IMU的连续性。

六、FAST-PLVIO

这篇论文是一篇特别新的对PLVIO的改进，看到的时候仅发表了五天，这篇文章针对于PLSVO提取线段的部分做了一些修改，这里简单记录一下。

论文对于线段的表示和PLVIO一样，也是用普朗克坐标表示空间直线，用正交坐标系在优化时表示线段，针对于PLSVO使用的LSD提取线段，其时间开销过大，该论文将这部分换成了速度更快的EDLine，除此之外，考虑到EDLine在光照变化较大的情况下，会出现提取到线段数目变化较大的问题，这个情况对于帧间匹配来说是有副作用的，针对于这点，论文认为是因为在EDLine的第二步的时候，使用的是全局一致的阈值（gradient threshold），所以对于暗环境下的变化，提取会不明显。
针对于这一点，论文提出了一种动态的阈值，计算方法是在周围3×3的方格内，取两个十字：

个人看这段的理解就是论文现在用这个Gij作为阈值，查看EDLine的代码和论文，这一个阈值对应的应该是二值化的那一步，也就是原论文b图的部分，虽然不知道这样子做原理是什么，但是从论文给出的效果图，对于环境暗一些的情况，效果确实有所提升。但问题在于，换用这种方法，相当于考虑了周围环境的平均梯度，而在后面锚点提取的时候，也采用了同样考虑周围环境的方法，这两种方法是否有些重复。

除了改进线段提取方法，论文还优化了帧间匹配的部分，具体来说就是引入了一个叫做结构相似度的东西，简写SSIM，这个东西论文也是引用了别人的方法，没有介绍得很仔细，大体就是将光照、差异以及结构相似度三部分放在一起，以此作为相似度，由于在计算这个相似度的过程中，会将周围的范围的像素纳入考虑，所以让这个相似度带有一定的局部信息，在匹配时会更加准确。计算方法如下：

更换结构相似性评分之后，论文又提出了几种筛选方法来优化匹配结果，主要还是因为计算这个SSIM挺麻烦，所以在计算前先通过各种方法稍微筛掉一些，具体流程如下：

第一部分是线段的挑选，简单来说就是只保留长度超过30像素的前300条线段，之后利用如下图的坐标系进行貌似是一个方向的筛选：

第二部分是用几何的相似度进行筛选，也就是角度和位置上的筛选，角度的偏差不能超过π/9，长度比不能超过0.7，点的距离也做了限制，这主要还是为了考虑匹配的线段不会产生太大的位置偏差。之后计算SSIM值，利用阈值做一次筛选。最后，用论文所提出的链式法则来再做一次筛选。这里的链式法则，本质上就是说一条线一旦和另一张图上的一条线匹配，那么就不会和别的线匹配，所以论文用了这样的一个例子，假设jk是当前帧的两条线，而mn是上一帧的两条线，如果我们认为jm是匹配的，那么jn和km就是不匹配的，如果这个时候kn是匹配的，那么就可以构成一个环：

从这个环来考虑，我们应该让整体的相似度得分最高，相当于用dp代替了贪心，因此得分值的计算应该为：