PL-SLAM: Real-Time Monocular Visual SLAM with Points and Lines

PL-SLAM

文章目录

• • PL-SLAM

  • • 摘要
    • 系统介绍
    • • 综述

  • 方法综述

    • LINE-BASED SLAM
    • • 一、基于线的SLAM

  • 二、基于线和点的BA

  • 三、全局重定位

    • 使用线条初始化地图
    • 实验结果

  • 说明

  • • 位姿求解
    • 三角化
    • LSD 直线检测算法
    • • 📊 一、核心原理 * ⚙️ 二、实现方法 * 📐 三、应用场景 * ⚖️ 四、优缺点与优化 * 优缺点对比 * 💎 总结 * End

摘要

译文

• ——众所周知，低纹理场景是依赖点对应的几何计算机视觉算法的主要致命弱点之一，尤其是对于视觉 SLAM。 然而，在许多环境中，尽管纹理较低，但仍然可以可靠地估计基于线的几何基元，例如在城市和室内场景中，或者在结构化边缘占主导地位的所谓“曼哈顿世界”中。 在本文中，我们提出了一种解决方案来处理这些情况。 具体来说，我们建立在 ORB-SLAM 的基础上，大概是当前最先进的解决方案，无论是在准确性还是效率方面，并扩展其公式以同时处理点和线对应。
• 我们提出了一种解决方案，即使大多数点从输入图像中消失，它也可以工作，而且有趣的是，它可以仅通过检测三个连续帧中的线对应关系来初始化。 我们在 TUM RGB-D 基准测试中彻底评估了我们的方法和新的初始化策略，并证明了线的使用不仅提高了原始 ORB-SLAM 解决方案在纹理不良帧中的性能，而且在序列帧中系统地改进了它 结合点和线，不影响效率。

描述

首先指出了为什么要使用线特征：

复制代码

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

* 最终，选择了用端点来参数化，原因是：这种方式对于遮挡和无检测的情况更加鲁棒。

系统介绍

综述

• 跟orb-slam一样，Tracking线程 用来估计相机的位姿和决定是否要加入新关键帧；通过局部地图构建将新关键帧信息增加到地图并用BA优化；Loop closing线程 持续的进行闭环检测并修正。
• 
  

方法综述

在PL-SLAM中，与线特征相关的处理包括了：

检测：使用LSD的方法，时间复杂度是 O(n)，n为图片中像素的个数。

三角化：

匹配：

* 使用了以 `Line Band Descriptors`为基础的方法，通过一个关系图（relational graph）当前的线会和已经在地图中的线进行配对。
* 在获得了初始的`map-to-image`的线特征对集合后，所有在局部地图中的线都被投影到图像上，进一步寻找匹配对，然后，如果这个图片有足够多的新环境信息，他就会被标记为关键帧，它对应的线会被三角化并添加进地图。

线的剔除：从少于三个视点或少于 25% 的帧中看到的线会被丢弃（剔除）。

优化：线在地图中的位置使用局部BA进行优化。

重定位： 注意，因为对整个地图进行线的匹配的计算量非常大，所以回环检测中只使用点特征。

LINE-BASED SLAM

一、基于线的SLAM

1、线段代替直线

• 对于线的描述，采用了 端点(endpoints) 的方法，毕竟现实中，几乎不不可能碰到无限延伸的直线，几乎都是线段，所以，用端点的方法来描述直线是比较合理的。

2、计算直线齐次方程

我们假设 P, Q \in R^3 是空间直线的两个端点，p_d, q_d \in R^2是其在图像中的投影坐标，另外，记 p_d^h, q_d^h \in R^3 是对应的齐次坐标。于是，我们可以得到直线的参数：

* $I = \frac{p_d^h \times q_d^h}{ |p_d^h \times q_d^h| }$

其次坐标说明：

复制代码

* 对于向量(x,y,1)，可以在几何意义上理解为是在【第三维为常数的】平面上的一个二维向量。
* 这种用3维向量表示2维向量，或者一般而言，用一个n+1维的向量表示一个n维向量的方法称为**齐次坐标** 表示法。。

3、有了直线参数，下面再来看线的重投影误差，它是点到线距离之和。

3.1 图像说明误差

左图：

* 真实 $P,Q$ 两点投影过来 
  * $\mathbf{P}, \mathbf{Q} \in \mathbb{R}^3$表示3D线特征上的两个3D点（绿色点）
  * $\widetilde{\mathbf{p}}, \widetilde{\mathbf{q}} \in \mathbb{R}^2 $是他们在 Image图像平面上的投影点（绿色点）
  * $\tilde{l}$ 是投影的线的系数。

