论文题目：

Unified Multi-Modal Landmark Tracking for Tightly Coupled Lidar-Visual-Inertial Odometry

紧耦合激光视觉惯性里程计的统一多模态地标点跟踪

参考链接：

【泡泡点云时空】紧耦合激光视觉惯性里程计的统一多模态地标点跟踪

摘要：

我们为移动平台提出了一种高效的多传感器里程计系统，该系统在单个集成因子图中联合优化视觉、激光雷达和惯性信息。 这使用固定滞后平滑以全帧速率实时运行。为了执行这种紧密集成，提出了一种从激光雷达点云中提取 3D 线和平面图元的新方法。这种方法克服了典型的帧到帧跟踪方法的次优性 将图元视为地标并在多次扫描中跟踪它们。 使用激光雷达和相机帧的微妙被动同步，可以实现激光雷达特征与标准视觉特征和 IMU 的真正集成。  3D 功能的轻量级公式允许在单个 CPU 上实时执行。 我们提出的系统已经在各种平台和场景上进行了测试，包括使用腿式机器人进行地下探索和使用动态移动的手持设备进行户外扫描，总持续时间为 96 分钟，行驶距离为 2.4 公里。 在这些测试序列中，仅使用一个外部感受器会导致失败，原因是几何形状受限（影响激光雷达）或由剧烈的光照变化（影响视觉）引起的无纹理区域。 在这些情况下，我们的因子图自然会使用来自每个传感器模式的最佳信息，而无需任何硬开关。

主要贡献：

• 一种新颖的因子图公式，在单个一致的优化过程中紧密融合视觉、激光雷达和 IMU 测量值；

• 一种提取激光雷达特征的有效方法，然后将其优化为地标。 激光雷达和视觉特征共享统一的表示，因为地标都被视为 n 维参数流形（即点、线和平面）。 这种紧凑的表示允许我们以标称帧速率处理所有激光雷达扫描；

• 与更典型的方法相比，在单个传感器模式失败时挣扎的更典型的方法相比，在一系列场景中进行了广泛的实验评估，展示了卓越的鲁棒性。

我们的工作建立在 VILENS 估计系统的基础上 通过添加激光雷达特征跟踪和激光雷达辅助视觉跟踪，在我们之前的作品 [11]、[12] 中引入。 相机和激光雷达的组合即使在剧烈移动时也能在便携式设备上使用，因为它自然地处理场景中的退化（由于缺乏激光雷达或视觉特征）。

主要图：

 

雷达惯导，雷达视觉惯导，和loam，lego-loam对比：

总结：

1 在一个固定步长的平滑窗口内，视觉提取特征点，激光雷达提取线特征和面特征点集。

2 跟踪视觉特征，使用激光雷达提供深度（投影，查找3个像素之内的对应点，计算残差）如上图3。

3 使用海塞正规式来参数化平面p，使用一个单位向量n以及一个d表示其原点的距离。

4 使用一个旋转矩阵R和两个尺度参数a,b来参数化一个直线，R表示直线方向，ab表示直线上距离原点最近的点

5 使用FAST角点检测器检测特征，并使用KLT特征跟踪器跟踪它们，使用RANSAC滤除外点

6 使用imu做先验，对雷达点云去畸变，并转换到最近的相机帧时间，实现同步，如上图6。

7 进行聚类，滤波，计算曲率取角点和平面点（同loam）。

8 跟踪前一帧中的线和面，具体是：

使用imu预测到当前帧中对应的位置，为了帮助局部跟踪，我们使用最大的点到模型的距离对预测的特征位置进行CP和CL分割。然后，我们执行欧几里得聚类（和平面特征的正态过滤），以去除外点。然后，我们使用PROSAC鲁棒拟合算法将模型拟合到分割后的点云上。最后，我们检查了预测的地标和检测到的地标是否足够相似。当两个平面p_i和p_j的法线与距离原点的距离小于阈值时，被认为是匹配的

9 一旦跟踪了一个特征，特征的内点将从相应的候选集中删除，并对剩余的地标重复这个过程。在跟踪完成后，我们将在剩余的候选云中检测到新的地标。首先使用欧氏聚类对点云进行划分，平面采用基于法向的区域增长算法。然后，我们使用与地标跟踪相同的方法来检测每个集群中的新地标。点云特征只有在跟踪了至少多次连续帧后才包含在优化中

10 当平台静止时，为了限制漂移和因子图的增长，当三种模态(相机、激光雷达、IMU)中的两种更新报告没有运动时，我们向图中添加了零速度约束