作为NASA喷气推进实验室CoSTAR团队研究和开发DARPA地下挑战的里程计部分

《Direct LiDAR Odometry: Fast Localization with Dense Point Clouds》( arXiv:2110.00605 )

Motivation

就目前而言，以LOAM为依据的激光里程计算法都很难满足能够高速实时处理密集点云的要求。所以在这个工作里提出了能够实现高速高精度LO的思路。

Contribution

1. 提出了一个定制的速度优先的处理流程，这个流程可以实时精确的估计位姿，对预处理的需求比较小，并且IMU是可选项。

2. 提出了一个新的关键帧系统，可以根据环境信号自适应的选择关键帧，并且可以通过凸优化快速的生成子图。

3. 从实验中发展出几个重要的算法见解，以进一步降低计算开销，从而产生了NanoGICP(定制的迭代最接近点解算器，用于轻量级点云扫描与交叉对象数据共享匹配)。

4. 大量的实验。

Content

1. 系统框架

   首先通过RANSAC排除离群点和用IMU来获得旋转先验，然后获得帧到帧的相对位姿。这个相对位姿之后传播到世界坐标系，并且用于二级GICP的初始化值。

1. 预处理

   比较简单，应用了两个滤波器，一是滤除了自身周围1立方米的点，二是通过单位为0.25m的体素格降采样，不需要像loam那样提取特征点。

2. 基于GICP的位姿计算

   分成两步，第一步是提供瞬时的位姿估计(帧间位姿), 第二步是提供优化的位姿(帧图优化).

   A. 帧间匹配

   目标函数:

\[\hat{\mathbf{X}}_{k}^{\mathcal{L}}=\underset{\mathbf{x}_{k}^{\mathcal{L}}}{\arg \min } \mathcal{E}\left(\mathbf{X}_{k}^{\mathcal{L}} \mathcal{P}_{k}^{\mathrm{s}}, \mathcal{P}_{k}^{\mathrm{t}}\right)\]

误差函数:

\[\mathcal{E}\left(\mathbf{X}_{k}^{\mathcal{L}} \mathcal{P}_{k}^{\mathrm{s}}, \mathcal{P}_{k}^{\mathrm{t}}\right)=\sum_{i}^{N} d_{i}^{\mathrm{T}}\left(\mathcal{C}_{k, i}^{\mathrm{t}}+\mathbf{X}_{k}^{\mathcal{C}} \mathcal{C}_{k, i}^{\mathrm{s}} \mathbf{X}_{k}^{\mathcal{L}}\right)^{-1} d_{i}\\ d_{i}=p_{i}^{t}-\mathbf{X}_{k}^{L} p_{i}^{\mathrm{s}}, p_{i}^{\mathrm{s}} \in \mathcal{P}_{k}^{\mathrm{s}}, p_{i}^{\mathrm{t}} \in \mathcal{P}_{k}^{\mathrm{t}}\]

B. 帧图匹配

基本和帧间匹配一样，不过是把当前帧和局部地图匹配:

\[\hat{\mathbf{X}}_{k}^{\mathcal{W}}=\underset{\mathbf{x}_{k}^{w}}{\arg \min } \mathcal{E}\left(\mathbf{X}_{k}^{\mathcal{W}} \mathcal{P}_{k}^{\mathrm{s}}, \mathcal{S}_{k}\right)\] \[\hat{\mathbf{X}}_{k}^{\mathcal{W}}=\underset{\mathbf{x}_{k}^{w}}{\arg \min } \sum_{j}^{M} d_{j}^{\mathrm{T}}\left(\mathcal{C}_{k, j}^{\mathcal{S}}+\mathbf{X}_{k}^{\mathcal{W}} \mathcal{C}_{k, j}^{\mathrm{s}} \mathbf{X}_{k}^{\mathcal{W}^{\mathrm{T}}}\right)^{-1} d_{j}\]

1. 优化先验

   通过IMU实现, 将IMU测量的角速度定义为(后面两项分别是静态误差和零白噪声):

\[\hat{\boldsymbol{\omega}}_{k}=\boldsymbol{\omega}_{k}+\mathbf{b}_{k}^{\omega}+\mathbf{n}_{k}^{\omega}\]

在误差矫正后通过四元数运动学公式可以获得帧间的旋转数据:

\[\mathbf{q}_{k+1}=\mathbf{q}_{k}+\left(\frac{1}{2} \mathbf{q}_{k} \otimes \boldsymbol{\omega}_{k}\right) \Delta t\]

1. 基于关键帧的快速子图构建

并不是直接将点云村初到kdtree中，而是创建一个关键帧的副本点云进行搜索，其中每个关键帧都与关键帧字典中的相应点云关联。因此，用于帧图匹配的局部子图，可以通过字典中的点云串联生成，而不用通过地理位置的搜索。

这样做的好处有两方面: 一是关键帧空间比点云空间更加节约计算资源; 二是基于关键帧的子图构建的子图比基于距离的子图构建的子图更加宽松，这个意味着基于距离的子图构建重复位置比较多，而基于关键帧的子图具有较少的重复。

1. 定制的Nano-GICP

   本质上是将开源的FastGICP和NanoFLANN两个包进行合并，使用NanoGICP的NanoFLANN来高效构建KD-tree, 然后通过FastGICP进行高效匹配.

2. 实验结果

Conclusion

这篇文章因为是预刊印版本，所以很多细节说的不是很清楚，但是从实验效果来看，是非常好的，但是他的第二个创新点并没有在实验里证明，感觉有点问题，总体上来讲，这个论文用GICP代替了loam的特征提取过程，取得了速度上的提升，至于精度上的提升，个人认为可能跟调参更有关系。