FAST-LIO笔记
1.FAST-LIO
FAST-LIO 是一个计算效率高、鲁棒性强的激光-惯性里程计系统。该系统通过紧耦合的迭代扩展卡尔曼滤波器（IEKF）将激光雷达特征点与IMU数据进行融合，使其在快速运动、噪声较大或环境复杂、存在退化的情况下仍能实现稳定的导航。
1.1问题
1）LiDAR测量中提取的特征点通常是环境中的几何结构（如边缘和平面）。当无人机在特征不明显的复杂环境中运行时，LiDAR方案容易出现退化问题。该问题在视场角（FoV）较小的LiDAR中尤为明显； （特征稀缺的环境中或使用小FoV的LiDAR时，性能容易退化）
2）由于在扫描方向上具有高分辨率，一帧LiDAR扫描通常包含大量特征点（例如几千个）。尽管这些特征点在退化场景中无法稳定估计位姿，但若将其与IMU测量进行紧耦合融合，将导致巨大的计算负担，而这对于无人机的机载计算机来说是不可承受的；（嵌入式平台对算力有要求）
3）由于LiDAR使用少数激光发射/接收对逐点扫描，因此每个激光点的采样时间不同，这种时间差造成的运动畸变会显著降低点云配准的准确性。此外，无人机螺旋桨和电机的持续旋转还会对IMU测量引入显著噪声。（点云去畸变以及外部因素影响IMU测量）
1.2解决方法
为使LiDAR导航适用于小型移动机器人（如无人机），提出了FAST-LIO——一个计算高效且鲁棒的LiDAR-IMU里程计算法。主要贡献如下：
1）为应对高速运动、有噪声或复杂环境中发生退化的情况，我们采用紧耦合的迭代卡尔曼滤波器，将LiDAR特征点与IMU测量数据融合。我们提出了正式的后向传播方法来补偿运动畸变；
2）为减少大量LiDAR特征点带来的计算负担，我们提出了一种新的卡尔曼增益计算公式，并证明其与传统公式等价。新公式的计算复杂度取决于状态维度，而不是测量维度；
3）我们将这些公式实现为一个快速且鲁棒的LiDAR-IMU里程计算软件包，系统可以运行在小型四旋翼的机载计算机上；
4）我们在多种室内和室外环境下，以及实际无人机飞行测试中，验证了系统在快速运动或强烈振动噪声条件下的鲁棒性。
1.3系统框架
- LiDAR原始数据首先输入到特征提取模块，提取出平面特征和边缘特征。
- 提取出的特征与IMU测量数据一同输入到状态估计模块中，以10Hz或50Hz的频率进行状态估计。IMU数据前向传播（粗略地位姿估计），后向传播（运动补偿去畸变）
- 估计得到的位姿用于将当前帧的特征点配准到全局坐标系中，并与当前已构建的特征点地图进行融合。更新后的地图将用于下一步中新点云的配准操作。
1.4系统描述
关于流行的解释
流形(Manifold )是局部具有欧几里得空间性质的空间，是欧几里得空间中的曲线、曲面等概念的推广。参考文章中关于二维圆和三维球的例子解释的很好，二维空间中的圆就是一个一维流形，三维空间中的球面是一个二维流形。
参考：https://www.cnblogs.com/icmzn/p/11082509.html
IMU运动模型
本质上是对P、V、Q的求导
离散模型
需要估计的状态量是个18维的量，包括位置、速度、姿态、偏置和加速度g .
数据预处理
由于原始LiDAR点是以非常高的频率（例如200kHz）采样的，因此通常无法在每接收到一个新点时立即处理它们。更实际的方法是将这些点积累一段时间，然后一次性处理它们。
在FAST-LIO中，最小的积累间隔设置为20毫秒，从而实现高达50Hz的全状态估计（即里程计输出）和地图更新。这种积累后的点集被称为一个扫描（scan），并且处理该扫描的时间为 tk。从原始点云中，我们提取具有较高局部平滑性的平面点和具有较低局部平滑性的边缘点。
1.5状态估计
使用迭代扩展卡尔曼滤波估计状态量X
前向传播 Forward Propagation
在 FAST-LIO 系统中，一旦接收到 IMU 输入，就会执行一次前向传播，用于预测系统当前的状态（积分得到粗略地位姿估计）。
后向传播与运动补偿 Backward Propagation and Motion Compensation
激光雷达一帧数据由很多点组成，这些点显然不是同一时间测量得到的，所以需要补偿时间差带来的运动误差。把一帧不同采样时间的点转换到同一时刻（反向传播去运动畸变）
残差计算和迭代状态更新
计算lidar里程计的残差，和LOAM、LIO-SAM一样，计算点线、点面之间的距离作为残差。
利用迭代卡尔曼滤波估计位姿。
算法流程
1.6地图更新 MapUpdate
根据求解的状态量 X 把激光雷达点投影到世界坐标系的过程.
1.7初始化
为了获得系统状态（例如重力向量 Gg、偏差和噪声协方差）的良好初始估计，从而加速状态估计，需要进行初始化。在 FAST-LIO 中，初始化非常简单：保持 LiDAR 静止几秒钟（本文中的所有实验为 2 秒），然后将收集的数据用于初始化 IMU 偏差和重力向量。如果 LiDAR 支持非重复扫描（例如，Livox AVIA），保持静止还允许 LiDAR 捕捉一个初始的高分辨率地图，这对后续的导航非常有利。
2.代码相关
注：FAST-LIO 的代码是6e1fa94之前的commit的，之后的都是FAST-LIO2
有大佬对原始的FAST-LIO进行修改，S-FAST_LIO
- 利用Sophus库定义状态变量，替代了FAST-LIO中复杂的IKFOM
- 重力分量直接利用Vector3d定义，因此可以省去2维流形的计算
- 优化了代码结构，删除了冗余的代码
- 代码中添加了详细的中文注释
- 添加了对于 Robosense LiDAR 的支持
而且新增了重定位功能
roslaunch sfast_lio mapping_rs_relocalization.launch
rosbag play YOUR_DOWNLOADED.bag