论文笔记-PL-SLAM: Real-time monocular visual SLAM with points and lines

这一篇论文的主要贡献有两个，一个是在orb-slam的基础上加入了线特征，在不影响效率的情况下，提升了系统在低纹理的情况下的表现；二是提出了一个只用到线匹配就估计出初始地图的方法（连续三帧）。这篇论文是基于单目的，还有一个同名的PL-SLAM是双目的SLAM系统，不要弄混了~

I. INTRODUCTION

首先指出了为什么要使用线特征：
对于ORB-SLAM，如果在低纹理场景，或者是图像有运动模糊时，系统就会失效，但是在这些场景中往往还有线条，所以就希望能够应用线特征。
同时，论文也说明了使用的线特征的难点在哪里：
1）要找到合适的线条检测子和参数化方式
2）从线匹配去计算位姿的可靠性是低于用点的，而且这些线特征对遮挡非常敏感
3）使用端点还可以对orb点特征的代码进行部分复用
最终，选择了用端点来参数化，原因是：这种方式对于遮挡和无检测的情况更加鲁棒。

III. SYSTEM OVERVIEW

PL-SLAM的pipeline如下图所示：

跟orb-slam一样，追踪线程用来估计相机的位姿和决定是否要加入新关键帧；局部建图增加新关键帧信息到地图并用BA优化；闭环线程持续的进行闭环检测并修正。
在PL-SLAM中，和线特征相关的处理包括了：

• 检测：使用LSD的方法，时间复杂度是$O(n)$，n为图片中像素的个数
• 三角化
• 匹配：使用了以Line Band Descriptors为基础的方法，通过一个关系图（relational graph）当前的线会和已经在地图中的线进行配对。在获得了初始的map-to-image的线特征对集合后，所有在局部地图中的线都被投影到图像上，进一步寻找匹配对，然后，如果这个图片有足够多的新环境信息，他就会被标记为关键帧，它对应的线会被三角化并添加进地图。
• 线的剔除：从少于三个视点或少于 25% 的帧中看到的线会被丢弃（剔除）。
• 优化：线在地图中的位置使用局部BA进行优化。
• 重定位： 注意，因为对整个地图进行线的匹配的计算量非常大，所以回环检测中只使用点特征。

IV. LINE-BASED SLAM

这一节重点介绍线的参数化方式以及误差函数，还会解释怎么把它用到特征匹配，BA和全局重定位中。

A.基于线的重投影误差

根据参考文献[30], P,Q是一条线段的两个端点，${\text{p}}_d,{\text{q}}_d\in {\mathbb{R}}^2$是这两端点在图像平面的2D检测结果，${\text{p}}_d^h,{\text{q}}_d^h\in {\mathbb{R}}^3$是对应的齐次坐标。从齐次坐标，可以得到一个归一化的线系数：
$$I=\frac{{\text{p}}_d^h\times {\text{q}}_d^h}{|{\text{p}}_d^h\times {\text{q}}_d^h|}(1)$$
有了这个系数，下面再来看线的重投影误差，它是点到线距离之和，这里点是指的两端点投影后的结果，线是指的在图像中检测到的线段。具体可以看下面这张图：对于左图：
P,Q是三维空间中某一条线段的端点（绿色点），而在图像中也有对应的两个绿点，这个就是由真实的P,Q投影过来的。而在图像中，本身就存在两个拍摄下来的端点$p_d,q_d\in {\mathbb{R}}^2$（对应于P,Q的观测值），观测值对应三维空间的端点$P_d,Q_d\in {\mathbb{R}}^3$. $I$是检测到的线系数，而$\tilde{I}$则是指的投影的线系数。
对于右图：
$d_1,d_2$就是我们要的线重投影误差，而$d_1^{{}^{\prime }},d_2^{{}^{\prime }}$是检测到的线重投影误差（检测到的2D线段（蓝色）和对应的投影的3D线段（绿色）之间的误差）。

线重投影误差的公式就为：
$$E_{line}(P,Q,I,\theta ,K)=E_{pl}^2(P,I,\theta ,K)+E_{pl}^2(Q,I,\theta ,K)(2)$$

$$E_{pl}(P,I,\theta ,K)=I^T\pi (P,\theta ,K)(3)$$
$I$是检测到的线系数，$\pi (P,\theta ,K)$是端点P在图像上的投影，K是内参矩阵，相机参数θ={R,t}\theta = \{R,t\}θ={R,t}

注意！在实际中，由于线的遮挡和误检测，图像中检测到的线段端点$p_d,q_d$往往不能和$P,Q$匹配，所以这里才又定义了个检测到的线投影误差。
检测到的线重投影误差的公式就为：
$$E_{line,d}(p_d,p_d,I)=E_{pl,d}^2(p_d,I)+E_{pl,d}^2(q_d,I)(4)$$

$$E_{pl,d}(P_d,I)=I^T{p}_d$$
对于“检测到的线重投影误差”会有递归操作，一边优化相机位姿，一边把$E_{line,d}$近似到$E_{line}$

B.使用点和线的BA

