SLAM文献之-SuperOdometry: Lightweight LiDAR-inertial Odometry and Mapping

《Super Odometry: IMU-centric LiDAR-Visual-Inertial Estimator for Challenging Environments》是一篇旨在增强 SLAM 系统在恶劣环境下鲁棒性的工作，尤其关注尘雾、烟雾等遮挡条件下的鲁棒估计。下面从算法原理、公式推导、创新点和应用场景四个方面进行详细解析：

---

一、算法原理概述

Super Odometry 提出了一个以 IMU 为中心的多传感器融合框架，集成了 IMU、LiDAR 和相机信息，核心思想是：

• IMU 作为主导惯导轨迹提供器（IMU-centric），在结构退化或视觉遮挡时仍可提供高频、稳定的运动估计；
• LiDAR 和视觉信息作为约束项，增强轨迹精度；
• 通过 多度量 ICP（Multi-metric ICP） 与 动态八叉树（Dynamic Octree） 提高匹配质量与实时性；
• 利用 质量权重（Quality Weighting）机制 来判定不可靠匹配并降低其优化影响；
• 最终构造统一的优化框架，融合 IMU 先验、LiDAR ICP、视觉重投影误差进行非线性优化。

---

二、主要公式推导

1. 匹配质量权重（用于抑制退化点云匹配）

用于判断一个点是否真的属于点、线、面三种结构：

对应点质量（点 → 点）：

$$w_i^{po\to po}=\frac{{\sigma }_{3D}}{{\sigma }_{3D_{\text{max}}}}$$

其中：

• ${\sigma }_{3D}$：点周围的 3D 曲率
• ${\sigma }_{3D_{\text{max}}}$：最大曲率阈值

点 → 线 / 面的误差项：

$$w_i^{po\to li/pl}=1-\frac{1}{k\cdot d_{\text{max}}}\sum _{i=1}^k(p_i-\bar{p}{)}^{\top }A(p_i-\bar{p})$$

• $A=I-n_k{n}_k^{\top }$（点到线） 或 $A=n_k{n}_k^{\top }$（点到面）
• $n_k$：法向量
• $p_i$：邻近点，$\bar{p}$：邻近点均值
• $d_{\text{max}}$：最大误差范围

2. 全局优化目标函数（基于 LM 法）：

最终优化目标是：

$$\underset{T_{i+1}}{\min }\left\{\sum _{p\in F_i}\Vert W_l\cdot e_{po\to po,li,pl}{\Vert }^2+\sum _{(i,i+1)\in \mathcal{B}}\Vert W_{imu}\cdot e_{imu}{\Vert }^2+E_{prior}^{imu\_odom}\right\}$$

• 第一项：多度量 ICP 残差（带权）
• 第二项：IMU 预积分残差
• 第三项：IMU 初始先验

3. 动态八叉树构建（Dynamic Octree）

• 地图采用 Hash Table 存储体素 (voxel)：

$$\text{Hash}:(x,y,z)\to \text{Voxel}$$

• 每个 Voxel 内部包含一个小型 Octree（用于点云检索）
• 新点到来时仅更新对应 voxel 中的 Octree，避免重建整棵 KD-Tree，提高效率。

---

三、创新点总结

模块	创新点	作用
IMU 中心建模	将 IMU 作为核心轨迹估计器	即使视觉和 LiDAR 退化，仍能提供稳定估计
多度量 ICP	点-点、点-线、点-面联合建模	提高几何匹配鲁棒性
质量评估机制	判断特征是否可靠并加权	抑制雾霾等环境下错误匹配的影响
动态 Octree	每个 voxel 拥有独立 octree 结构	替代传统 KD-Tree，极大提升匹配效率
多模态优化	联合视觉重投影、LiDAR ICP、IMU 先验	构建稳健一致的因子图优化

---

四、适用场景与优势

适用场景：

• 尘雾、烟雾等遮挡严重的场景
• 结构退化（如隧道、矿井、管道）中点云特征不足的环境
• UAV、UGV、机器人等对实时鲁棒性要求高的导航任务

相较传统方法的优势：

方法	IMU 融合	点云匹配策略	实时性	雾霾鲁棒性
LOAM	弱融合	点-线、点-面	中等	差
LIO-SAM	中等	经典 ICP	中等	一般
Super Odometry	强融合（IMU-centric）	多度量 ICP + 动态 octree	高	强

---

五、Super Odometry与当前主流LIO/VLO系统对比

