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

2.1 基于线的重投影误差

这部分基本都是原文的翻译，所以本文主要参考的文章就是论文笔记-PL-SLAM进行学习，并在基础上加入了自己的理解。根据参考文献[30], P,QP,QP,Q 是一条线段的两个端点， pd,qd∈R2p_d, q_d \in \mathbb R^2pd,qd∈R2是这两端点在图像平面的2D检测结果，pdh,qdh∈R3p_d^h, q_d^h \in \mathbb R^3pdh,qdh∈R3是对应的齐次坐标,这算是pd,qd∈R2p_d, q_d \in \mathbb R^2pd,qd∈R2的逆投影到空间中。从齐次坐标，可以得到一个归一化的线系数：
I=pdh×qdh∣pdh×qdh∣(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,QP,QP,Q是三维空间中某一条线段的端点（绿色点），而在图像中也有对应的两个绿点，这个就是由真实的P,QP,QP,Q投影过来的。而在图像中，本身就存在两个拍摄下来的端点 pd,qd∈R2p_d, q_d \in \mathbb R^2pd,qd∈R2（对应于P,QP,QP,Q的观测值），观测值对应三维空间的端点Pd,Qd∈R3P_d, Q_d \in \mathbb R^3Pd,Qd∈R3. III是检测到的线系数，而I~\widetilde{I}I 则是指的投影的线系数，对应的公式在上方。
对于右图：
图中表示了检测到的线段和投影的线段在平面上的关系，如果存在夹角，则d1,d2d_1, d_2d1,d2就是我们要的线重投影误差，而 d1′,d2′d_1^{'}, d_2^{'}d1′,d2′是检测到的线重投影误差（检测到的2D线段（蓝色）和对应的投影的3D线段（绿色）之间的误差）。

线重投影误差的公式就为：
Eline(P,Q,I,θ,K)=Epl2(P,I,θ,K)+Epl2(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)

Epl(P,I,θ,K)=ITπ(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)
III是检测到的线系数， π(P,θ,K)\pi(P,θ,K)π(P,θ,K)是端点P在图像上的投影，K是内参矩阵，相机参数θ={R,t}\theta = \{R,t\}θ={R,t}

注意！在实际中，由于线的遮挡和误检测，图像中检测到的线段端点 pd,qdp_d, q_dpd,qd往往不能和 P,QP,QP,Q匹配，所以这里才又定义了个检测到的线投影误差。
检测到的线重投影误差的公式就为：
Eline,d(pd,qd,I)=Epl,d2(pd,I)+Epl,d2(qd,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)

Epl,d(pd,I)=ITpdE_{pl,d}(p_d,I)=I^T p_dEpl,d(pd,I)=ITpd
对于“检测到的线重投影误差”会有递归操作，一边优化相机位姿，一边把 Eline,dE_{line,d}Eline,d近似到到方程（2）中定义的ElineE_{line}Eline

2.2 使用点和线的BA

相机位姿参数θ=R,tθ={R,t}θ=R,t在每一帧都用BA进行优化，θθθ约束在SE(3)李群中。
下面就是对BA要优化的cost function的定义（包括点和线两种特征）：
Xj∈R3X_j \in \mathbb R^3Xj∈R3是地图中的第jjj个点，对于第iii个关键帧，这个点可以被投影到图像平面上，这部分内容是由公式（3）和公示（4）转变过来，从而得到x~i,j\widetilde{x}_{i,j}xi,j的等式：

x~i,j=π(Xj,θi,K)(5)\widetilde{x}_{i,j} = \pi (X_j, \theta_i, K) \space \space \space(5)xi,j=π(Xj,θi,K) (5)

θi\theta_iθi就是第iii个关键帧的位姿。又知道这个点的观测量xi,jx_{i,j}xi,j，那么3D误差就为：

ei,j=xi,j−x~i,j(6)e_{i,j} = x_{i,j} - \widetilde{x}_{i,j} \space \space \space(6)ei,j=xi,j−xi,j (6)

类似的， Pj,QjP_j, Q_jPj,Qj表示地图上第j个线特征的端点。对应的投影到相同的关键帧上的图像投影点（表示为齐次坐标，这里的hhh表示齐次坐标）可以写为：

p~i,jh=π(Pj,θi,K)(7)q~i,jh=π(Qj,θ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)pi,jh=π(Pj,θi,K) (7)qi,jh=π(Qj,θi,K) (8)

已知在图像上的第jjj条线段两个端点的观测值 pi,jp_{i,j}pi,j和 qi,jq_{i,j}qi,j，可以使用公式（1）来估计观测到的线系数I~i,j\widetilde{I}_{i,j}Ii,j。所以，定义线的误差向量为(点到线的误差), 其中 K−1K^{-1}K−1代表了二维点通过内参计算出三维点的反向投影形式。然后计算三维点层面上误差距离。

ei,j′=(I~i,j)T(K−1pi,jh)(9)ei,j′′=(I~i,j)T(K−1qi,jh)(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′=(Ii,j)T(K−1pi,jh) (9)ei,j′′=(Ii,j)T(K−1qi,jh) (10)

误差(9)(10)实际上是点到线的距离误差(3)。按照文献[30]中解释的，这个误差值在端点 Pj,QjP_j, Q_jPj,Qj沿着3D线发生偏移的时候也会发生变化，作为隐式正则化，允许我们在 BA 中使用这种非最小线参数化。

然后，就把两种误差统一到一个cost function中：

C=∑i,jρ(ei,jTΩi,j−1ei.j+ei,j′TΩi,j′−1ei,j′+ei,j′′TΩi,j′′−1ei,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）先最小化检测到的线重投影误差，并估计线的端点 pd,qdp_d, q_dpd,qd
2）再沿着线移动观测到的端点，以便于能匹配到三维空间端点P，Q的投影p~d,q~d\widetilde{p}_d, \widetilde{q}_dpd,qd.只要这些匹配建立了，相机的位姿就可以被可靠的估计出来。

3. 线特征初始化地图

…详情请参照古月居

经典文献阅读之--PL-SLAM(点线SLAM)相关推荐

经典文献阅读之--NICE-SLAM（SLAM的神经隐含可扩展编码）
0. 简介对于深度学习而言,NeRF一定是最近两年最火的工作之一了,**NeRF(Neural Radiance Fields)**是最早在2020年ECCV会议上的Best Paper,其将隐式表 ...
货运服务网络设计：经典文献阅读笔记（3）复现Netplan
**货运服务网络设计:经典文献阅读笔记(2)**提到说要把Crainic T G(1984年)文献使用的模型复现一下,但是文章给出的通用框架还是太笼统,在尝试后决定使用Jacques Roy & ...
经典文献阅读之--Swin Transformer
0. 简介 Transfomer最近几年已经霸榜了各个领域,之前我们在<经典文献阅读之–Deformable DETR>这篇博客中对DETR这个系列进行了梳理,但是想着既然写了图像处理领域 ...
经典文献阅读之--OV2SLAM(高速视觉slam)
0. 简介视觉里程计最近几年越来越受到学术界以及工业界的认可,以ORB和VINS为代表的视觉SLAM已经可以满足绝大多数场景,而OV2SLAM在其他VSLAM中脱颖而出,其实时性以及具体的回环性能在 ...
经典文献阅读之--Lifelong SLAM(变化环境中Lifelong定位建图)
0. 简介商场.超市等大多数现实场景的环境随时都在变化.不考虑这些变化的预建地图很容易变得过时.因此,有必要拥有一个最新的环境模型,以促进机器人的长期运行.为此<A General Frame ...
经典文献阅读之--SuMa++
0. 简介作为一名技术博主,最主要的还是需要不断地学习新的知识,而最好的学习就是不断地阅读新的文章,并不断地学习和总结前人的思路和方法.所以博主打算开一个新的系列来介绍.这里主要来介绍一下<S ...
货运服务网络设计：经典文献阅读笔记（1）
服务网络设计 --(1984,Crainic,Multicommodity, Multimode Freight Transportation: A General Modeling and Algo ...
经典文献阅读之--PON
0. 简介作为Transformer在机器视觉领域的爆火,在自动驾驶领域目前很多工作都集中在前视转鸟瞰图的方法中,这里我们来讲2020年一篇经典的论文<Predicting Semantic ...
经典文献阅读之--Globally Consistent and Tightly Coupled 3D LiDAR Inertial Mapping（紧耦合3D激光雷达）
0. 简介传统的地图生成方法一般是依靠Lidar和IMU结合的,但是问题在于,目前Lidar和IMU的紧耦合主要集中在前端里程计,基本没有涉及到后端全局优化以及建图的融合.为此文章<Globa ...

经典文献阅读之--PL-SLAM(点线SLAM)