【Structure PLP-SLAM】点-线-面三基元融合SLAM：从Plücker坐标到Graph-Cut平面重建的完整技术解析

摘要

Structure PLP-SLAM 是 DFKI 团队提出的多基元视觉 SLAM 系统（ICRA 2023），在 ORB 点特征基础上融合 LSD 线段检测与 PlaneRecNet 实例分割，实现点(Point)、线(Line)、面(Plane)三基元联合优化。系统采用 Plücker 坐标表示 3D 线段，通过 Graph-Cut RANSAC 拟合平面，并在 BA 中隐式约束点-面距离，在 ICL-NUIM、TUM RGB-D 等基准数据集上取得了优于 ORB-SLAM2、PL-SLAM 等方法的轨迹精度。

图 1：Structure PLP-SLAM 系统整体架构。三种相机输入经前端特征提取与跟踪后，进入后端的 Local BA（点+线+面联合优化）、平面建图和全局优化模块，最终输出相机轨迹与点-线-面稀疏地图。重点关注平面建图与 Local BA 之间的双向约束反馈。重绘自 design skill

一、问题背景

1.1 纯点特征 SLAM 的局限

传统视觉 SLAM（如 ORB-SLAM2）仅依赖点特征，在以下场景中面临严峻挑战：

场景类型	具体问题
低纹理环境	ORB 特征点稀疏，匹配失败率高
光照变化	点特征描述子鲁棒性下降
人造结构场景	大量几何结构信息（线段、平面）被浪费
语义理解	稀疏点云缺乏场景结构表达能力

1.2 现有 Point-Line SLAM 的不足

PL-SLAM 等方法虽引入线段，但存在两个关键问题：

线段参数化过度：用两个 3D 端点（6-DOF）表示 3D 线段，实际自由度仅为 4，导致优化不稳定
EPnPL 两步法：强制端点对应的方式与 g2o 等图优化框架不兼容，无法在 BA 中统一优化

1.3 本文动机

Structure PLP-SLAM 的核心思路：

用 Plücker 坐标 （6D）表示 3D 线段，优化时转换为 正交表示（Orthonormal Representation, 4-DOF），彻底解决过参数化问题
引入 平面基元，通过 CNN 实例分割 + Graph-Cut RANSAC 重建分段平面（Piece-wise Planar Reconstruction, PPR）
在 BA 中通过 点-面距离隐式约束 将平面信息融入优化，无需显式平面残差项

二、核心方法

2.1 整体框架

