论文笔记-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 ) O(n) O(n)，n为图片中像素的个数
- 三角化
- 匹配：使用了以Line Band Descriptors为基础的方法，通过一个关系图（relational graph）当前的线会和已经在地图中的线进行配对。在获得了初始的map-to-image的线特征对集合后，所有在局部地图中的线都被投影到图像上，进一步寻找匹配对，然后，如果这个图片有足够多的新环境信息，他就会被标记为关键帧，它对应的线会被三角化并添加进地图。
- 线的剔除：从少于三个视点或少于 25% 的帧中看到的线会被丢弃（剔除）。
- 优化：线在地图中的位置使用局部BA进行优化。
- 重定位： 注意，因为对整个地图进行线的匹配的计算量非常大，所以回环检测中只使用点特征。
IV. LINE-BASED SLAM
这一节重点介绍线的参数化方式以及误差函数，还会解释怎么把它用到特征匹配，BA和全局重定位中。
A.基于线的重投影误差
根据参考文献[30], P,Q是一条线段的两个端点， p d , q d ∈ R 2 \textbf{p}_d, \textbf{q}_d \in \mathbb R^2 pd,qd∈R2是这两端点在图像平面的2D检测结果， p d h , q d h ∈ R 3 \textbf{p}_d^h, \textbf{q}_d^h \in \mathbb R^3 pdh,qdh∈R3是对应的齐次坐标。从齐次坐标，可以得到一个归一化的线系数：
I = p d h × q d h ∣ p d h × q d h ∣ ( 1 ) I= \frac{\textbf{p}_d^h \times \textbf{q}_d^h}{\vert \textbf{p}_d^h \times \textbf{q}_d^h \vert} \space \space \space (1) I=∣pdh×qdh∣pdh×qdh (1)
有了这个系数，下面再来看线的重投影误差，它是点到线距离之和，这里点是指的两端点投影后的结果，线是指的在图像中检测到的线段。具体可以看下面这张图：
对于左图：
P,Q是三维空间中某一条线段的端点（绿色点），而在图像中也有对应的两个绿点，这个就是由真实的P,Q投影过来的。而在图像中，本身就存在两个拍摄下来的端点 p d , q d ∈ R 2 p_d, q_d \in \mathbb R^2 pd,qd∈R2（对应于P,Q的观测值），观测值对应三维空间的端点 P d , Q d ∈ R 3 P_d, Q_d \in \mathbb R^3 Pd,Qd∈R3. I I I是检测到的线系数，而 I ~ \widetilde{I} I 则是指的投影的线系数。
对于右图：
d 1 , d 2 d_1, d_2 d1,d2就是我们要的线重投影误差，而 d 1 ′ , d 2 ′ d_1^{'}, d_2^{'} d1′,d2′是检测到的线重投影误差（检测到的2D线段（蓝色）和对应的投影的3D线段（绿色）之间的误差）。
线重投影误差的公式就为：
E l i n e ( P , Q , I , θ , K ) = E p l 2 ( P , I , θ , K ) + E p l 2 ( Q , I , θ , K ) ( 2 ) E_{line}(P,Q,I,θ,K)=E^2_{pl}(P,I,θ,K)+E^2_{pl}(Q,I,θ,K) \space \space \space(2) Eline(P,Q,I,θ,K)=Epl2(P,I,θ,K)+Epl2(Q,I,θ,K) (2)
E p l ( P , I , θ , K ) = I T π ( P , θ , K ) ( 3 ) E_{pl}(P,I,θ,K)=I^T \pi(P,θ,K) \space \space \space(3) Epl(P,I,θ,K)=ITπ(P,θ,K) (3)
I I I是检测到的线系数， π ( P , θ , K ) \pi(P,θ,K) π(P,θ,K)是端点P在图像上的投影，K是内参矩阵，相机参数 θ = { R , t } \theta = \{R,t\} θ={R,t}
注意！在实际中，由于线的遮挡和误检测，图像中检测到的线段端点 p d , q d p_d, q_d pd,qd往往不能和 P , Q P,Q P,Q匹配，所以这里才又定义了个检测到的线投影误差。
检测到的线重投影误差的公式就为：
E l i n e , d ( p d , p d , I ) = E p l , d 2 ( p d , I ) + E p l , d 2 ( q d , I ) ( 4 ) E_{line,d}(p_d, p_d,I)=E^2_{pl,d}(p_d,I)+E^2_{pl,d}(q_d, I) \space \space \space(4) Eline,d(pd,pd,I)=Epl,d2(pd,I)+Epl,d2(qd,I) (4)
E p l , d ( P d , I ) = I T p d E_{pl,d}(P_d,I)=I^T p_d Epl,d(Pd,I)=ITpd
对于“检测到的线重投影误差”会有递归操作，一边优化相机位姿，一边把 E l i n e , d E_{line,d} Eline,d近似到 E l i n e E_{line} Eline
B.使用点和线的BA
相机位姿参数θ={R,t}在每一帧都用BA进行优化，θ约束在SE(3)李群中。
下面就是对BA要优化的cost function的定义（包括点和线两种特征）：
X j ∈ R 3 X_j \in \mathbb R^3 Xj∈R3是地图中的第j个点，对于第i个关键帧，这个点可以被投影到图像平面上，有
x ~ i , j = π ( X j , θ i , K ) ( 5 ) \widetilde{x}_{i,j} = \pi (X_j, \theta_i, K) \space \space \space(5) x i,j=π(Xj,θi,K) (5)
θ i \theta_i θi就是第i个关键帧的位姿。又知道这个点的观测量 x i , j x_{i,j} xi,j，那么3D误差就为：
e i , j = x i , j − x ~ i , j ( 6 ) e_{i,j} = x_{i,j} - \widetilde{x}_{i,j} \space \space \space(6) ei,j=xi,j−x i,j (6)
类似的， P j , Q j P_j, Q_j Pj,Qj是地图上的第j条线段，对应的图像投影（第i个关键帧）可以被写成：
p ~ i , j h = π ( P j , θ i , K ) ( 7 ) q ~ i , j h = π ( Q j , θ i , K ) ( 8 ) \widetilde{p}^h_{i,j} = \pi (P_j, \theta_i, K) \space \space \space(7) \\ \widetilde{q}^h_{i,j} = \pi (Q_j, \theta_i, K) \space \space \space(8) p i,jh=π(Pj,θi,K) (7)q i,jh=π(Qj,θi,K) (8)
已知在图像上的第j条线段两个端点的观测值 p i , j p_{i,j} pi,j和 q i , j q_{i,j} qi,j，可以使用公式（1）来估计观测到的线系数 I ~ i , j \widetilde{I}_{i,j} I i,j(这里的h表示齐次坐标)。所以，定义线的误差向量为(点到线的误差)：
e i , j ′ = ( I ~ i , j ) T ( K − 1 p i , j h ) ( 9 ) e i , j ′ ′ = ( I ~ i , j ) T ( K − 1 q i , j h ) ( 10 ) e_{i,j}^{'} = (\widetilde{I}_{i,j})^T(K^{-1}p_{i,j}^h) \space \space \space(9)\\ e_{i,j}^{''} = (\widetilde{I}_{i,j})^T(K^{-1}q_{i,j}^h) \space \space \space(10) ei,j′=(I i,j)T(K−1pi,jh) (9)ei,j′′=(I i,j)T(K−1qi,jh) (10)
按照文献[30]中解释的，这个误差值在端点沿着3D线发生偏移的时候也会发生变化，作为隐式正则化，允许我们在 BA 中使用这种非最小线参数化（？）。
然后，就把两种误差统一到一个cost function中：
C = ∑ i , j ρ ( e i , j T Ω i , j − 1 e i . j + e i , j ′ T Ω i , j ′ − 1 e i , j ′ + e i , j ′ ′ T Ω i , j ′ ′ − 1 e i , j ′ ′ C = \sum_{i,j} \rho (e_{i,j}^T \Omega_{i,j}^{-1}e_{i.j} +{e_{i,j}^{'}}^{T}{\Omega_{i,j}^{'}}^{-1}e_{i,j}^{'} + {e_{i,j}^{''}}^{T}{\Omega_{i,j}^{''}}^{-1}e_{i,j}^{''} C=i,j∑ρ(ei,jTΩi,j−1ei.j+ei,j′TΩi,j′−1ei,j′+ei,j′′TΩi,j′′−1ei,j′′
这里的 ρ \rho ρ是Huber robust cost function，三个 Ω \Omega Ω是分别与检测到关键点和线端点的尺度相关的协方差矩阵。
C.全局重定位
当相机的追踪失效时，就要进行重定位，一种典型的解决方式就是PnP算法，它可以利用匹配好的之前关键帧的3D地图点来估计当前帧（lost）的位姿。在PnP之上，还用RANSAC来去除外点匹配。
ORB-SLAM中使用的是EPnP，但它只能使用点来作为输入；所以为了解决线特征的重定位，使用了EPnPL，它可以最小化“检测到的线重投影误差”，即公式（4）。
EPnPL的优点: 对线遮挡和误检测的情况有鲁棒性
为什么EPnPL鲁棒呢，具体是怎么做的呢？
因为这个方法分为两个步骤
1）先最小化检测到的线重投影误差，并估计线的端点 p d , q d p_d, q_d pd,qd
2）再沿着线移动观测到的端点，以便于能匹配到三维空间端点P，Q的投影 p ~ d , q ~ d \widetilde{p}_d, \widetilde{q}_d p d,q d.只要这些匹配建立了，相机的位姿就可以被可靠的估计出来。
V. MAP INITIALIZATION WITH LINES
这一节介绍了怎么只用线匹配来估计初始地图（进行初始化）。
ORB-SLAM的初始化：使用至少两帧的点特征对应关系，用单应性矩阵或者本质矩阵估计算法来算出初始地图和位姿参数。
本论文提出的：
从图中看到，P,Q是三维空间中一条线段的两个端点，把这两端点投影到3帧相机视角下， { p 1 , q 1 } , { p 1 , q 1 } , { p 1 , q 1 } \{p_1, q_1\}, \{p_1, q_1\}, \{p_1, q_1\} {p1,q1},{p1,q1},{p1,q1}是每一帧的端点投影坐标， I 1 , I 2 , I 3 ∈ R 3 I_1, I_2, I_3 \in \mathbb R^3 I1,I2,I3∈R3是对应的线系数（由投影的端点算出）。
算法提出的假设： 在连续相机位姿之间，只有小且连续的旋转，所以有第一到第二帧的旋转和第二到第三帧的旋转一样。
在这个假设下，三个相机旋转是 R 1 = R T , R 2 = I , R 3 = R R_1 = R^T, R_2=I, R_3=R R1=RT,R2=I,R3=R, I是3X3单位阵。
注意，线系数 I i , i = { 1 , 2 , 3 } I_i,i=\{1,2,3\} Ii,i={1,2,3}也代表了向量的参数，该向量垂直于由投影中心 O i O_i Oi和投影点 p i , q i p_i,q_i pi,qi形成的平面。这样的两个向量 I i I_i Ii的叉乘将和直线P,Q平行，同时与第三个矢量正交，所有这些矢量经过适当的旋转并放入到公共参考系中。这个约束可以写为：
I 2 T ( ( R T I 1 ) × ( R I 3 ) ) = 0 ( 11 ) I^T_2((R^TI_1)×(RI_3))=0 \space \space \space(11) I2T((RTI1)×(RI3))=0 (11)
对于小旋转，R可以近似为：
R = ( 1 − r 3 r 2 r 3 1 − r 1 − r 2 r 1 1 ) ( 12 ) R= \left( \begin{array}{ccc} 1& -r_3 & r_2 \\ r_3 & 1& -r_1 \\ -r_2 & r_1 & 1\\ \end{array}\right) \space \space \space(12) R=⎝⎛1r3−r2−r31r1r2−r11⎠⎞ (12)
对于这个参数化，有三条匹配线，我们将会有三个含有三个未知数 r 1 , r 2 , r 3 r_1,r_2,r_3 r1,r2,r3的二次方程，如方程11。我们调整文献[15]的多项式求解器，最多可以得到8个解。对每个可能的旋转矩阵，我们可以通过三焦张量方程[11]来获得 t1,t3。假设 t2=0。我们评估8个可能解，然后保留使得方程11最小的那个解。
为了在使用三焦点张量方程求解平移分量时，获得足够的独立约束，我们需要两个额外的线对应关系，因此，我们算法所需的线特征匹配数量是5。
论文翻译： https://blog.csdn.net/u013341645/article/details/78668316