SLAM文献之SuMa++: Efficient LiDAR-based Semantic SLAM
SuMa++是基于Surfel的SLAM算法SuMa的改进版本，通过引入语义分割信息提升动态环境下的鲁棒性和回环检测性能。以下从算法原理和公式推导两方面详细阐述：
一、SuMa++算法原理
1. 基础：SuMa算法
SuMa使用Surfel（表面元素）构建地图，每个Surfel包含位置、法向量、半径和时间戳。关键步骤包括：
- 帧间匹配：利用ICP（迭代最近点）算法，通过最小化当前帧点云与地图Surfel之间的点到平面距离，求解位姿变换。
- 地图管理：根据新数据更新Surfel，淘汰过时元素。
- 回环检测：基于几何一致性的场景识别和位姿图优化。
2. SuMa++的改进
- 语义分割融合：使用RangeNet++对点云进行语义分割，区分静态与动态物体（如车辆、行人）。
- 动态点剔除：在ICP优化中，为动态点分配低权重或直接剔除，减少其对位姿估计的影响。
- 语义增强的回环检测：结合语义标签与几何特征，生成更具区分度的场景描述子。
二、公式推导
1. 语义加权的ICP优化
目标函数：引入语义权重的最小化点到平面距离。
min
T
∑
i
w
i
[
(
T
⋅
p
i
−
q
i
)
⋅
n
i
]
2
\min_{T} \sum_{i} w_i \left[ (\mathbf{T} \cdot \mathbf{p}_i - \mathbf{q}_i) \cdot \mathbf{n}_i \right]^2
Tmini∑wi[(T⋅pi−qi)⋅ni]2
- ( T ∈ S E ( 3 ) ) ( \mathbf{T} \in SE(3) ) (T∈SE(3))：待求的位姿变换矩阵。
- ( p i ) ( \mathbf{p}_i ) (pi)：当前帧点云中的点。
- ( q i , n i ) ( \mathbf{q}_i, \mathbf{n}_i ) (qi,ni)：地图中对应Surfel的位置和法向量。
- ( w i ) ( w_i ) (wi)：语义权重（静态物体 ( w i = 1 ) ( w_i=1 ) (wi=1)，动态物体 ( w i = 0 ) ( w_i=0 ) (wi=0)）。
雅可比矩阵与优化：
- 残差项： ( r i = w i ( T ⋅ p i − q i ) ⋅ n i ) ( r_i = w_i (\mathbf{T} \cdot \mathbf{p}_i - \mathbf{q}_i) \cdot \mathbf{n}_i ) (ri=wi(T⋅pi−qi)⋅ni)
- 李代数参数化： ( T = exp ( ξ ∧ ) ) ( \mathbf{T} = \exp(\boldsymbol{\xi}^\wedge) ) (T=exp(ξ∧))，其中 ( ξ ∈ s e ( 3 ) ) ( \boldsymbol{\xi} \in \mathfrak{se}(3) ) (ξ∈se(3))
- 雅可比计算：对
(
ξ
)
( \boldsymbol{\xi} )
(ξ)求导，利用链式法则展开：
J i = ∂ r i ∂ ξ = w i n i T [ − I ( T ⋅ p i ) ∧ ] \mathbf{J}_i = \frac{\partial r_i}{\partial \boldsymbol{\xi}} = w_i \mathbf{n}_i^T \left[ -\mathbf{I} \quad (\mathbf{T} \cdot \mathbf{p}_i)^\wedge \right] Ji=∂ξ∂ri=winiT[−I(T⋅pi)∧] - 高斯-牛顿法求解增量
(
Δ
ξ
)
( \Delta \boldsymbol{\xi} )
(Δξ)：
( ∑ i J i T J i ) Δ ξ = − ∑ i J i T r i \left( \sum_i \mathbf{J}_i^T \mathbf{J}_i \right) \Delta \boldsymbol{\xi} = -\sum_i \mathbf{J}_i^T r_i (i∑JiTJi)Δξ=−i∑JiTri
2. 语义分割与权重分配
- RangeNet++输出：对点云投影生成的深度图像进行逐像素分类，获得语义标签。
- 权重策略：
- 静态类别（道路、建筑）： ( w i = 1 ) ( w_i = 1 ) (wi=1)
- 动态类别（车辆、行人）： ( w i = 0 ) ( w_i = 0 ) (wi=0)
- 半静态类别（植被）： ( w i ∈ ( 0 , 1 ) ) ( w_i \in (0,1) ) (wi∈(0,1))（依场景调整）
3. 语义增强的回环检测
- 描述子生成：结合几何特征（如FPFH）与语义统计（如各类别占比）。
- 相似性度量：对两个区域( A, B )，计算联合相似度：
S ( A , B ) = λ S geo ( A , B ) + ( 1 − λ ) S sem ( A , B ) S(A,B) = \lambda S_{\text{geo}}(A,B) + (1-\lambda) S_{\text{sem}}(A,B) S(A,B)=λSgeo(A,B)+(1−λ)Ssem(A,B)- ( S geo ) ( S_{\text{geo}} ) (Sgeo)：几何特征余弦相似度。
- ( S sem ) ( S_{\text{sem}} ) (Ssem)：语义分布的卡方检验相似度。
- ( λ ) ( \lambda ) (λ)：平衡权重（通常取0.5）。
4. 地图更新策略
- Surfel语义融合：新观测与地图中Surfel的语义标签通过贝叶斯更新：
P ( c ∣ z 1 : t ) ∝ P ( z t ∣ c ) P ( c ∣ z 1 : t − 1 ) P(c|z_{1:t}) \propto P(z_t|c) P(c|z_{1:t-1}) P(c∣z1:t)∝P(zt∣c)P(c∣z1:t−1)- ( c )：语义类别，( z_t )：当前观测。
三、关键改进分析
- 动态环境鲁棒性：通过语义加权ICP，显著降低动态物体对位姿估计的干扰。
- 回环检测增强：语义信息提升场景辨识力，减少外观变化（如昼夜、季节）导致的误匹配。
- 计算效率：RangeNet++的极坐标投影与轻量解码器设计，确保实时性。
四、小结
SuMa++通过语义分割与SLAM的深度融合，解决了动态场景下的定位与建图难题。其核心在于将语义信息嵌入到ICP优化与回环检测中，通过数学建模提升系统鲁棒性。