系统基于 OpenVSLAM 构建，包含四个并行模块：
#mermaid-svg-kGOANaBupzLBrB5J{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-kGOANaBupzLBrB5J .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-kGOANaBupzLBrB5J .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-kGOANaBupzLBrB5J .error-icon{fill:#552222;}#mermaid-svg-kGOANaBupzLBrB5J .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-kGOANaBupzLBrB5J .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-kGOANaBupzLBrB5J .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-kGOANaBupzLBrB5J .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-kGOANaBupzLBrB5J .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-kGOANaBupzLBrB5J .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-kGOANaBupzLBrB5J .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-kGOANaBupzLBrB5J .marker{fill:#333333;stroke:#333333;}#mermaid-svg-kGOANaBupzLBrB5J .marker.cross{stroke:#333333;}#mermaid-svg-kGOANaBupzLBrB5J svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-kGOANaBupzLBrB5J p{margin:0;}#mermaid-svg-kGOANaBupzLBrB5J .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-kGOANaBupzLBrB5J .cluster-label text{fill:#333;}#mermaid-svg-kGOANaBupzLBrB5J .cluster-label span{color:#333;}#mermaid-svg-kGOANaBupzLBrB5J .cluster-label span p{background-color:transparent;}#mermaid-svg-kGOANaBupzLBrB5J .label text,#mermaid-svg-kGOANaBupzLBrB5J span{fill:#333;color:#333;}#mermaid-svg-kGOANaBupzLBrB5J .node rect,#mermaid-svg-kGOANaBupzLBrB5J .node circle,#mermaid-svg-kGOANaBupzLBrB5J .node ellipse,#mermaid-svg-kGOANaBupzLBrB5J .node polygon,#mermaid-svg-kGOANaBupzLBrB5J .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-kGOANaBupzLBrB5J .rough-node .label text,#mermaid-svg-kGOANaBupzLBrB5J .node .label text,#mermaid-svg-kGOANaBupzLBrB5J .image-shape .label,#mermaid-svg-kGOANaBupzLBrB5J .icon-shape .label{text-anchor:middle;}#mermaid-svg-kGOANaBupzLBrB5J .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-kGOANaBupzLBrB5J .rough-node .label,#mermaid-svg-kGOANaBupzLBrB5J .node .label,#mermaid-svg-kGOANaBupzLBrB5J .image-shape .label,#mermaid-svg-kGOANaBupzLBrB5J .icon-shape .label{text-align:center;}#mermaid-svg-kGOANaBupzLBrB5J .node.clickable{cursor:pointer;}#mermaid-svg-kGOANaBupzLBrB5J .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-kGOANaBupzLBrB5J .arrowheadPath{fill:#333333;}#mermaid-svg-kGOANaBupzLBrB5J .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-kGOANaBupzLBrB5J .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-kGOANaBupzLBrB5J .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-kGOANaBupzLBrB5J .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-kGOANaBupzLBrB5J .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-kGOANaBupzLBrB5J .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-kGOANaBupzLBrB5J .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-kGOANaBupzLBrB5J .cluster text{fill:#333;}#mermaid-svg-kGOANaBupzLBrB5J .cluster span{color:#333;}#mermaid-svg-kGOANaBupzLBrB5J div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-kGOANaBupzLBrB5J .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-kGOANaBupzLBrB5J rect.text{fill:none;stroke-width:0;}#mermaid-svg-kGOANaBupzLBrB5J .icon-shape,#mermaid-svg-kGOANaBupzLBrB5J .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-kGOANaBupzLBrB5J .icon-shape p,#mermaid-svg-kGOANaBupzLBrB5J .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-kGOANaBupzLBrB5J .icon-shape .label rect,#mermaid-svg-kGOANaBupzLBrB5J .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-kGOANaBupzLBrB5J .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-kGOANaBupzLBrB5J .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-kGOANaBupzLBrB5J :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 后端
前端
关键帧
每帧位姿
平面约束
回环校正
输入图像

Mono/Stereo/RGB-D
Tracking 模块
Mapping 模块
位姿输出
Planar Mapping 模块
Global Optimization 模块

对应代码模块（src/PLPSLAM/）：

模块	源文件	职责
Tracking	`tracking_module.cc/h`	每帧位姿估计（Motion-only BA）
Mapping	`mapping_module.cc/h`	关键帧管理 + Local BA
Planar Mapping	`planar_mapping_module.cc/h`	平面检测 + 重建 + 合并
Global Optimization	`global_optimization_module.cc/h`	回环检测 + Global BA

2.2 线段特征：从 LSD 到 Plücker 坐标

2D 线段提取与匹配

系统使用 LSD（Line Segment Detector） 提取 2D 线段，通过 LBD（Line Band Descriptor） 描述子进行跨帧匹配。LSD 参数经过调优，线段提取速度达到 OpenCV 原始实现的 3 倍。

Plücker 坐标表示

一条 3D 线段用 6D 向量的 Plücker 坐标 表示：

L=(m⊤,d⊤)⊤ \mathbf{L} = (\mathbf{m}^\top, \mathbf{d}^\top)^\top L=(m⊤,d⊤)⊤

其中 m∈R3\mathbf{m} \in \mathbb{R}^3m∈R3 是矩矢量（Moment Vector），垂直于线段所在的解释平面；d∈R3\mathbf{d} \in \mathbb{R}^3d∈R3 是方向矢量（Direction Vector），满足 Klein 二次约束 m⊤d=0\mathbf{m}^\top \mathbf{d} = 0m⊤d=0。