相机位姿参数θ={R,t}在每一帧都用BA进行优化，θ约束在SE(3)李群中。
下面就是对BA要优化的cost function的定义（包括点和线两种特征）：
$X_j\in {\mathbb{R}}^3$是地图中的第j个点，对于第i个关键帧，这个点可以被投影到图像平面上，有
$${\tilde{x}}_{i,j}=\pi (X_j,{\theta }_i,K)(5)$$
${\theta }_i$就是第i个关键帧的位姿。又知道这个点的观测量$x_{i,j}$，那么3D误差就为：
$$e_{i,j}=x_{i,j}-{\tilde{x}}_{i,j}(6)$$
类似的，$P_j,Q_j$是地图上的第j条线段，对应的图像投影（第i个关键帧）可以被写成：
$$\begin{gathered} {\tilde{p}}_{i,j}^h=\pi (P_j,{\theta }_i,K)(7) \\ {\tilde{q}}_{i,j}^h=\pi (Q_j,{\theta }_i,K)(8) \end{gathered}$$
已知在图像上的第j条线段两个端点的观测值$p_{i,j}$和$q_{i,j}$，可以使用公式（1）来估计观测到的线系数${\tilde{I}}_{i,j}$(这里的h表示齐次坐标)。所以，定义线的误差向量为(点到线的误差)：
$$\begin{gathered} e_{i,j}^{{}^{\prime }}=({\tilde{I}}_{i,j}{)}^T(K^{-1}{p}_{i,j}^h)(9) \\ {e}_{i,j}^{{}^{\prime \prime }}=({\tilde{I}}_{i,j}{)}^T(K^{-1}{q}_{i,j}^h)(10) \end{gathered}$$
按照文献[30]中解释的，这个误差值在端点沿着3D线发生偏移的时候也会发生变化，作为隐式正则化，允许我们在 BA 中使用这种非最小线参数化（？）。
然后，就把两种误差统一到一个cost function中：
$$C=\sum _{i,j}\rho (e_{i,j}^T{\Omega }_{i,j}^{-1}{e}_{i.j}+{e_{i,j}^{{}^{\prime }}}^T{{\Omega }_{i,j}^{{}^{\prime }}}^{-1}{e}_{i,j}^{{}^{\prime }}+{e_{i,j}^{{}^{\prime \prime }}}^T{{\Omega }_{i,j}^{{}^{\prime \prime }}}^{-1}{e}_{i,j}^{{}^{\prime \prime }}$$

这里的$\rho$是Huber robust cost function，三个$\Omega$是分别与检测到关键点和线端点的尺度相关的协方差矩阵。

C.全局重定位

当相机的追踪失效时，就要进行重定位，一种典型的解决方式就是PnP算法，它可以利用匹配好的之前关键帧的3D地图点来估计当前帧（lost）的位姿。在PnP之上，还用RANSAC来去除外点匹配。
ORB-SLAM中使用的是EPnP，但它只能使用点来作为输入；所以为了解决线特征的重定位，使用了EPnPL，它可以最小化“检测到的线重投影误差”，即公式（4）。
EPnPL的优点: 对线遮挡和误检测的情况有鲁棒性
为什么EPnPL鲁棒呢，具体是怎么做的呢？
因为这个方法分为两个步骤
1）先最小化检测到的线重投影误差，并估计线的端点$p_d,q_d$
2）再沿着线移动观测到的端点，以便于能匹配到三维空间端点P，Q的投影${\tilde{p}}_d,{\tilde{q}}_d$.只要这些匹配建立了，相机的位姿就可以被可靠的估计出来。

V. MAP INITIALIZATION WITH LINES

这一节介绍了怎么只用线匹配来估计初始地图（进行初始化）。
ORB-SLAM的初始化：使用至少两帧的点特征对应关系，用单应性矩阵或者本质矩阵估计算法来算出初始地图和位姿参数。
本论文提出的：
从图中看到，P,Q是三维空间中一条线段的两个端点，把这两端点投影到3帧相机视角下，{p1,q1},{p1,q1},{p1,q1}\{p_1, q_1\}, \{p_1, q_1\}, \{p_1, q_1\}{p1​,q1​},{p1​,q1​},{p1​,q1​}是每一帧的端点投影坐标，$I_1,I_2,I_3\in {\mathbb{R}}^3$是对应的线系数（由投影的端点算出）。
算法提出的假设： 在连续相机位姿之间，只有小且连续的旋转，所以有第一到第二帧的旋转和第二到第三帧的旋转一样。
在这个假设下，三个相机旋转是$R_1=R^T,R_2=I,R_3=R$, I是3X3单位阵。
注意，线系数Ii,i={1,2,3}I_i,i=\{1,2,3\}Ii​,i={1,2,3}也代表了向量的参数，该向量垂直于由投影中心$O_i$和投影点$p_i,q_i$形成的平面。这样的两个向量$I_i$的叉乘将和直线P,Q平行，同时与第三个矢量正交，所有这些矢量经过适当的旋转并放入到公共参考系中。这个约束可以写为：
$$I_2^T((R^T{I}_1)\times (R{I}_3))=0(11)$$
对于小旋转，R可以近似为：
$$R=\left(\begin{array}{ccc}1& -r_3& r_2\\ r_3& 1& -r_1\\ -r_2& r_1& 1\end{array}\right)(12)$$

对于这个参数化，有三条匹配线，我们将会有三个含有三个未知数$r_1,r_2,r_3$的二次方程，如方程11。我们调整文献[15]的多项式求解器，最多可以得到8个解。对每个可能的旋转矩阵，我们可以通过三焦张量方程[11]来获得 t1,t3。假设 t2=0。我们评估8个可能解，然后保留使得方程11最小的那个解。

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

论文翻译： https://blog.youkuaiyun.com/u013341645/article/details/78668316