【论文阅读】DPLVO: Direct Point-Line Monocular Visual Odometry

一、公式及符号约定

这篇文章与之前看的EDPLVO是同一个作者，EDPLVO是在这篇文章的基础上改进的，符号约定方面，DPLVO同样采用R和t表示旋转矩阵和平移向量，使用Π表示投影函数，利用R和t以及Π投影函数，我们可以得到一个特征点在另一帧上的对应投影点为：

这篇文章中空间直线的坐标也使用普朗克坐标，相关的内容参考链接
使用普朗克坐标，线段也可以直接进行投影，需要使用稍微变化一下的T矩阵：

二、点线直接法模型

A 点的光度误差

无论是EDPLVO还是DPLVO，缩写中的D表示的都是direct，也就是直接，点的直接法残差使用的依然是点的光度误差：

将误差公式与滑动窗口结合，得到的就是总的残差公式中点的部分：

B 线的共线约束

主要的难点在于如何定义线的直接法，DPLVO使用的是线段的共线约束的方法。论文中将线段第一次出现的帧称为锚图（anchor image），将后续每次观测到这条线段的图称为关联图（associate image），共线约束对于锚图和关联图的公式是不同的，在锚图中，我们首先记一条线段为L，从线段上取样得到PL点，这里认为深度是已经知道的，记为dPL，但是并没有说深度是怎么知道的，利用反向投影函数我们可以求得这个点的空间坐标：

之后使用普朗克坐标来计算点到直线的距离：

对所有的采样点都计算一次并求和，得到的就是锚图上的共线约束：

锚图上的共线约束，简单来说就是2d图像上采样点的反投影3d点到直线的距离应该尽可能小，但需要明确的一点是，公式9中的L在我们只有锚图的时候是计算不出来的，所以这一个公式要想计算出来，至少要后面几帧恢复出线段的空间坐标之后才可以实现。

共线约束的另一部分是说关联图上点的共线约束，类似地我们设Lj为关联图Ij上的一条线段，与其关联的线段为L，上面有一个采样点pLj，其深度为dpLj，那么用反向投影函数，我们可以得到一个关联图上采样点的3d坐标：

但是现在我们求得的XLj是在当前帧的相机坐标系下的，与线段L并不在一个坐标系，这里我们用普朗克坐标的位姿，将线段L投影到当前帧的相机坐标系中：

之后利用点到线的距离，计算出反向投影点到投影线的距离：

对所有采样点的距离误差求和，得到的就是关联图上共线约束的残差：

关联图上的共线约束，简单来说就是关联图上的采样点的3d坐标到直线投影的距离应该尽可能小。得到这两个共线约束之后，我们并不能直接简单地相加作为全部的线段的残差，由于运动模糊等原因，线段的提取存在一定的不确定性，所以这里论文用ρ来表示提取出来的线段的宽度，宽度越大表示因为模糊导致线段越来越不可靠。另外，前面也提到过采样点的问题，这里采样的方法是对线段，根据长度进行划分，划分出来的每一段中，用梯度作为选择点的标准，选取梯度最大的点作为采样点。这样，我们将一条线段的锚图的共线约束和关联图的共线约束根据权重相加，得到的就是一条线的共线约束：

对所有线的共线约束相加，得到的就是窗口内的线段的残差：

C 完整的残差公式

那么对于整个的残差函数来说，直接将点的残差和线的残差加起来就可以了：

D 基于滑动窗口的优化策略

对于这篇论文的优化公式，也就是15，其优化的开销是很大的，所以DPLVO采用了基于滑动窗口的策略来约束计算量，提到滑动窗口，首先想到的就是VINS，使用滑动窗口，就是让优化的范围固定在窗口内，而窗口外的量通过边缘化的策略给予窗口内的量一定的指导作用。
在DPLVO里面，边缘化分为两类，正好对应残差公式中的两部分。点的残差在边缘化时，很久没有产生关联的关键帧和点会利用舒尔补进行边缘化。而线在边缘化时，论文想实现的是利用新的观测不断更新旧的参数，所以使用的是先验共线约束来代替一般的边缘化策略。

先验共线约束，个人理解还是一个边缘化的过程，首先当一帧从滑动窗口中移动出去时，就固定这一帧的位姿，既然固定了位姿，那么线上的采样点就可以利用投影和变换，将其转换到全局坐标系下，并且由于位姿固定了，这些点也相当于在全局坐标系下被固定了。对于这些固定点，他们所处的坐标系是世界坐标系，那么按道理，提取到的线段投影到世界坐标系下，固定点应该落在投影线上，由此产生一个边缘化的约束：

对于一条线上所有的采样点都做这么一个操作，得到的就是一条线的先验共线约束：

之后论文还论证了一下两步LM优化的问题，不过优化的部分个人了解不多，也就没再看。总的来说，先验共线约束，是为了减少优化量，采用了滑动窗口，窗口内部的点线、位姿需要进行优化，而窗口外的部分是固定了的，但固定了不代表就与他们没关系了，采用这种方式，让已经固定的内容依然可以给窗口内的量一定的约束。虽然DPLVO里面没有写边缘化的残差体现在哪里，但是如果参考同样使用了边缘化策略的VINS，边缘化的残差应该会作为总的残差公式的一部分。如果是这样，残差公式应该是三部分：点的光度误差、线的光度误差、边缘化残差。其中对于窗口内新出现的线，锚图使用锚图的共线约束，窗口内的关联图使用关联图的共线约束，而对于旧的线，也就是锚图已经被移动出窗口的，就是用边缘化残差来约束。

三、前端

这一章里面论文介绍了一下前端的一些实现细节。在线段提取的部分，使用的是LSD算法，并且为了加速线段的检索，使用了类似于ORBSLAM跟踪的策略，只在没有线段投影的区域内进行检索。并且考虑到线在提取时可能出现的断裂问题，这里采用了一种利用角度和到原点的距离来检测合并的方法，具体来说就是检测两条空间直线的方向以及到相机光心的距离，当方向角偏差不大且到光心的距离差别也不大的时候，认为这两条线是断裂的一条线，这时就利用两条线上的采样点，拟合出一条新的合并的线。

线的初始化部分，不同于PL-SVO里面用两条线的延长平面做相交的方法，这里采用的是拟合新曲线的方法，采样点单独计算深度并反向投影得到空间坐标，之后对于所有的3d点，使用PCA主成分分析检测反向投影的准确性，如果准确性符合要求，就用这些3d点拟合一条直线并用普朗克坐标表示。

当线初始化之后，DPLVO采用了跟踪-扩展-再拟合的方法建立不同帧之间线的联系。具体来说，跟踪部分将线上的采样点投影到另一帧通过最小化光度误差来确定一部分点，利用这部分点拟合一条2d线，作为跟踪的结果。但是跟踪和实际情况存在一定的差别，也就是跟踪的起点终点可能与实际提取的存在一定的出入，这里采用了一种扩充线段的方法，也就是从投影直线的基础上，端点不断向外延伸，通过比较延伸前后线上点的梯度情况来检验线段扩充是否成功。扩充成功后，扩充出来的点再次反向投影，这次深度直接设置为与空间线段最近的深度。