《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?d_{\text{max}}}\sum _{i=1}^k(p_i?\bar{p}{)}^{?}A(p_i?\bar{p})$$

• $A=I?n_k{n}_k^{?}$（點到線） 或 $A=n_k{n}_k^{?}$（點到面）
• $n_k$：法向量
• $p_i$：鄰近點，$\bar{p}$：鄰近點均值
• $d_{\text{max}}$：最大誤差范圍

2. 全局優化目標函數（基于 LM 法）：

最終優化目標是：

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

• 第一項：多度量 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 要求，如國防、災害、地下隧道等

---