* 相机本身测量 
  * $\mathbf{p}_{\mathrm{d}}, \mathbf{q}_{\mathrm{d}} \in \mathbb{R}^2$是提取出的线特征（线段）的两个端点，测量值
  * $\mathbf{P}, \mathbf{Q} \in \mathbb{R}^3$ 就是这两个端点反投影回到3D空间中对应的3D点，
  * $I$ 是检测到的线的系数。

右图：

* $d_1、d_2$ 就是我们要的线重投影误差。$d'_1、d'_2$ 就是我们要的线重投影误差。
* （检测到的2D线段（蓝色）和对应的投影的3D线段（绿色）之间的误差）。

3.2 定义线的重投影误差

投影线段的端点与在图像中检测到的直线之间的点到直线距离之和：

E_{\text {line }}(P, Q, I, \theta, k)=E_{p l}^2(P, I, \theta, k)+E_{p l}^2(Q, I, \theta, k) \space \space \space(2)

而 E_{p l}(P, I, \theta, k)=I^T \pi(P, \theta, K)

其中，I 为上面提到的直线参数，\pi(P, \theta, K)为空间点 P 投影到图像平面的坐标，而其中K 为相机内参，\theta = \{R_c,t_c\}为相机参数，表示平移和旋转。

理想情况下，空间点的投影点是在直线上的，所以点到直线距离 E_{p l}=0或者说，点在直线上。

通过这样的计算就可以获取到两条线之间的重投影误差了。

3.3 线的遮挡和误检，检测端点与P、Q无法直接匹配

• 在实际情况中，总会存在线之间有遮挡、误检测等状况，所以，我们在图像中检测到的端点p_d, q_d 可能与空间直线端点 P, Q 在图像中的投影点不匹配，
• 所以，这里还要定义一个“检测到的线的重投影误差”：
  • E_{\text {line }, d}\left(p_d, q_d, I\right)=E_{p l, d}^2\left(p_d, I\right)+E_{p l, d}^2\left(q_d, I\right) \space \space \space(4)
  • E_{p l, d}(p_d,I)=I^T p_d
  • 这儿，I 是投影的三维直线参数和检测到的点到线误差。

3.4 递推直到两线重合

• 对于“检测到的线重投影误差”会有递归操作，一边优化相机位姿，一边把 E_{line,d}近似到 E_{line}

二、基于线和点的BA

相机位姿参数 \theta={R,t} 在每一帧都用BA进行优化。下面定义点+线代价值。

点 Costfunction

• 我们定义X_j \in R^3为地图上的第 j 个点。对于第 i 个关键帧，X_j在该图像平面中的 投影点 为：

  • \tilde{x}_{i, j}=\pi\left(X_j, \theta_i, K\right)
  • 其中，\theta_i为第 i 个关键帧的相机位姿 \theta_i = \{R_i,t_i\}。

• 对于给定的观测点 x_{i, j}，我们可以定义如下的3D误差：

  • e_{i, j}=x_{i, j}-\tilde{x}_{i, j}

线 Costfunction

• 类似的，定义地图中第 j 个线段的端点P_j, Q_j。在第 i 个关键帧中的投影坐标（采用齐次坐标形式）为：

  • \tilde{p}_{i, j}^h=\pi\left(P_j, \theta_i, K\right)
  • \tilde{q}_{i, j}^h=\pi\left(Q_j, \theta_i, K\right)

• 然后，对于给定的所观测到的第 j 个直线端点 p_{i, j}, q_{i, j}，计算出观测到直线的参数 \tilde{I}_{i, j} ，则误差向量可以定义为：

  • e_{i, j}^{\prime}=\left(\tilde{I}_{i, j}\right)^T\left(K^{-1} p_{i, j}^h\right)
  • e_{i, j}^{\prime \prime}=\left(\tilde{I}_{i, j}\right)^T\left(K^{-1} q_{i, j}^h\right)
  • 按照文献[30]中解释的，这个误差值在端点沿着3D线发生偏移的时候也会发生变化，作为隐式正则化，允许我们在 BA 中使用这种非最小线参数化

• 于是，我们最终的误差目标函数就可以写成：

  • C=\sum_{i, j} \rho\left(\mathbf{e}_{i, j}^{\top} \Omega_{i, j}^{-1} \mathbf{e}_{i, j}+\mathbf{e}_{i, j}^{\prime}{ }^{\top} \Omega_{i, j}^{\prime}{ }^{-1} \mathbf{e}_{i, j}^{\prime}+\mathbf{e}_{i, j}^{\prime \prime} \Omega_{i, j}^{\prime \prime}{ }^{-1} \mathbf{e}_{i, j}^{\prime \prime}\right)
    • 其中，\rho(\cdot) 为鲁棒代价函数，文中使用的是Huber。
    • \Omega_{i, j}, \Omega_{i, j}^{\prime}, \Omega_{i, j}^{\prime \prime} 为关键点和线段端点在不同尺度下的协方差矩阵。