线段投影与重投影误差

3D 线段从世界坐标系到相机坐标系的变换：

Lc= $mcdc$ =TcwLw= $Rcw\[tcw$ ×Rcw0Rcw] $mwdw$ \mathbf{L}c = \begin{bmatrix} \mathbf{m}c \\ \mathbf{d}c \end{bmatrix} = \mathbf{T}{cw} \mathbf{L}w = \begin{bmatrix} \mathbf{R}{cw} & $\\mathbf{t}_{cw}$ \times \mathbf{R}{cw} \\ \mathbf{0} & \mathbf{R}_{cw} \end{bmatrix} \begin{bmatrix} \mathbf{m}_w \\ \mathbf{d}_w \end{bmatrix} Lc= $mcdc$ =TcwLw= $Rcw0\[tcw$ ×RcwRcw] $mwdw$

投影到图像平面得到 2D 直线 l= $l1,l2,l3$ ⊤=KLmc\mathbf{l} = $l_1, l_2, l_3$ ^\top = \mathbf{K}_L \mathbf{m}_cl= $l1,l2,l3$ ⊤=KLmc，其中 KL\mathbf{K}_LKL 是线段投影内参矩阵：

KL= $fy000fx0-fycx-fxcyfxfy$ \mathbf{K}_L = \begin{bmatrix} f_y & 0 & 0 \\ 0 & f_x & 0 \\ -f_y c_x & -f_x c_y & f_x f_y \end{bmatrix} KL= fy0−fycx0fx−fxcy00fxfy

线段重投影误差定义为 2D 线段端点 {xs,xe}\{\mathbf{x}_s, \mathbf{x}_e\}{xs,xe} 到投影直线 l\mathbf{l}l 的距离：

el= $xs⊤ll12+l22,xe⊤ll12+l22$ ⊤ \mathbf{e}_l = \left $\\frac{\\mathbf{x}_s\^\\top \\mathbf{l}}{\\sqrt{l_1\^2 + l_2\^2}}, \\frac{\\mathbf{x}_e\^\\top \\mathbf{l}}{\\sqrt{l_1\^2 + l_2\^2}} \\right$ ^\top el= $l12+l22 xs⊤l,l12+l22 xe⊤l$ ⊤

双视图三角化

通过两帧匹配的 2D 线段 l1\mathbf{l}_1l1、l2\mathbf{l}_2l2 反投影得到两个 3D 平面，取交线恢复 3D 线段：

π1=l1⊤P1,π2=l2⊤P2 \boldsymbol{\pi}_1 = \mathbf{l}_1^\top \mathbf{P}_1, \quad \boldsymbol{\pi}_2 = \mathbf{l}_2^\top \mathbf{P}_2 π1=l1⊤P1,π2=l2⊤P2

从对偶 Plücker 矩阵提取线段参数：

L∗=π1π2⊤−π2π1⊤= $\[d$ ×m−m⊤0] \mathbf{L}^* = \boldsymbol{\pi}_1 \boldsymbol{\pi}_2^\top - \boldsymbol{\pi}_2 \boldsymbol{\pi}1^\top = \begin{bmatrix} $\\mathbf{d}$ \times & \mathbf{m} \\ -\mathbf{m}^\top & 0 \end{bmatrix} L∗=π1π2⊤−π2π1⊤= $\[d$ ×−m⊤m0]

端点修剪（Endpoints Trimming）

三角化产生的是无穷长直线，需要通过参考关键帧的 2D 线段端点估计 3D 端点位置。同时在 Local BA 中利用端点修剪做异常值剔除 ：若 3D 端点的位移 ΔX\Delta XΔX 超过场景中位深度的 0.1 倍，判定为外点。

对应优化代码：

