LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping论文阅 读笔记。
简介
这是一种基于平滑和建图的紧耦合激光雷达惯性里程计框架，对于点云去畸变假设一个非线性运动模型，使用原始imu测量数据估计一帧激光雷达扫描过程中传感器的运动。除了用于对点云去畸变，该估计的运动也作为激光雷达里程计优化的初始值。优化完的激光雷达里程计结果接着用于在因子图中估计imu的偏置。通过在机器人轨迹估计中引入一个全局因子图，能够使用激光雷达和imu测量数据高效地进行传感器融合，包括机器人位姿间的位置识别，同时当GPS位置或指南针朝向数据可用时，也能够很容易地融入绝对测量数据。这些从不同数据源采集的因子被用于因子图的联合优化。除此之外，在位姿优化中边缘(舍弃)旧的激光雷达扫描，以替代类似于LOAM中将扫描与全局地图的匹配。局部尺度扫描匹配代替全局尺度扫描匹配显著提高了系统的实时性能，关键帧选择性插入以及高效滑动窗口方法同样如此，滑动窗口方法将一个新的关键帧和一个固定尺寸的先验“子关键帧”集合进行配准。
主要贡献和框架
- 一种建立在因子图上的紧耦合激光雷达里程计框架，适用于多传感器融合和全局优化。
- 基于局部滑动窗口的高效帧匹配方法，通过将有选择的新关键帧配准到一组固定大小的先验的子关键帧 集合上，从而实现实时性能。
- 通过对各种规模、车辆和环境的测试，对提出的框架进行了广泛的验证。
方法
系统概述
定义世界坐标为
如上图所示，这个系统接受LiDar、IMU、GPS的测量信号来估计位姿和轨迹。状态估计问题一般是一个最大后验估计，用因子图来建模比贝叶斯网络更适合于这类问题的估计。假设高斯噪声模型，最大后验估计就等同于求解非线性最小二乘问题。当然这个系统还可以加入其它的传感器测量（如高度计、罗盘）等。
本文有四个因子：1、IMU预积分，2、lidar里程计，3、GPS因子，4、回环检测因子。当机器人的位姿变化超过阈值，则新的状态因子就被加入图优化中，在插入新节点后，会使用贝叶斯树进行增量优化（主要使用iSAM2的优化思想和集成iSAM2实现的gtsam库）。
IMU预积分因子
imu的测量角速度和加速度可以被如下定义
计算机器人在
还有就是IMU的偏置在雷达里程计因子中优化，对应文章开头的“imu预计分的运动用于点云去畸变并且给激光里程计提供初始估计值，同时获取的激光里程计结果用于估计imu的偏置”这句话。
Lidar里程计因子
当一个新的雷达帧到来的时候，首先提取特征。通过计算点在局部区域的曲率来提取边缘和平面特征，大曲率被归为边缘特征，小曲率被归为平面特征。
然后是构建由子关键帧组成的局部地图（局部地图点云同样是由边缘点和平面点两种点云组成），子关键帧的数量作为一个超参数。并且对局部地图做了降采样处理。对于新到的关键帧
GPS因子
GPS测量数据用时间戳和雷达帧线性差值对齐。因为激光惯性里程计漂移增长相对较为缓慢，因此GPS因子加入时机是判断当估计的位置方差比接收到的GPS位置方差大时，才将其加入因子图优化。
回环检测因子
回环检测因子也是做帧间匹配。当一个新的状态
