FAST-LIVO2 深度技术解析
一、核心技术突破
1. 序列化更新的 ESIKF（Sequential Update ESIKF）
这是 FAST-LIVO2 最重要的理论创新：
问题本质：
- 激光雷达：单帧数千至数万点
- 视觉图像：百万级像素
- 直接融合会导致维度爆炸，协方差矩阵求逆计算量巨大
解决方案：
传统方法：p(x | y_lidar, y_visual) → 联合更新
FAST-LIVO2：p(x | y_lidar, y_visual) = p(y_visual | x) · p(y_lidar | x) · p(x)分两步执行：
- 第一阶段（LiDAR 更新）：先用激光点云更新状态，锁定几何约束
- 第二阶段（视觉更新）：再用视觉数据微调，处理几何退化区域
数学优势：
- 将一个大矩阵求逆分解为两个小矩阵求逆
- 激光的高精度几何约束先"稳定"状态空间
- 避免视觉野值破坏几何估计
2. 扫描重组（Scan Recombination）
这是一个巧妙的工程解决方案：
同步问题：
- 激光雷达：连续扫描
- 相机：瞬间曝光
- 如何在数学上视为同步测量？
重组策略：
激光点云流 ───┬──> 时刻 t_img 之前的点 → Scan 1
│
└──> 时刻 t_img 之后的点 → Scan 2（下一帧）强制将点云在相机时间戳处"切断"，使 LIO 和 VIO 在时间上对齐，简化了滤波器设计。
3. 视觉地图点的生成机制
传统 VIO 痛点：
- 需要三角化恢复深度
- 单目存在尺度漂移
- 深度滤波器收敛慢
FAST-LIVO2 方案：
激光点云投影到图像 → 选择高纹理区域 → 提取 8×8 像素块 → 附着到激光点上优势：
- 视觉点天生具有精确深度（来自激光测距）
- 消除深度不确定性
- "即插即用"的视觉约束
二、状态空间设计
流形上的状态定义
x = [^G R_I, ^G p_I, ^G v_I, b_g, b_a, ^G g, τ]^T
├─ SO(3): 旋转矩阵（3维）
├─ R³: 位置（3维）
├─ R³: 速度（3维）
├─ R³: 陀螺仪零偏（3维）
├─ R³: 加速度计零偏（3维）
├─ R³: 重力向量（3维）
└─ R: 时间偏移/曝光参数（1维）
总维度：19关键设计：
- 旋转在 SO(3) 流形上，避免奇异性
- 在线估计重力向量，适应初始对齐误差
- 估计光度参数（τ），增强光照适应性
三、观测模型
1. 激光点面残差
r_l = u_j^T (^G R_I · ^I T_L · ^L p_j + ^G p_I - q_j)
└─ 法向量 └──────── 点变换到全局 ────┘ └ 平面中心利用 ikd-Tree 高效查询最近邻平面。
2. 视觉光度残差
r_c = I_curr(π(p_map)) - I_ref(p_ref)
└─ 当前帧投影 └─ 参考图块关键创新：基于激光法向量的仿射变换
- 传统假设：物理表面平行于相机（深度恒定）
- FAST-LIVO2：利用激光点云拟合的局部平面法向量
- 效果：大视角变化时仍能准确投影
四、系统架构模块
1. 动态参考图块更新
监控视差角 → 超过阈值 → 从当前帧提取新图块 → 替换旧图块延长视觉特征跟踪寿命，适应长距离运行。
2. 混合地图结构
激光部分：ikd-Tree（增量式 k-d 树）
├─ 高效 KNN 查询
└─ 动态增删改
视觉部分：Voxel Hash Map
├─ 快速视锥体剔除
└─ 遮挡查询五、关键配置参数
从你的实际应用角度，这些参数需要特别注意：
| 参数 | 推荐值 | 调试建议 |
|---|---|---|
point_filter_num |
高速：3-4 高精：1 | 你的 Fast-Drone 建议从 2 开始 |
filter_size_surf |
室外：0.5 室内：0.2 | 无人机室内建议 0.3 |
outlier_threshold |
暗：50-250 亮：500-1000 | 最关键，需要根据环境实测 |
外参 extrinsic_T |
毫米级精确 | 用 FAST-Calib 工具精确标定 |
六、硬件同步方案
STM32 触发信号
├─ 信号 A → 相机硬触发引脚（控制曝光）
└─ 信号 B → Livox PPS/Sync（时间戳）代码检查：
cpp
if (abs(lidar_timestamp - image_timestamp) > 10ms) {
warning("时间同步失败！");
}七、性能对比
Hilti 数据集 RMSE（米）
| 场景 | FAST-LIVO2 | FAST-LIO2 | R3LIVE | 优势 |
|---|---|---|---|---|
| 楼梯（几何退化） | 0.016 | 0.320 | 0.784 | 20倍提升 |
| 长走廊（视觉退化） | 0.067 | 0.064 | 0.061 | 保持 LIO 精度 |
| 综合场景 | 0.007 | 0.024 | 0.008 | 7mm 精度 |
计算效率
- 桌面 i7：35ms/帧（28Hz）
- ARM RK3588：50-67ms/帧（15-20Hz）
- 对比 R3LIVE：ARM 上仅个位数帧率
八、对你的项目启示
1. Fast-Drone-250 集成建议
当前方案：PX4 + VINS-Fusion
可升级为：PX4 + FAST-LIVO2
优势：
├─ 解决楼梯、走廊等退化场景
├─ 提供彩色点云建图能力
└─ ARM 平台实时性更好2. 关键改进点
针对你之前的 IMU 频率问题：
- FAST-LIVO2 的 IMU 预积分和连续时间模型更稳健
- 序列化更新框架对高频 IMU 数据处理更高效
针对 VINS 不稳定：
- 直接法比特征法在弱纹理环境更鲁棒
- 激光提供的深度先验消除尺度漂移
3. 标定工作
使用配套的 FAST-Calib 工具：
相机-IMU 外参
相机-激光雷达 外参
时间偏移标定精度要求：平移误差 < 1mm，旋转误差 < 0.1°
九、实现要点
核心代码流程
cpp
// src/laserMapping.cpp 主循环
while(ros::ok()) {
sync_packages(); // 扫描重组
esikf_predict(); // IMU 预测
// 序列化更新
esikf_update_lidar(); // 第一阶段：LiDAR
esikf_update_visual(); // 第二阶段：视觉
map_incremental(); // 地图更新
publish_results();
}IKFoM 库的使用
cpp
// 流形上的状态更新
state = state ⊞ (Δt * f(state, imu, noise));
// └─ 盒加运算，保持在流形上这个库封装了复杂的流形运算，让状态扩展变得简单。
十、总结
FAST-LIVO2 的核心价值：
- 理论创新：序列化更新 ESIKF 解决维度失配
- 工程优化：扫描重组实现时间硬同步
- 精度突破：激光辅助的直接视觉法，7mm 级精度
- 效率领先：ARM 平台实时运行
- 应用价值：实时彩色点云，支持 3DGS 等下游任务