FAST-LIVO2 快速总结

FAST-LIVO2 作为一套优秀的开源SLAM框架，在许多实践中表现出不俗的性能。本文仅作阶段性自用总结。

1. 模型输入与输出

在这套框架中，模型的输入主要由三个部分组成：

1. imu 数据（加速度，角加速度，时间戳）
2. lidar 数据（点云，时间戳）
3. 相机数据（图像，时间戳）

而模型的输出主要包括两项内容：

1. 轨迹（即带时间戳的位姿轨迹）
2. 体素点云地图（以位置为哈希索引，记录该位置的点云和图像，同时带有协方差信息）

2. 数据处理流程

该系统为典型的生产者-消费者系统，输入信号会进入buffer池，随着计算进程根据时间戳先后被依次处理。

2.1 IMU 数据处理

IMU数据包含加速度估计和角加速度估计，显然可以通过积分的方式估计出位置和姿态信息，从而对当前的系统状态（位置和位姿）进行外推估计（即提供了IESKF中的状态预测方程）。

注意：位姿在机器人系统中常常用单位四元数表示，所以直接对四元数进行卡曼滤波估计会产生奇异性问题，而用李群流形上的误差状态表示则可以避免这个问题，这也是使用IESKF的主要原因。

2.2 Lidar 数据处理

Lidar 数据会在接收后利用IMU信息进行去畸变计算，同时数据量比较大，会进行点云降采样处理，后续主要包含两个部分处理：

1. 点云与体素地图的匹配误差计算（筛选出与现有地图中的平面能够产生匹配的点，计算匹配误差，提供了IESKF中的观测方程，此处FAST-LIVO2设计了不错的点云及地图的误差模型以及误差匹配计算，值得一读）
2. 体素地图生成（将所有降采样点云根据位置储存到对应的体素地图中，对地图进行更新）

2.3 相机数据处理

由于LIO效果已经很出色，暂未详细分析，大致思路是将基于位姿估计将现有地图投影到图像上，计算灰度匹配误差，提供了IESKF中的观测方程，此处暂不展开。

3. IESKF 更新注意事项

SLAM系统中最棘手的问题是退化问题，而速度和角速度常常是耦合在一起的，会产生较大的相互干扰，因此在分析退化时，需要对增益矩阵进行适当拆分，再进一步分析。