optimize/local_bundle_adjuster_extended_line.cc --- 线段 Local BA
optimize/pose_optimizer_extended_line.cc --- 含线段的位姿优化

2.3 平面特征：从 CNN 分割到 Graph-Cut 拟合

实例分割

系统使用 PlaneRecNet （BMVC 2021）在关键帧 上执行平面实例分割，生成逐像素的平面掩码。分割结果通过预计算的 mask 文件加载（存储于数据集的 mask/ 或 seg/ 目录）。

平面表示为无穷平面参数 π=(n⊤,d)⊤\boldsymbol{\pi} = (\mathbf{n}^\top, d)^\topπ=(n⊤,d)⊤，其中 n\mathbf{n}n 是平面法向量，ddd 是到世界原点的距离。

Graph-Cut RANSAC 平面拟合

将平面拟合建模为最优二值标注问题，能量函数：

E(Π)=∑v∥Πv∥+λ⋅∑(u,v)∈Nδ(Πu≠Πv) E(\boldsymbol{\Pi}) = \sum_v \|\boldsymbol{\Pi}v\| + \lambda \cdot \sum{(u,v) \in \mathcal{N}} \delta(\boldsymbol{\Pi}_u \neq \boldsymbol{\Pi}_v) E(Π)=v∑∥Πv∥+λ⋅(u,v)∈N∑δ(Πu=Πv)

其中第一项是一元能量 （数据项），衡量 3D 点到平面的距离；第二项是空间正则化 （平滑项），惩罚邻域图中标签不一致的节点对。λ=0.6\lambda = 0.6λ=0.6 平衡两项。

一元能量的 0-1 测度：

