Classical Literature Reading--DLO

0. 简介

这篇SLAM论文《Direct LiDAR Odometry: Fast Localization with Dense Point Clouds》作为NASA喷气推进实验室CoSTARNew work researched and developed by the team,received extensive attention from academia.It is mainly usedDARPAThe odometer of the underground challenge,A laser odometry capable of high-speed, high-precision processing and high-speed real-time processing of dense point clouds is proposed(LO)的思路,下面是他们的Github开源代码.

1. 文章贡献

This paper proposes a lightweight front-end lidar odometry solution,用于在计算能力受限的机器人平台上,具有快速和精确的定位能力,我们的直接激光雷达里程计（DLO）方法包括几个关键的算法上的创新,这些创新优先考虑计算效率,并使用稠密的、预处理最少的点云实时提供准确的姿势估计.The following are the main innovations of the article：

1. A customized speed-first processing flow is proposed,This process can accurately estimate the pose in real time,The need for preprocessing is relatively small,并且IMU是可选项.
2. A new keyframe system is proposed,Keyframes can be adaptively selected according to environmental signals,And can quickly generate subgraphs through convex optimization.
3. Several important algorithmic insights developed from the experiments,to further reduce computational overhead,从而产生了NanoGICP(Custom iterative closest point solver,For lightweight point cloud scanning and cross-object data sharing matching).

The figure below shows two robotic platforms with limited computing resources for this lidar odometry application（A） 定制的四旋翼平台,顶部有一个驱逐OS1激光雷达传感器.（B） Boston Dynamics Spot机器人,装有负载和带防护装置的Velodyne VLP-16.（C） 使用我们的轻型里程计方法在这些机器人上绘制的石矿的俯视图

2. 整体思路

The whole system is throughRANSACExclude outliers and useIMUto obtain the rotation prior,The frame-to-frame relative pose is then obtained.This relative pose is then propagated to the world coordinate system,and for secondaryGICP的初始化值.

2.1 预处理

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

2.2 基于GICP(Generalized-ICP)pose calculation

This part is divided into two steps,The first step is to provide an instantaneous pose estimate(frame-to-frame pose), The second step is to provide the optimized pose(Frame picture optimization).This part is also mentioned at the end of the followingNanoGICP高效匹配.To facilitate point cloud scans and cross-object data sharing matching,Developed in the textNanoGICP,This is a custom iterative closest point solver,Essentially it will be open sourceFastGICP和NanoFLANNThe two packages are merged,使用NanoGICP的NanoFLANNto build efficientlyKD-tree, 然后通过FastGICPfor efficient matching.

A. 帧间匹配

Lidar-based odometers can be considered pass-throughCompare the continuous point cloud with the in-memory point cloud for recoverySE（3）变换To analyze the process of robot self-motion.此过程通常分两个阶段运行,首先是提供最佳初始值,随后将其优化为与先前关键帧位置保持其全局一致.

The objective function is the following formula,其中${\hat{\mathbf{X}}}_k^{\mathcal{L}}$代表了在$s$时刻和$t$The relative transformation of the pose at the moment,${\mathcal{P}}_k^{\mathrm{s}}$代表了sourceof scanned point clouds,${\mathcal{P}}_k^{\mathrm{t}}$代表了targetof scanned point clouds(${\mathcal{P}}_k^{\mathrm{t}}={\mathcal{P}}_{k-1}^{\mathrm{s}}$这代表了$k$时刻的target点云与$k-1$时刻的source点云).

$${\hat{\mathbf{X}}}_k^{\mathcal{L}}=\underset{{\mathbf{x}}_k^{\mathcal{L}}}{\mathrm{argmin}}\mathcal{E}\left({\mathbf{X}}_k^{\mathcal{L}}{\mathcal{P}}_k^{\mathrm{s}},{\mathcal{P}}_k^{\mathrm{t}}\right)$$

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

$$\begin{gathered} \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}} \end{gathered}$$

其中$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}}$Represents the distance difference between two points in the point cloud.${\mathcal{C}}_{k,i}^{\mathrm{s}},{\mathcal{C}}_{k,i}^{\mathrm{t}}$Refers to each point in both point clouds$i$The corresponding estimated covariance matrix.

B. 帧图匹配

…详情请参照古月居