相关文件&视频： IROS 2019 slides Youtube
背景
LIC-Fusion: LiDAR-Inertial-Camera Odometry是2019年发表的文章。作者在相关工作中将Zhang和Singh提出的Laser-visual-inertial Odometry and Mapping with High Robustness and Low Drift作为对比，并指出该论文中无论是迭代优化还是ICP配准都需要复杂的流水线和并行处理以实现实时运行。同时，该论文中的方法仅将VIO系统的位姿同激光雷达扫描进行松耦合，损失了激光雷达和VIO系统的相关信息。
贡献
本文提出了一种快速、紧耦合、单线程的激光雷达-IMU-相机里程计。该里程计基于MSCKF开发（感觉事实上就是OpenVINS），并实现了在线的传感器时偏和外参标定。具体贡献如下：
- 开发了紧耦合的激光雷达-IMU-相机里程计（LIC-Fusion），在实现高效的6自由度位姿估计的同时对传感器时差和外参进行在线标定。提出的LIC-Fusion在MSCKF框架下有效地结合了IMU测量、稀疏视觉特征以及两种不同的激光雷达特征（线，面）。并清晰地建模和推导l待标定的外参、位姿估计值和各观测值之间的关系；
- 在真实世界场景（室内、室外）下对提出的方法进行了大量的实验验证，证明提出的LIC-Fusion比SOTA方法更精确和鲁棒；
LIC-Fusion
状态变量
$$ \bold x = \begin{bmatrix} \bold x_I^T & \bold x_{calib_C}^T & \bold x_{calib_L}^T & \bold x_C^T & \bold x_L^T \end{bmatrix}^T $$
分别为：
- IMU状态（PVQ）
- 相机标定参数（外参PQ,时差）
- 激光雷达标定参数（外参PQ,时差）
- 滑窗中的相机位姿
- 滑窗中的激光雷达位姿
IMU传播
状态增广
测量模型
激光雷达
该文中使用的激光雷达是机械式的，所以在文中作者通过计算激光雷达扫描线上点的曲率以提取特征点（线/面），并在帧间利用KD-tree做索引对特征进行跟踪。
对于当前帧的每个线特征点$^{L_{l+1}}\bold p_{fi}$，将其投影到上一帧得到：$^{L_{l}}\bold p_{fi}$。在上一帧中搜索两个最近邻的线特征点$^{L_{l}}\bold p_{fj},^{L_{l}}\bold p_{fk}$。若$^{L_{l}}\bold p_{fj}$位于激光雷达的第$r$条扫描线上，则$^{L_{l}}\bold p_{fk}$应当位于第$r$或$r+1$条扫描线上。
根据三个线特征点$^{L_{l}}\bold p_{fi},^{L_{l}}\bold p_{fj},^{L_{l}}\bold p_{fk}$可以计算残差： $$ r(^{L_{l+1}}\bold p_{fi}) = \cfrac{ \lVert (^{L_{l}}\bold p_{fi} - ^{L_{l}}\bold p_{fj}) \times (^{L_{l}}\bold p_{fi} - ^{L_{l}}\bold p_{fk})\rVert_2 }{ \lVert ^{L_{l}}\bold p_{fj} - ^{L_{l}}\bold p_{fk} \rVert_2 } $$
随后根据雅可比矩阵$\bold H_\bold x$和协方差矩阵$\bold P_\bold x$进行卡方检验剔除外点。
面特征通过最近邻的三点构建，方法同线特征类似。
在执行MSCKF更新前对雅可比矩阵进行零空间投影，以消除特征点3D坐标（观测方程与此无关）。
相机
此部分应当同OpenVINS中的推导一致。
观测压缩
在线性化LiDAR特征和视觉特征观测后，由于特征点数量较多，导致EKF更新复杂度大。因此对雅可比矩阵$\bold H_\bold x$进行QR分解，移除无用维度。
实验
高动态室内实验