三、全局重定位

当相机的追踪失效时，就要进行重定位，一种典型的解决方式就是PnP算法，它可以利用匹配好的之前关键帧的3D地图点来估计当前帧（lost）的位姿。在PnP之上，还用RANSAC来去除外点匹配。

ORB-SLAM中使用的是EPnP，但它只能使用点来作为输入；所以为了解决线特征的重定位，使用了EPnPL，它可以最小化“检测到的线重投影误差”，即公式（4）。

* $E_{\text {line }, d}\left(p_d, q_d, I\right)=E_{p l, d}^2\left(p_d, I\right)+E_{p l, d}^2\left(q_d, I\right)$

EPnPL的优点 : 对线遮挡和误检测的情况有鲁棒性

• 因为这个方法分为两个步骤:
  • 1>先最小化检测到的线重投影误差，并估计线的端点 p_d, q_d。
  • 2>再沿着线移动观测到的端点，以便于能匹配到三维空间端点P，Q的投影\widetilde{p}_d, \widetilde{q}。只要这些匹配建立了，相机的位姿就可以被可靠的估计出来。

使用线条初始化地图

• 这一节介绍了怎么只用线匹配来估计初始地图（进行初始化）。
• ORB-SLAM的初始化：使用至少两帧的点特征对应关系，用单应性矩阵或者本质矩阵估计算法来算出初始地图和位姿参数。

本论文提出的

从图中看到，P,Q\in \mathbb R^3是三维空间中一条线段的两个端点，把这两端点投影到3帧相机视角下 \{\mathbf p_1,\mathbf q_1\}、\{\mathbf p_2,\mathbf q_2\}、\{\mathbf p_3,\mathbf q_3\} 是每一帧的端点投影坐标。\mathbf I_1、\mathbf I_2、\mathbf I_3 \in \mathbb R^3 是对应的线系数（由投影的端点算出）。

算法提出的假设：

• 在连续相机位姿之间，只有 小 且 连续 的旋转，所以有第一到第二帧的旋转和第二到第三帧的旋转一样。在这个假设下，三个相机旋转是R_1 = R^T, R_2=\mathbf I, R_3=R,\mathbf I 是 3\times3的单位阵。

• 注，线系数 \mathbf I_i,i=\{1,2,3\} 也代表了向量的参数，该向量垂直于由投影中心Q_i和投影点 \{\mathbf p_i,\mathbf q_i\}。这样的两个向量\mathbf I_i 的叉乘将和直线P、Q，同时与第三个矢量正交，所有这些矢量经过适当的旋转并放入到公共参考系中。这个约束可以写为：

  • \mathbf I^T_2((R^T\mathbf I_1)×(R\mathbf I_3))=0 \space \space \space(11)

• 对于小旋转，R 可以近似为：

  • R = \begin{pmatrix} 1 & -r_3 & r_2 \\ r_3 & 1 & -r_1 \\ -r_2 & r_1 & 1\end{pmatrix}

• 对于此参数化，由于有三条匹配线，我们将会有三个含有三个未知数 r_1,r_2,r_3 的二次方程，如方程11。我们采用 [15] 中的多项式求解器，最多可得到八个解。我们可以利用三焦点张量方程[11]得到来获得 t_1,t_2，该方程关于t_1、t_3 是线性的。假设 t_2 =0。我们评估八个可能的解，并保留使方程(11)最小化的解。

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

实验结果

• 我们使用 TUM RGB-D 基准 [28] 将我们的系统与当前最先进的 Visual SLAM 方法进行了比较。此外，我们还使用合成数据和真实数据评估了所提出的初始化方法，并比较了我们的 PL-SLAM 算法与 ORB-SLAM [18] 的计算时间。
• 所有实验均基于 Intel Core i7-4790（4 核 @3.6 GHz）、8GB RAM 和 ROS Hydro [22] 进行。
• 

定位精度

• 为了评估定位精度，我们将我们的 PL-SLAM 方法与当前最先进的 Visual SLAM 方法进行了比较，包括 ORB-SLAM [18]、PTAM [13]、LSD-SLAM [7] 和 RGBD-SLAM [6]。
• 比较使用的指标是绝对轨迹误差 (ATE)，由基准测试的评估脚本提供。在计算误差之前，所有轨迹都使用相似性扭曲进行对齐，RGBD-SLAM [6] 除外，它通过刚体变换进行对齐。

地图初始化 - 真实实验

• 我们还分别使用经典初始化方法（基于单应性矩阵或本质矩阵计算）和本文提出的仅基于直线的地图初始化方法（参见表一）来评估我们的PL-SLAM方法。正如预期的那样，由于旋转假设较小，直线地图初始化方法的准确率有所下降。然而，在低纹理序列 f3_nstr_tex_far 中，经典初始化方法检测到了模糊性，导致无法初始化地图。相比之下，本文提出的直线初始化方法能够估计初始地图