∥Πv∥{0,1}={0if (Πv=1∧dist(v,π)<ϵd)∨(Πv=0∧dist(v,π)≥ϵd)1otherwise \|\boldsymbol{\Pi}v\|{\{0,1\}} = \begin{cases} 0 & \text{if } (\boldsymbol{\Pi}_v = 1 \land \text{dist}(\mathbf{v}, \boldsymbol{\pi}) < \epsilon_d) \lor \\ & (\boldsymbol{\Pi}_v = 0 \land \text{dist}(\mathbf{v}, \boldsymbol{\pi}) \geq \epsilon_d) \\ 1 & \text{otherwise} \end{cases} ∥Πv∥{0,1}=⎩ ⎨ ⎧01if (Πv=1∧dist(v,π)<ϵd)∨(Πv=0∧dist(v,π)≥ϵd)otherwise

点-面距离度量：dist(v,π)=∣n⊤v+d∣∥n∥\text{dist}(\mathbf{v}, \boldsymbol{\pi}) = \frac{|\mathbf{n}^\top \mathbf{v} + d|}{\|\mathbf{n}\|}dist(v,π)=∥n∥∣n⊤v+d∣

邻域图 N\mathcal{N}N 使用 FLANN（Fast Approximate Nearest Neighbors） 构建，球半径 r=2ϵdr = 2\epsilon_dr=2ϵd，最少 3 个点即可拟合一个平面。

增量式合并与精化

在 Local Mapping 线程中，满足以下两个条件的平面自动合并：

法向量近似平行 ：∣cos⁡(θ)∣=∣ni⋅nj∣∥ni∥∥nj∥>Tθ|\cos(\theta)| = \frac{|\mathbf{n}_i \cdot \mathbf{n}_j|}{\|\mathbf{n}i\| \|\mathbf{n}j\|} > T\theta∣cos(θ)∣=∥ni∥∥nj∥∣ni⋅nj∣>Tθ（默认 Tθ=0.8T\theta = 0.8Tθ=0.8）
几何距离接近 ：∣di∥di∥−dj∥dj∥∣<Td\left| \frac{d_i}{\|d_i\|} - \frac{d_j}{\|d_j\|} \right| < T_d ∥di∥di−∥dj∥dj <Td

合并后重新执行 RANSAC 拟合，关联的 3D 点通过最小化点-面距离投影到平面上：v^=v−dist(v,π)n∥n∥\hat{\mathbf{v}} = \mathbf{v} - \text{dist}(\mathbf{v}, \boldsymbol{\pi}) \frac{\mathbf{n}}{\|\mathbf{n}\|}v^=v−dist(v,π)∥n∥n

自适应几何阈值

关键参数 ϵd\epsilon_dϵd（内点距离阈值）、TdT_dTd（合并距离阈值）、ϵΠ\epsilon_\PiϵΠ（模型残差阈值）根据参考关键帧观测到的局部地图场景深度动态调整，避免手工调参。

对应配置文件（planar_mapping_parameters.yaml）核心参数：

参数	默认值	含义
`use_graph_cut`	true	启用 Graph-Cut RANSAC
`min_number_points_before_ransac`	12	RANSAC 最少点数
`plane_distance_correction`	0.02	点-面距离校正阈值
`inliers_ratio_thr`	0.7	最小内点比例
`dot_product_threshold`	0.8	法向量平行判定（合并）
`spatial_coherence_weight`	0.6	Graph-Cut 空间一致性权重
`inlier_outlier_threshold`	0.02	内点/外点分界阈值
`transparency_alpha`	0.7	可视化透明度

对应代码：

planar_mapping_module.cc/h --- 平面检测与重建主模块
optimize/local_bundle_adjuster_extended_plane.cc --- 含平面约束的 Local BA

2.4 联合优化：点-线-面 BA

图 2：点-线-面 BA 优化流程。上层三条流水线分别处理点、线、面特征并定义各自的误差项；中层统一代价函数融合三类误差；下层展示三级 BA 的触发时机与代码位置。重点关注平面约束的"隐式"设计------不添加显式残差边。重绘自 design skill

BA 的总体代价函数融合点和线的重投影误差：

C=∑i,jρh(eij⊤Ωijeij)+∑i,zρh(elz⊤Ωlzelz) C = \sum_{i,j} \rho_h(\mathbf{e}{ij}^\top \boldsymbol{\Omega}{ij} \mathbf{e}{ij}) + \sum{i,z} \rho_h(\mathbf{e}{lz}^\top \boldsymbol{\Omega}{lz} \mathbf{e}_{lz}) C=i,j∑ρh(eij⊤Ωijeij)+i,z∑ρh(elz⊤Ωlzelz)

其中 ρh\rho_hρh 是 Huber 鲁棒核函数，Ωij\boldsymbol{\Omega}{ij}Ωij 和 Ωlz\boldsymbol{\Omega}{lz}Ωlz 分别是点和线的协方差矩阵（与图像金字塔层级关联）。

关键设计 ：平面约束以隐式方式融入------不添加显式平面残差项，而是通过以下约束隐式优化：

∑j,kdist(Xj,πk) \sum_{j,k} \text{dist}(\mathbf{X}_j, \boldsymbol{\pi}_k) j,k∑dist(Xj,πk)

即最小化所有关联 3D 点 Xj\mathbf{X}_jXj 到所属平面 πk\boldsymbol{\pi}_kπk 的距离。平面位置由 RANSAC + SVD 统计最优确定，优化时保持固定，通过将 3D 点投影到平面上实现约束。

这种设计的优势：避免将显式平面残差引入位姿图导致大规模非线性优化问题（某些边如地面平面可能主导优化、破坏局部优化策略）。

优化代码对应关系：

优化器	源文件	优化内容
Pose Optimizer	`pose_optimizer.cc`	仅优化当前帧位姿（点）
Pose Optimizer + Line	`pose_optimizer_extended_line.cc`	位姿优化（点+线）
Local BA	`local_bundle_adjuster.cc`	局部窗口 BA（点）
Local BA + Line	`local_bundle_adjuster_extended_line.cc`	局部窗口 BA（点+线）
Local BA + Plane	`local_bundle_adjuster_extended_plane.cc`	局部窗口 BA（点+线+面）
Global BA	`global_bundle_adjuster.cc`	全局 BA
Graph Optimizer	`graph_optimizer.cc`	位姿图优化（回环）

2.5 回环检测与重定位

回环检测

使用 DBoW2 词袋模型进行位置识别，估计 7-DOF 相似变换 Sim(3)\text{Sim}(3)Sim(3)：

Sim(3)={Spoint= $sRt01$ ∈R4×4} \text{Sim}(3) = \left\{ \mathbf{S}_{point} = \begin{bmatrix} s\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \right\} Sim(3)={Spoint= $sR0t1$ ∈R4×4}

3D 线段的相似变换同样使用相同的尺度因子 sss、旋转 R\mathbf{R}R 和平移 t\mathbf{t}t：

Sline= $sR\[t$ ×R0R]∈R6×6 \mathbf{S}{line} = \begin{bmatrix} s\mathbf{R} & $\\mathbf{t}$ \times \mathbf{R} \\ \mathbf{0} & \mathbf{R} \end{bmatrix} \in \mathbb{R}^{6 \times 6} Sline= $sR0\[t$ ×RR]∈R6×6

回环校正后，利用端点修剪重新估计 3D 线段端点，使线段地图更加紧凑、准确。

重定位

基于预建点-线地图，使用 BoW 图像检索 + EPnP 初始化 + 点-线联合 Motion-only BA 精化位姿，比纯点方法获得更小的绝对位姿误差（APE）。

三、实验分析

3.1 线段特征的有效性（ICL-NUIM 单目）

ICL-NUIM 提供低对比度、低纹理的合成室内序列，对单目 SLAM 极具挑战。绝对轨迹误差 ATE RMSE（cm）：

序列	仅点	点+线	点+线+面	Structure-SLAM	Object-Plane
living_rm_0	0.28	0.32	0.39	---	10.00
living_rm_1	2.21	1.88	2.67	4.50	2.06
living_rm_2	1.95	1.81	2.54	4.60	5.58
office_0	5.14	4.50	4.26	---	5.93
office_2	4.07	1.55	5.31	3.10	2.63
office_3	3.67	2.88	3.65	6.50	---
平均	2.63	2.59	7.04	4.50	5.59

线段在低纹理场景中显著提升匹配鲁棒性，点+线 配置平均误差最低（2.59cm）。

3.2 平面约束的有效性（TUM RGB-D 单目）

TUM RGB-D 提供真实室内序列，ATE RMSE（cm）：

序列	仅点	点+线	点+线+面	PL-SLAM	Elaborate
fr1_xyz	1.06	1.16	1.09	1.21	1.02
fr1_floor	2.03	2.24	1.88	7.59	3.49
fr1_desk	2.02	1.89	1.92	---	---
fr2_xyz	0.26	0.26	0.25	0.43	0.31
fr2_desk	0.94	0.77	1.14	---	4.65
fr3_st_tex_far	0.99	1.11	1.05	0.89	0.87
fr3_nst_tex_near	1.14	1.44	1.15	1.25	1.40
fr3_nst_tex_far	1.26	1.41	1.48	2.06	1.40
fr3_lon_office	3.24	1.37	3.51	---	3.68
fr3_long_office_hh	1.16	1.04	1.54	1.97	2.98
平均	1.49	1.41	1.41	4.20	2.45

在纯平面场景（如 fr1_floor、fr3_st_tex_near）中，点+线+面配置比纯点提升显著。隐式平面约束在深度传感器可用时（RGB-D）效果更明显。

3.3 RGB-D SLAM（ICL-NUIM）

ATE RMSE（cm）：

序列	点+线+面	点+线	仅点	Manhattan SLAM
lr_kt_0	0.54	0.91	1.05	1.38
lr_kt_1	1.46	1.56	1.50	2.00
office_0	1.07	1.16	1.10	1.20
office_2	2.38	2.20	1.36	2.00
平均	0.84	0.84	---	---

3.4 失败案例分析

单目初始化失败 ：ICL-NUIM living_room_1 序列需调整初始化阈值
剧烈运动丢失 ：office_room_1 相机对准无纹理角落时跟踪丢失
CNN 分割失败：EuRoC MH 系列（工厂环境）PlaneRecNet 无法有效分割平面，此时系统降级为点+线模式

四、代码结构总览

复制代码

Structure-PLP-SLAM/
├── src/PLPSLAM/
│   ├── system.cc/h                    # 系统入口：初始化各模块、调度线程
│   ├── tracking_module.cc/h           # 前端跟踪：track_monocular/stereo/RGBD_image()
│   ├── mapping_module.cc/h            # 后端建图：关键帧管理 + Local BA
│   ├── planar_mapping_module.cc/h     # 平面重建：Graph-Cut RANSAC + 合并
│   ├── global_optimization_module.cc/h # 回环 + Global BA
│   ├── optimize/
│   │   ├── pose_optimizer.cc/h                       # 纯点位姿优化
│   │   ├── pose_optimizer_extended_line.cc/h          # 点+线位姿优化
│   │   ├── local_bundle_adjuster.cc/h                # 纯点 Local BA
│   │   ├── local_bundle_adjuster_extended_line.cc/h   # 点+线 Local BA
│   │   ├── local_bundle_adjuster_extended_plane.cc/h  # 点+线+面 Local BA
│   │   ├── global_bundle_adjuster.cc/h               # Global BA
│   │   ├── graph_optimizer.cc/h                      # 位姿图优化
│   │   └── g2o/                                      # g2o 自定义顶点/边
│   ├── feature/          # ORB/LSD/LBD 特征提取
│   ├── match/            # 特征匹配（点+线）
│   ├── initialize/       # 单目初始化
│   ├── solve/            # PnP/三角化求解器
│   ├── data/             # 关键帧/地图点/地图线/平面 数据结构
│   └── planar_mapping_parameters.yaml  # 平面模块配置
├── example/              # TUM/KITTI/EuRoC 运行配置
├── 3rd/                  # DBoW2, g2o, Graph-Cut RANSAC
└── orb_vocab/            # ORB 词袋文件

#mermaid-svg-bdPT3diRsNZJQ6BW{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-bdPT3diRsNZJQ6BW .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-bdPT3diRsNZJQ6BW .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-bdPT3diRsNZJQ6BW .error-icon{fill:#552222;}#mermaid-svg-bdPT3diRsNZJQ6BW .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-bdPT3diRsNZJQ6BW .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-bdPT3diRsNZJQ6BW .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-bdPT3diRsNZJQ6BW .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-bdPT3diRsNZJQ6BW .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-bdPT3diRsNZJQ6BW .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-bdPT3diRsNZJQ6BW .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-bdPT3diRsNZJQ6BW .marker{fill:#333333;stroke:#333333;}#mermaid-svg-bdPT3diRsNZJQ6BW .marker.cross{stroke:#333333;}#mermaid-svg-bdPT3diRsNZJQ6BW svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-bdPT3diRsNZJQ6BW p{margin:0;}#mermaid-svg-bdPT3diRsNZJQ6BW .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-bdPT3diRsNZJQ6BW .cluster-label text{fill:#333;}#mermaid-svg-bdPT3diRsNZJQ6BW .cluster-label span{color:#333;}#mermaid-svg-bdPT3diRsNZJQ6BW .cluster-label span p{background-color:transparent;}#mermaid-svg-bdPT3diRsNZJQ6BW .label text,#mermaid-svg-bdPT3diRsNZJQ6BW span{fill:#333;color:#333;}#mermaid-svg-bdPT3diRsNZJQ6BW .node rect,#mermaid-svg-bdPT3diRsNZJQ6BW .node circle,#mermaid-svg-bdPT3diRsNZJQ6BW .node ellipse,#mermaid-svg-bdPT3diRsNZJQ6BW .node polygon,#mermaid-svg-bdPT3diRsNZJQ6BW .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-bdPT3diRsNZJQ6BW .rough-node .label text,#mermaid-svg-bdPT3diRsNZJQ6BW .node .label text,#mermaid-svg-bdPT3diRsNZJQ6BW .image-shape .label,#mermaid-svg-bdPT3diRsNZJQ6BW .icon-shape .label{text-anchor:middle;}#mermaid-svg-bdPT3diRsNZJQ6BW .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-bdPT3diRsNZJQ6BW .rough-node .label,#mermaid-svg-bdPT3diRsNZJQ6BW .node .label,#mermaid-svg-bdPT3diRsNZJQ6BW .image-shape .label,#mermaid-svg-bdPT3diRsNZJQ6BW .icon-shape .label{text-align:center;}#mermaid-svg-bdPT3diRsNZJQ6BW .node.clickable{cursor:pointer;}#mermaid-svg-bdPT3diRsNZJQ6BW .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-bdPT3diRsNZJQ6BW .arrowheadPath{fill:#333333;}#mermaid-svg-bdPT3diRsNZJQ6BW .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-bdPT3diRsNZJQ6BW .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-bdPT3diRsNZJQ6BW .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-bdPT3diRsNZJQ6BW .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-bdPT3diRsNZJQ6BW .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-bdPT3diRsNZJQ6BW .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-bdPT3diRsNZJQ6BW .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-bdPT3diRsNZJQ6BW .cluster text{fill:#333;}#mermaid-svg-bdPT3diRsNZJQ6BW .cluster span{color:#333;}#mermaid-svg-bdPT3diRsNZJQ6BW div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-bdPT3diRsNZJQ6BW .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-bdPT3diRsNZJQ6BW rect.text{fill:none;stroke-width:0;}#mermaid-svg-bdPT3diRsNZJQ6BW .icon-shape,#mermaid-svg-bdPT3diRsNZJQ6BW .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-bdPT3diRsNZJQ6BW .icon-shape p,#mermaid-svg-bdPT3diRsNZJQ6BW .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-bdPT3diRsNZJQ6BW .icon-shape .label rect,#mermaid-svg-bdPT3diRsNZJQ6BW .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-bdPT3diRsNZJQ6BW .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-bdPT3diRsNZJQ6BW .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-bdPT3diRsNZJQ6BW :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} Sim3
system.cc
tracking_module
mapping_module
planar_mapping_module
global_optimization_module
pose_optimizer_extended_line
local_bundle_adjuster_extended_plane
Graph-Cut RANSAC
graph_optimizer + global_bundle_adjuster

小结

三个核心创新：

Plücker + 正交表示：用 6D Plücker 坐标统一线段表示与投影，优化时转为 4-DOF 正交参数，彻底解决线段过参数化问题，与 g2o 框架无缝集成
Graph-Cut RANSAC 平面拟合：将平面检测建模为带空间正则化的能量最小化问题，比传统 RANSAC 更鲁棒地处理错误分割和噪声点云
隐式点-面距离约束：不在位姿图中显式添加平面残差边，而是通过将 3D 点投影到关联平面上隐式施加约束，避免大规模非线性优化问题

局限性：

平面重建强依赖 3D 点云质量，低纹理区域点少时平面拟合可能失败
PlaneRecNet 在非室内场景（如工厂、户外）泛化能力有限
自适应几何阈值策略仍需针对特定场景微调

个人判断 ：Structure PLP-SLAM 的 隐式平面约束 思路值得借鉴------相比直接在 BA 中加平面残差项，这种"投影即约束"的方式更轻量、更稳定。对于我们在 Polaris/GRSlam 中集成平面特征，可以参考其 Graph-Cut RANSAC 平面拟合流程和自适应阈值策略，但 BA 中的平面融合方式可考虑更紧耦合的显式残差设计。