经典文献阅读之--PL-SLAM(点线SLAM)
0. 简介
之前作者基本都在围绕着特征点提取的路径在学习，最近看到了最近点云PCL推送的《Structure PLP-SLAM: Efficient Sparse Mapping and Localization using Point, Line and Plane for Monocular, RGB-D and Stereo Cameras》。这个工作是基于OpenVSLAM架构的，但是由于OpenVSLAM被认为侵权，所以作者想从PL-SLAM开始，学习一下点线面SLAM的相关原理以及知识。（因为是基于ORB-SLAM2的）
1. PL-SLAM 文章贡献
PL-SLAM是基于ORB-SLAM2框架和LSD（Line segment Detector）来做的SLAM，主要是结合点线特征，提高SLAM在环境中运行的可靠性。基本框架与ORB-SLAM2一致，但是在代码中并行地添加了点和线的特征。文中使用了LSD去提取线特征，官网上的开源代码，就直接拿来测试了一下：
提出了PL-SLAM，这是一种立体视觉SLAM系统，它结合了点和线段，能够在更广泛的场景中稳定地工作。PL-SLAM将点和线特征应用在slam全过程。通过线特征的长度和方向的比较，通过剔除异常值来解决特定于线段的跟踪和匹配问题，而对于残差计算，我们用线段的端点坐标表示地图中的线段。因此，通过图像平面上这些端点的投影与与被观测线段相关的无穷多条直线之间的距离来计算被观测线段与其在地图中的对应直线之间的残差。这两种特征也被用于在相机导航期间鲁棒地检测循环闭合，这是一种**新的单词包（BoW）**方法，它结合了使用它们执行位置识别的优点。所以综上来说本文的主要贡献点主要有三点：
1）第一个实时开源的同时使用点和线分割特征深度SLAM系统，并且在弱纹理环境中传统点特征方法失效的情况下拥有较高的运行鲁棒性。
2）一种新的同时处理点和线特征调整关键字位姿。
3）一种扩展BoW方法同时考虑点和线特征分割提高闭环检测
2. 具体算法
图中的算法为PL-SLAM框架的整个示意图。该文提出的PL-SLAM是在ORB-SLAM的框架基础之上，添加了一些与线特征有关的模块，从而构建了一套单目SLAM系统。整个系统的框架如下图所示：
可以看得出来，就是比ORB-SLAM多了线特征的流程，所以，接下来，主要把篇幅放在如何将线特征整合进来：
在PL-SLAM中，和线特征相关的处理包括了：
- 检测：使用LSD的方法，时间复杂度是 O ( n ) O(n) O(n)， n n n为图片中像素的个数
- 三角化
- 匹配：使用了以Line Band Descriptors为基础的方法，通过一个关系图（relational graph）当前的线会和已经在地图中的线进行配对。在获得了初始的map-to-image的线特征对集合后，所有在局部地图中的线都被投影到图像上，进一步寻找匹配对，然后，如果这个图片有足够多的新环境信息，他就会被标记为关键帧，它对应的线会被三角化并添加进地图。
- 线的剔除：从少于三个视点或少于 25% 的帧中看到的线会被丢弃（剔除）。
- 优化：线在地图中的位置使用局部BA进行优化。
- 重定位： 注意，因为对整个地图进行线的匹配的计算量非常大，所以回环检测中只使用点特征。
2.1 基于线的重投影误差
这部分基本都是原文的翻译，所以本文主要参考的文章就是论文笔记-PL-SLAM进行学习，并在基础上加入了自己的理解。根据参考文献[30], P , Q P,Q P,Q 是一条线段的两个端点， p d , q d ∈ R 2 p_d, q_d \in \mathbb R^2 pd,qd∈R2是这两端点在图像平面的2D检测结果， p d h , q d h ∈ R 3 p_d^h, q_d^h \in \mathbb R^3 pdh,qdh∈R3是对应的齐次坐标,这算是 p d , q d ∈ R 2 p_d, q_d \in \mathbb R^2 pd,qd∈R2的逆投影到空间中。从齐次坐标，可以得到一个归一化的线系数：
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,Q是三维空间中某一条线段的端点（绿色点），而在图像中也有对应的两个绿点，这个就是由真实的 P , Q P,Q P,Q投影过来的。而在图像中，本身就存在两个拍摄下来的端点 p d , q d ∈ R 2 p_d, q_d \in \mathbb R^2 pd,qd∈R2（对应于 P , Q P,Q 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 , q d , I ) = E p l , d 2 ( p d , I ) + E p l , d 2 ( q d , I ) ( 4 ) E_{line,d}(p_d, q_d,I)=E^2_{pl,d}(p_d,I)+E^2_{pl,d}(q_d, I) \space \space \space(4) Eline,d(pd,qd,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近似到到方程（2）中定义的 E l i n e E_{line} Eline
2.2 使用点和线的BA
相机位姿参数 θ = R , t θ={R,t} θ=R,t在每一帧都用BA进行优化， θ θ θ约束在SE(3)李群中。
下面就是对BA要优化的cost function的定义（包括点和线两种特征）：
X j ∈ R 3 X_j \in \mathbb R^3 Xj∈R3是地图中的第 j j j个点，对于第 i i i个关键帧，这个点可以被投影到图像平面上，这部分内容是由公式（3）和公示（4）转变过来，从而得到 x ~ i , j \widetilde{x}_{i,j} x i,j的等式：
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 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个线特征的端点。对应的投影到相同的关键帧上的图像投影点（表示为齐次坐标，这里的 h h h表示齐次坐标）可以写为：
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 j 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。所以，定义线的误差向量为(点到线的误差), 其中 K − 1 K^{-1} K−1代表了二维点通过内参计算出三维点的反向投影形式。然后计算三维点层面上误差距离。
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)
误差(9)(10)实际上是点到线的距离误差(3)。按照文献[30]中解释的，这个误差值在端点 P j , Q j P_j, Q_j Pj,Qj沿着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核函数，三个 Ω \Omega Ω是分别与检测到关键点和线端点的尺度相关的协方差矩阵。
2.3.全局重定位
当相机的追踪失效时，就要进行重定位，一种典型的解决方式就是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.只要这些匹配建立了，相机的位姿就可以被可靠的估计出来。