计算时间

• 虽然在视觉SLAM中添加线基元可以提高精度和鲁棒性，但也增加了计算复杂度。表二总结了PL-SLAM和ORB-SLAM [18]中“追踪”和“局部建图”模块中每个子任务所需的时间。需要注意的是，惩罚较大的子任务是地图特征创建和局部BA。无论如何，在标准PC（未优化）上，PL-SLAM的最终帧率接近实时（20 fps）。

说明

位姿求解

vins estimator 5点法

ORB EPNP

三角化

三角化求解3D空间点坐标

LSD 直线检测算法

• LSD（Line Segment Detector）是一种高效且无需人工调参的直线段检测算法，能在线性时间内 实现亚像素级精度的线段提取。以下从核心原理、实现方法、应用场景及优化方向展开说明：

📊 一、核心原理

1. 梯度与方向场计算 * 梯度计算 ：使用2×2模板计算像素点灰度变化，生成梯度幅值（G(x,y) = √(gx² + gy²)）和level-line角度（θ = arctan(-gy/gx)）。梯度值反映边缘强度，角度指示边缘方向。

   • 半像素偏移补偿 ：梯度实际位于(x+0.5, y+0.5)处，输出时需补偿0.5像素偏移。

2. 梯度伪排序与区域生长 * 伪排序 ：将梯度值分为1024个等级（bin），优先处理高梯度像素（如建筑物边缘），复杂度降至O(n)。

   • 区域生长 ：以种子点为中心，合并八邻域内角度差小于容忍阈值（默认τ=22.5°）的像素，形成线支撑域（Line Support Region） 。

3. 矩形拟合与验证 * 矩形生成 ：对每个线支撑域拟合矩形，其中心为梯度加权质心，方向由协方差矩阵最小特征值对应的特征向量确定。

   • NFA验证 ：通过误报数（Number of False Alarms, NFA） 控制误检率，仅保留满足NFA < ε（ε通常取1）的矩形。

4. 抗锯齿处理 * 默认将图像缩放至80%（高斯滤波σ=0.6），消除阶梯效应（Staircase Effect）。

⚙️ 二、实现方法

OpenCV调用示例

    import cv2

    # 读取图像并转灰度
    gray = cv2.cvtColor(cv2.imread("image.jpg"), cv2.COLOR_BGR2GRAY)
    # 创建LSD检测器并执行检测
    lsd = cv2.createLineSegmentDetector()  # OpenCV 3.x
    lines, _, _, _ = lsd.detect(gray)
    # 绘制线段
    result = np.copy(image)
    for line in lines:
    x1, y1, x2, y2 = line[0]
    cv2.line(result, (x1, y1), (x2, y2), (0, 0, 255), 2)
    cv2.imshow("LSD Result", result)
    
    
    python
    
    
![](https://ad.itadn.com/c/weblog/blog-img/images/2025-08-16/dH4GSFvxVlnsiTkCEeQrXLj6gAwp.png)

新版本OpenCV适配

OpenCV 4.5+：LSD移至 ximgproc 模块：

    #include <opencv2/ximgproc.hpp>

    cv::Ptr<cv::ximgproc::LSDDetector> lsd = cv::ximgproc::createLSDPtr();
    std::vector<cv::Vec4f> lines;
    lsd->detect(image, lines);
    
    

旧版本移植 ：若环境为OpenCV 4.x，可从OpenCV 3.1提取lsd.cpp和precomp.hpp，修改命名空间后集成到项目。

📐 三、应用场景

⚖️ 四、优缺点与优化

优缺点对比

性能优化方向

1. 预处理降噪 ：使用高斯滤波或非局部均值去噪减少误检。

2. 参数调整：

   • 增大scale（如0.5→0.8）抑制短线。
   • 调整sigma_scale控制梯度平滑强度。

3. 后处理融合：

   • 聚类断裂线段（基于方向与端点距离）。
   • 结合Hough变换补全长直线。

4. 硬件加速 ：通过OpenCV GPU模块或OpenMP并行化处理。

💎 总结

• LSD凭借其高效性 和自适应参数 成为工业视觉中线段检测的首选，尤其适合实时性要求高的场景（如机器人导航）。针对其噪声敏感和断裂问题，可通过预处理降噪+后处理聚类 优化。若需检测倾斜线段，建议结合Hough变换互补使用。

附：LSD处理流程简图
输入图像 → 高斯降采样 → 梯度场计算 → 伪排序 → 区域生长 → 矩形拟合 → NFA验证 → 输出线段

End