0. 简介

这篇SLAM论文《Direct LiDAR Odometry: Fast Localization with Dense Point Clouds》作为NASA喷气推进实验室CoSTAR团队研究和开发的新作，收到了学术界广泛的关注。其主要用作DARPA地下挑战的里程计，提出了一种能够实现高速高精度处理高速实时处理密集点云的激光里程计(LO)的思路，下面是他们的Github开源代码。

1. 文章贡献

文中提出了一种轻量前端激光雷达里程计解决方案，用于在计算能力受限的机器人平台上，具有快速和精确的定位能力，我们的直接激光雷达里程计（DLO）方法包括几个关键的算法上的创新，这些创新优先考虑计算效率，并使用稠密的、预处理最少的点云实时提供准确的姿势估计。以下是文章的主要创新点：

提出了一个定制的速度优先的处理流程，这个流程可以实时精确的估计位姿，对预处理的需求比较小，并且IMU是可选项。
提出了一个新的关键帧系统，可以根据环境信号自适应的选择关键帧，并且可以通过凸优化快速的生成子图。
从实验中发展出几个重要的算法见解，以进一步降低计算开销，从而产生了NanoGICP(定制的迭代最接近点解算器，用于轻量级点云扫描与交叉对象数据共享匹配)。

下图展示了该激光雷达里程计应用的两个计算资源有限的机器人平台（A）定制的四旋翼平台，顶部有一个驱逐OS1激光雷达传感器。（B） Boston Dynamics Spot机器人，装有负载和带防护装置的Velodyne VLP-16。（C）使用我们的轻型里程计方法在这些机器人上绘制的石矿的俯视图

2. 整体思路

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

2.1 预处理

在预处理过程中只使用了两个滤波器：首先，通过原点周围大小为1立方米的盒子滤波器删除机器人自身可能返回的所有点云。然后，生成的点云通过分辨率为0.25m的三维体素网格滤波器发送，以便在保持周围环境中的主要结构的同时，略微减少后续任务的数据采样。在这项工作中，我们不校正运动失真，因为非刚性变换可能会带来计算负担，我们直接使用稠密点云，而不是提取特征，平均而言，每帧点云包含了预处理后得到1000点。

2.2 基于GICP(Generalized-ICP)的位姿计算

这部分分成了两步，第一步是提供瞬时的位姿估计(帧间位姿), 第二步是提供优化的位姿(帧图优化)。这部分也就是下文最后提到的NanoGICP高效匹配。为了促进点云扫描与交叉对象数据共享匹配，文中提出开发了NanoGICP，这是一个自定义迭代最近点解算器，本质上是将开源的FastGICP和NanoFLANN两个包进行合并，使用NanoGICP的NanoFLANN来高效构建KD-tree, 然后通过FastGICP进行高效匹配。

A. 帧间匹配

基于激光雷达的里程计可被视为通过比较连续点云和内存中的点云来恢复SE（3）变换来解析机器人自我运动的过程。此过程通常分两个阶段运行，首先是提供最佳初始值，随后将其优化为与先前关键帧位置保持其全局一致。

目标函数为下式，其中 X ^ k L \hat{\mathbf{X}}_{k}^{\mathcal{L}} X^kL代表了在 s s s时刻和 t t t时刻的位姿相对变换， P k s \mathcal{P}_{k}^{\mathrm{s}} Pks代表了source的扫描的点云， P k t \mathcal{P}_{k}^{\mathrm{t}} Pkt代表了target的扫描的点云( P k t = P k − 1 s \mathcal{P}_{k}^{\mathrm{t}}=\mathcal{P}_{k-1}^{\mathrm{s}} Pkt=Pk−1s这代表了 k k k时刻的target点云与 k − 1 k-1 k−1时刻的source点云)。

X ^ k L = arg ⁡ min ⁡ x k L E ( X k L P k s , P k t ) \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) X^kL=xkLargminE(XkLPks,Pkt)

其中 E \mathcal{E} E为误差函数，这里被定义为:

E ( X k L P k s , P k t ) = ∑ i N d i T ( C k , i t + X k C C k , i s X k L ) − 1 d i d i = p i t − X k L p i s , p i s ∈ P k s , p i t ∈ P k t \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}} E(XkLPks,Pkt)=i∑NdiT(Ck,it+XkCCk,isXkL)−1didi=pit−XkLpis,pis∈Pks,pit∈Pkt

其中 d i = p i t − X k L p i s ， p i t ∈ P k t , p i s ∈ P k s d_{i}=p_i^t-\mathbf{X}_{k}^{\mathcal{L}}p_i^s，p_i^t\in\mathcal{P}_{k}^{\mathrm{t}}, p_i^s\in\mathcal{P}_{k}^{\mathrm{s}} di=pit−XkLpis，pit∈Pkt,pis∈Pks表示了点云中的两个点的距离差。 C k , i s , C k , i t \mathcal{C}_{k, i}^{\mathrm{s}}, \mathcal{C}_{k, i}^{\mathrm{t}} Ck,is,Ck,it指的是两个点云中每个点 i i i对应的估计协方差矩阵。

B. 帧图匹配

…详情请参照古月居

经典文献阅读之--DLO相关推荐

货运服务网络设计：经典文献阅读笔记（3）复现Netplan
**货运服务网络设计:经典文献阅读笔记(2)**提到说要把Crainic T G(1984年)文献使用的模型复现一下,但是文章给出的通用框架还是太笼统,在尝试后决定使用Jacques Roy & ...
经典文献阅读之--Swin Transformer
0. 简介 Transfomer最近几年已经霸榜了各个领域,之前我们在<经典文献阅读之–Deformable DETR>这篇博客中对DETR这个系列进行了梳理,但是想着既然写了图像处理领域 ...
经典文献阅读之--NICE-SLAM（SLAM的神经隐含可扩展编码）
0. 简介对于深度学习而言,NeRF一定是最近两年最火的工作之一了,**NeRF(Neural Radiance Fields)**是最早在2020年ECCV会议上的Best Paper,其将隐式表 ...
经典文献阅读之--PL-SLAM(点线SLAM)
0. 简介之前作者基本都在围绕着特征点提取的路径在学习,最近看到了最近点云PCL推送的<Structure PLP-SLAM: Efficient Sparse Mapping and Loc ...
经典文献阅读之--PON
0. 简介作为Transformer在机器视觉领域的爆火,在自动驾驶领域目前很多工作都集中在前视转鸟瞰图的方法中,这里我们来讲2020年一篇经典的论文<Predicting Semantic ...
经典文献阅读之--SuMa++
0. 简介作为一名技术博主,最主要的还是需要不断地学习新的知识,而最好的学习就是不断地阅读新的文章,并不断地学习和总结前人的思路和方法.所以博主打算开一个新的系列来介绍.这里主要来介绍一下<S ...
经典文献阅读之--OV2SLAM(高速视觉slam)
0. 简介视觉里程计最近几年越来越受到学术界以及工业界的认可,以ORB和VINS为代表的视觉SLAM已经可以满足绝大多数场景,而OV2SLAM在其他VSLAM中脱颖而出,其实时性以及具体的回环性能在 ...
货运服务网络设计：经典文献阅读笔记（1）
服务网络设计 --(1984,Crainic,Multicommodity, Multimode Freight Transportation: A General Modeling and Algo ...
经典文献阅读之--Deformable DETR
0. 简介已经好久不写深度学习相关的博客了.但是我觉得DETR值得我重新拾起来进行详细介绍.Deformable DETR作为这两年来最有名的DETR变种之一,当然代码量也是比较多的.这里我们从论文 ...

经典文献阅读之--DLO