下面将《Super Odometry: IMU-centric LiDAR-Visual-Inertial Estimator for Challenging Environments》与当前主流的三种 LVI（LiDAR-Visual-Inertial）系统进行系统性对比分析，主要围绕以下五个维度：

1. 系统架构（IMU主导 / LiDAR主导 / Vision主导）
2. 数据关联策略（特征提取 + 匹配）
3. 地图组织与效率（KD-Tree / Octree / 动态结构）
4. 优化方式（因子图 / 滑窗 / 非线性优化）
5. 鲁棒性与适用环境

---

1. 总览对比表格

系统	主导传感器	数据关联	地图结构	优化策略	特殊能力 / 适用环境
VINS-Fusion	视觉 + IMU（视觉主导）	视觉特征匹配	无地图（轨迹为主）	滑窗 + 非线性 BA	室内/良好光照环境，不能单独使用 LiDAR
FAST-LIO	LiDAR + IMU（LiDAR 主导）	点 → 面（surf）	动态 KD-Tree	点云因子图优化	无需回环，全 LiDAR 特征
LVI-SAM	LiDAR + Vision + IMU（三者融合）	LiDAR 点面、视觉重投影	GTSAM + KD-Tree	联合因子图优化	回环 + 语义 + 多模态地图
Super Odometry	IMU 主导（IMU-centric）	点 → 点/线/面（多度量）+ 视觉	动态 Octree + 哈希	IMU 先验 + 多因子优化	雾霾/灰尘环境强鲁棒，点云退化适应强

---

2. 架构对比分析

系统	架构核心	优点	缺点
VINS-Fusion	视觉滑窗 + IMU 预积分	成熟，延迟小，开源广泛使用	对光照变化敏感，不能处理严重遮挡
FAST-LIO	LiDAR 点面残差 + IMU 优化	高精度，实时性强	在低结构环境中（隧道、烟雾）易退化
LVI-SAM	多模态因子图（视觉+激光+IMU）	通用性强，有回环	架构复杂，对同步和标定要求高
Super Odometry	IMU 为轨迹主导者	在 IMU 数据质量好时极稳定，适应退化	对 IMU 噪声与漂移更敏感，初始化要求较高

---

3. 数据关联方式对比

系统	LiDAR 匹配策略	视觉处理	特点
VINS-Fusion	无（只用相机）	ORB + 光流 + BA	不依赖点云，依赖光照和纹理
FAST-LIO	点到面残差（边缘/平面特征）	无	高效，适合室外结构场景
LVI-SAM	点到面 + 视觉重投影误差	ORB + 重投影 + 同步	对位姿初值依赖大
Super Odometry	点 → 点/线/面 多度量匹配 + 质量权重滤波	视觉重投影 + 前端边缘辅助	匹配鲁棒性强，可自适应退化特征剔除

---

4. 地图与结构组织

系统	地图结构	特点
VINS-Fusion	无地图，仅轨迹滑窗	轻量，但不能做密集建图
FAST-LIO	KD-Tree 动态维护	易受更新频率限制，插入慢
LVI-SAM	KD-Tree + 回环维护	精度高，实时性适中
Super Odometry	动态 Octree + Hash Voxel	插入更新仅局部 voxel，极高效率

---

5. 优化模型

系统	优化引擎	优化项	注释
VINS-Fusion	Ceres / GTSAM	视觉重投影 + IMU 预积分	类似视觉 SLAM，结构清晰
FAST-LIO	Ceres / LIO-Mapping	点云残差 + IMU	实时性强，因子图稀疏
LVI-SAM	GTSAM	点云 ICP + 相机 + 回环	多传感器融合
Super Odometry	自定义 LM / GTSAM	ICP 多度量 + IMU + 视觉重投影 + 权重筛选	优先信任 IMU，融合中引入权重机制提高鲁棒性

---

6. 在遮挡/雾霾等极端环境下表现

系统	在恶劣条件下表现	原因分析
VINS-Fusion	极易失效	视觉严重依赖光照、清晰纹理
FAST-LIO	退化严重	点云特征稀疏或被遮挡后误配严重
LVI-SAM	有一定缓解	依赖视觉与回环，遮挡下仍可能失效
Super Odometry	鲁棒性极强	IMU 主导 + 质量评估 + 多度量ICP，自动剔除低质量约束

---

六、 总结：何时选择 Super Odometry

推荐使用场景：

• 无法保证视觉质量的任务（如无人机穿越烟雾、采矿机器人）
• 高速运动下需要稳定惯导估计
• 点云特征退化严重但 IMU 保持稳定输入的系统
• 高鲁棒 SLAM 要求，如国防、灾害、地下隧道等

---