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

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

![在这里插入图片描述](https://i-blog.csdnimg.cn/direct/1cb5e2da44e94c4f8ae7817f1ac4d5a4.png

一、算法原理概述

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

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

二、主要公式推导

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

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

对应点质量（点 → 点）：

$w^{po \rightarrow po}_i = \frac{\sigma_{3D}}{\sigma_{3D_{\text{max}}}}$

其中：

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

点 → 线 / 面的误差项：

$w^{po \rightarrow li/pl}_i = 1 - \frac{1}{k \cdot d_{\text{max}}} \sum_{i=1}^{k} (p_i - \bar{p})^\top A (p_i - \bar{p})$

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

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

最终优化目标是：

$\min_{T_{i+1}} \left\{ \sum_{p \in F_i} \| W_l \cdot e_{po \rightarrow po, li, pl} \|^2 + \sum_{(i, i+1) \in \mathcal{B}} \| W_{imu} \cdot e_{imu} \|^2 + E_{prior}^{imu\_odom} \right\}$

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

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

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

$\text{Hash}: (x, y, z) \rightarrow \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）系统进行系统性对比分析，主要围绕以下五个维度：

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

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，自动剔除低质量约束