LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

在可变地形上的轻量级的利用地面点优化的Iidar 里程计和 建图

作者：Tixiao Shan and Brendan Englot

Abstract— We propose a lightweight and ground-optimized lidar odometry and mapping method, LeGO-LOAM, for realtime six degree-of-freedom pose estimation with ground vehicles. LeGO-LOAM is lightweight, as it can achieve realtime pose estimation on a low-power embedded system. LeGOLOAM is ground-optimized, as it leverages the presence of a ground plane in its segmentation and optimization steps. We first apply point cloud segmentation to filter out noise, and feature extraction to obtain distinctive planar and edge features. A two-step Levenberg-Marquardt optimization method then uses the planar and edge features to solve different components of the six degree-of-freedom transformation across consecutive scans. We compare the performance of LeGO-LOAM with a state-of-the-art method, LOAM, using datasets gathered from variable-terrain environments with ground vehicles, and show that LeGO-LOAM achieves similar or better accuracy with reduced computational expense. We also integrate LeGO-LOAM
into a SLAM framework to eliminate the pose estimation error caused by drift, which is tested using the KITTI dataset.

我们提出了一种轻型和基于地面优化的激光雷达里程计和建图的方法LeGO-LOAM，用于地面车辆的实时六自由度姿态估计。

• LeGO-LOAM是轻量的，因为它可以在低功耗嵌入式系统上实现实时姿态估计。
• LeGOLOAM是基于地面优化的，它在分割和优化步骤中利用了地面。

具体步骤：

1. 首先应用点云分割来滤除噪声
2. 然后进行特征提取以获得不同的平面和边缘特征。
3. 然后利用平面和边缘特征采用了两步Levenberg-Marquardt优化方法，来求解连续扫描中六自由度变换的不同分量。

我们使用车辆在不同地形环境中收集的数据集，将LeGO-LOAM的性能与LOAM进行了比较，结果表明，LeGO-LOAM在降低计算开支的情况下达到了与LOAM类似或更好的精度。我们还将LeGO-LOAM集成到SLAM框架中，以消除漂移引起的姿态估计误差，并使用KITTI数据集进行了测试。

state-of-the-art  最先进的;使用最先进技术的;体现最高水平的

I. INTRODUCTION
Among the capabilities of an intelligent robot, map-building and state estimation are among the most fundamental prerequisites. Great efforts have been devoted to achieving real-time 6 degree-of-freedom simultaneous localization and mapping (SLAM) with vision-based and lidar-based methods. Although vision-based methods have advantages in loop-closure detection, their sensitivity to illumination and viewpoint change may make such capabilities unreliable if used as the sole navigation sensor. On the other hand, lidar-based methods will function even at night, and the high resolution of many 3D lidars permits the capture of the fine details of an  environment at long ranges, over a wide aperture. Therefore, this paper focuses on using 3D lidar to support real-time state estimation and mapping.
对于智能机器人而言，地图构建和状态估计是最基本的先决要求之一。人们一直致力于利用基于视觉和基于激光雷达的方法实现实时6自由度同步定位和建图（SLAM）。

• 基于视觉的方法在回环检测方面具有优势，但如果将其用作唯一的导航传感器，相机对于照明和视点变化的敏感性可能会使此类功能不可靠。
• 基于激光雷达的方法即使在夜间也能发挥作用，许多3D激光雷达的高分辨率允许在大孔径上远距离捕捉环境的细节。

因此，本文主要研究利用三维激光雷达来实现SLAM。

The typical approach for finding the transformation between two lidar scans is iterative closest point (ICP) [1]. By finding correspondences at a point-wise level, ICP aligns two sets of points iteratively until stopping criteria are satisfied. When the scans include large quantities of points, ICP may suffer from prohibitive computational cost. Many variants of ICP have been proposed to improve its efficiency and accuracy [2]. [3] introduces a point-to-plane ICP variant that matches points to local planar patches. Generalized-ICP [4] proposes a method that matches local planar patches from both scans. In addition, several ICP variants have leveraged parallel computing for improved efficiency [5]–[8].
寻找两次激光雷达扫描之间位姿变换的典型方法是迭代最近邻（ICP）[1]。通过逐点查找对应关系，ICP迭代对齐两组点，直到满足停止条件为止。当扫描包含大量的点时，ICP可能会承受高昂的计算成本。为了提高ICP的效率和准确性，已经提出了许多ICP变体[2]。[3] 介绍一种点到平面ICP变体，该方法将点与局部平面面片相匹配。广义ICP[4]提出了一种匹配两次扫描的局部平面面片的方法。此外，几个ICP变种利用了并行计算来提高效率[5]–[8]。 

Feature-based matching methods are attracting more attention, as they require less  computational resources by extracting representative features from the environment. These
features should be suitable for effective matching and invariant of point-of-view. Many detectors, such as Point Feature Histograms (PFH) [9] and Viewpoint Feature Histograms (VFH) [10], have been proposed for extracting such features from point clouds using simple and efficient  techniques. A method for extracting general-purpose features from point clouds using a Kanade-Tomasi corner detector is introduced in [11]. A framework for extracting line and plane features
from dense point clouds is discussed in [12].
基于特征的匹配方法越来越受到人们的关注，因为它们通过从环境中提取具有代表性的特征来减少计算资源。这些特征应适用于有效匹配和视点不变性。社区已经提出了许多检测器，例如点特征直方图（PFH）[9]和视点特征直方图（VFH）[10]，用于使用简单有效的技术从点云中提取此类特征。[11]介绍了一种使用Kanade-Tomasi角点检测器从点云中提取通用特征的方法。[12]讨论了从密集点云中提取直线和平面特征的框架。

Many algorithms that use features for point cloud registration have also been proposed. [13] and [14] present a keypoint selection algorithm that performs point curvature calculations in a local cluster. The selected keypoints are then used to perform matching and place recognition. By
projecting a point cloud onto a range image and analyzing the second derivative of the depth values, [15] selects features from points that have high curvature for matching and place recognition. Assuming the environment is composed of planes, a plane-based registration algorithm is proposed in [16]. An outdoor environment, e.g., a forest, may limit the application of such a method. A collar line segments (CLS) method, which is especially designed for Velodyne lidar, is presented in [17]. CLS randomly generates lines using points from two consecutive “rings” of a scan. Thus two line clouds are generated and used for registration. However, this method suffers from challenges arising from the random generation of lines. A segmentation-based registration algorithm is proposed in [18]. SegMatch first applies segmentation to a point
cloud. Then a feature vector is calculated for each segment based on its eigenvalues and shape histograms. A random forest is used to match the segments from two scans. Though this method can be used for online pose estimation, it can only provide localization updates at about 1Hz.
许多使用特征进行点云配准的算法也被提出。[13] [14]提出了一种关键点选择算法，该算法在局部簇中执行点曲率计算。然后使用选定的关键点执行匹配和位置识别。通过将点云投影到距离图像上并分析深度值的二阶导数，[15]从具有高曲率的点中选择特征进行匹配和位置识别。假设环境由平面组成，在[16]中提出了一种基于平面的配准算法。室外环境，例如森林，可能会限制这种方法的应用。文献[17]中介绍了专门为Velodyne激光雷达设计的项圈线段（CLS）方法。CLS使用扫描的两个连续“环”中的点随机生成线。因此，将生成两个线云并用于注册。然而，这种方法受到随机生成线的挑战。文献[18]提出了一种基于分割的配准算法。SegMatch首先将分段应用于点云。然后根据特征值和形状直方图计算每个线段的特征向量。随机林用于匹配两次扫描的段。虽然这种方法可以用于在线姿态估计，但它只能提供1Hz左右的定位更新。

A low-drift and real-time lidar odometry and mapping (LOAM) method is proposed in [19] and [20]. LOAM performs point feature to edge/plane scan-matching to find correspondences between scans. Features are extracted by calculating the roughness of a point in its local region.
The points with high roughness values are selected as edge features. Similarly, the points with low roughness values are designated planar features. Real-time performance is
achieved by novelly dividing the estimation problem across two individual algorithms. One algorithm runs at high frequency and estimates sensor velocity at low accuracy. The
other algorithm runs at low frequency but returns high-accuracy motion estimation. The two estimates are fused together to produce a single motion estimate at both high frequency and high accuracy. LOAM’s resulting accuracy is the best achieved by a lidar-only estimation method on the KITTI odometry benchmark site [21].
[19]和[20]中提出了一种低漂移实时激光雷达里程计和建图（LOAM）方法。LOAM执行点特征到边/平面的扫描匹配，以查找扫描之间的对应关系。通过计算局部点的曲率（roughness） 来提取特征。选择曲率（roughness）较高的点作为边缘特征。同样，曲率（roughness）较低的点被指定为平面特征。实时性能是通过将估计问题分为两个单独的算法来实现的。一种算法运行频率高，以较低的精度估计传感器的速度。另一种算法运行频率较低，但可以以高精度获得运动估计。将这两个估计值融合在一起，以产生高频和高精度的单个运动估计值。在KITTI里程计基准点上，通过仅使用激光雷达的估算方法，LOAM的结果精度达到最佳[21]。

In this work, we pursue reliable, real-time six degreeof-freedom pose estimation for ground vehicles equipped with 3D lidar, in a manner that is amenable to efficient implementation on a small-scale embedded system. Such a task is non-trivial for several reasons. Many unmanned
ground vehicles (UGVs) do not have suspensions or powerful computational units due to their limited size. Non-smooth motion is frequently encountered by small UGVs driving on
variable terrain, and as a result, the acquired data is often distorted. Reliable feature correspondences are also hard to find between two consecutive scans due to large motions
with limited overlap. Besides that, the large quantities of points received from a 3D lidar poses a challenge to real-time processing using limited on-board computational resources.
在这项工作中，我们致力于为安装了3D激光雷达的地面车辆提供可靠、实时的六自由度姿态估计，其方式在小型嵌入式系统上可以实现高效运转。出于几个原因，这样一项任务并非微不足道。由于尺寸有限，许多无人地面车辆（UGV）没有悬架或强大的计算单元。小型无人地面车辆（UGV）在多变的地形上行驶时，经常会遇到非平稳运动，因此，采集的数据往往会带有运动畸变。由于在较大的运动下只有有限的重叠区域，在两次连续扫描之间也很难找到可靠的特征对应。此外，从3D激光雷达接收到的大量点对使用有限的车载计算资源进行实时处理提出了挑战。

When we implement LOAM for such tasks, we can obtain low-drift motion estimation when a UGV is operated with smooth motion admist stable features, and supported by sufficient computational resources. However, the performance of LOAM deteriorates when resources are limited. Due to the need to compute the roughness of every point in a dense 3D point cloud, the update frequency of feature extraction on a lightweight embedded system cannot always keep up with the sensor update frequency. Operation of UGVs in noisy environments also poses challenges for LOAM. Since the mounting position of a lidar is often close to the ground on
a small UGV, sensor noise from the ground may be a constant presence. For example, range returns from grass may result in high roughness values. As a consequence, unreliable edge
features may be extracted from these points. Similarly, edge or planar features may also be extracted from points returned from tree leaves. Such features are usually not reliable for
scan-matching, as the same grass blade or leaf may not be seen in two consecutive scans. Using these features may lead to inaccurate registration and large drift.
当我们为这些任务实施LOAM时，当UGV以平滑运动和稳定特性运行，并且有足够的计算资源支持时，我们可以获得低漂移运动估计。然而，当资源受限时，LOAM的性能会恶化。由于需要计算密集三维点云中每个点的曲率（roughness），轻量级嵌入式系统上特征提取的更新频率不能始终跟上传感器的更新频率。UGV在噪声环境中的运行也对loam提出了挑战。由于在小型无人地面车辆（UGV）上，激光雷达的安装位置通常接近地面，因此来自地面的传感器噪声可能持续存在。例如，从草地返回的激光可能会存在较高的roughness values。因此，可能会从这些点提取不可靠的边缘特征。类似地，激光也会从树叶返回的点中提取边缘或平面特征。这种特征对于扫描匹配通常不可靠，因为在两次连续扫描中可能看不到相同的草叶或树叶。使用这些特征可能会导致不准确的对齐和较大的drift。

We therefore propose a lightweight and ground-optimized LOAM (LeGO-LOAM) for pose estimation of UGVs in complex environments with variable terrain. LeGO-LOAM
is lightweight, as real-time pose estimation and mapping can be achieved on an embedded system. Point cloud segmentation is performed to discard points that may represent
unreliable features after ground separation. LeGO-LOAM is also ground-optimized, as we introduce a two-step optimization for pose estimation. Planar features extracted from
the ground are used to obtain [tz; θroll; θpitch] during the first step. In the second step, the rest of the transformation [tx; ty; θyaw] is obtained by matching edge features extracted
from the segmented point cloud. We also integrate the ability to perform loop closures to correct motion estimation drift. The rest of the paper is organized as follows. Section II introduces the hardware used for experiments. Section III describes the proposed method in detail. Section IV presents a set of experiments over a variety of outdoor environments.

因此，我们提出了一种轻量化，面向地面优化的LOAM（LeGO-LOAM），用于复杂地形环境中UGV的姿态估计。LeGO-LOAM是轻量级的，可以在嵌入式系统上实现实时姿态估计和建图。执行点云分割以在分离地面之后，丢弃那些可能代表不可靠特征的点。 LeGO-LOAM也是基于地面优化的，我们提出了两步优化姿势估计的方法。

在第一步中，使用从地面提取的平面特征来获得[tz; θroll; θpitch]（利用前后帧匹配的地面点）。

在第二步中，变换的其余部分 [tx; ty; θyaw] 通过匹配 分割点云提取的边缘特征 来获得。

我们还集成了回环检测的能力，以纠正运动估计漂移。

论文的其余部分组织如下。

第二节介绍了用于实验的硬件。

第三节详细介绍了提出的方法。

第四节介绍了在各种室外环境下进行的一系列实验。

II. SYSTEM HARDWARE

The framework proposed in this paper is validated using datasets gathered from Velodyne VLP-16 and HDL-64E 3D lidars. The VLP-16 measurement range is up to 100m with an accuracy of ± 3cm. It has a vertical field of view (FOV) of 30◦(±15◦) and a horizontal FOV of 360◦. The 16-channel sensor provides a vertical angular resolution of 2◦. The horizontal angular resolution varies from 0.1◦ to 0.4◦ based on the rotation rate. Throughout the paper, we choose a scan rate of 10Hz, which provides a horizontal angular resolution of 0.2◦. The HDL-64E (explored in this work via the KITTI dataset) also has a horizontal FOV of 360◦ but 48 more channels. The vertical FOV of the HDL-64E is 26.9◦.

二、系统硬件

使用从Velodyne VLP-16和HDL-64E 3D激光雷达收集的数据集验证了本文提出的框架。

VLP-16测量范围可达100m，精度为±3cm。它的垂直视野（FOV）为30◦(±15◦) 水平视场360度◦. 16通道传感器提供2的垂直角度分辨率◦. 水平角度分辨率为0.1◦ 至0.4◦ 基于旋转速率。在本文中，我们选择了10Hz的扫描速率，其水平角度分辨率为0.2◦. HDL-64E（本研究通过KITTI数据集探索）的水平视场也为360◦ 但还有48个频道。HDL-64E的垂直视场为26.9◦.

The UGV used in this paper is the Clearpath Jackal. Powered by a 270 Watt hour Lithium battery, it has a maximum speed of 2.0m/s and maximum payload of 20kg. The Jackal is also equipped with a low-cost inertial measurement unit (IMU), the CH Robotics UM6 Orientation Sensor.

本文中使用的无人值守地面车辆是Clearpath Jackal。由270瓦时锂电池供电，最大速度为2.0m/s，最大有效载荷为20kg。豺狼还配备了低成本惯性测量单元（IMU），即CH Robotics UM6方向传感器。

The proposed framework is validated on two computers: an Nvidia Jetson TX2 and a laptop with a 2.5GHz i7-4710MQ CPU. The Jetson TX2 is an embedded computing device that is equipped with an ARM Cortex-A57 CPU. The laptop CPU was selected to match the computing hardware used in [19] and [20]. The experiments shown in this paper use the CPUs of these systems only.  

该框架在两台计算机上进行了验证：一台Nvidia Jetson TX2和一台配备2.5GHz i7-4710MQ CPU的笔记本电脑。Jetson TX2是一款嵌入式计算设备，配备ARM Cortex-A57 CPU。选择笔记本电脑CPU以匹配[19]和[20]中使用的计算硬件。本文中的实验仅使用这些系统的CPU。

III. LIGHTWEIGHT LIDAR ODOMETRY AND MAPPING
A. System Overview
An overview of the proposed framework is shown in Figure 1. The system receives input from a 3D lidar and outputs 6 DOF pose estimation. The overall system is divided into five modules. The first, segmentation, takes a single scan’s point cloud and projects it onto a range image for
segmentation. The segmented point cloud is then sent to the feature extraction module. Then, lidar odometry uses features extracted from the previous module to find the transformation relating consecutive scans. The features are further processed in lidar mapping, which registers them to a global point cloud map. At last, the transform integration module fuses the pose estimation results from lidar odometry and lidar mapping and outputs the final pose estimate. The
proposed system seeks improved efficiency and accuracy for ground vehicles, with respect to the original, generalized LOAM framework of [19] and [20]. The details of these modules are introduced below.

论文框架的概述如图1所示。该系统接收来自3D激光雷达的输入，并输出6个自由度的姿态估计。

整个系统分为五个模块。

第一个模块是分割（分类），将单个扫描的点云投影到范围图像上进行分割（分类）。

然后将分割后的点云发送到特征提取模块，激光雷达里程计从上述模块中提取特征，找到与连续扫描相关的变换。

这些特征将在LIDAR建图模块中做进一步的处理，该模块会将它们对齐到全局点云地图。

最后，变换积分模块将激光雷达里程计和激光雷达建图的姿态估计结果进行融合，输出最终的姿态估计。与[19]和[20]的原始广义的LOAM框架相比，所提出的方法寻求提高地面车辆的效率和精度。下面介绍这些模块的详细信息。

 Fig. 2: Feature extraction process for a scan in noisy environment. The original point cloud is shown in (a). In (b), the red points are labeled as ground points. The rest of the points are the points that remain after segmentation. In (c), blue and yellow points indicate edge and planar features in Fe and Fp. In (d), the green and pink points represent edge and planar features in and  respectively.

图2：噪声环境中扫描的特征提取过程。原始点云如（a）所示。

在（b）中，红点为标记为接地点。其余的点是分割后保留。

在（c）中，蓝色和黄色点表示Fe和Fp中的边和平面特征。

在（d）中，绿色和粉色点分别表示和中的边缘和平面特征。

 

 B. Segmentation
Let Pt = fp1; p2; :::; png be the point cloud acquired at time t, where pi is a point in Pt. Pt is first projected onto a range image. The resolution of the projected range image is 1800 by 16, since the VLP-16 has horizontal and vertical angular resolution of 0.2◦ and 2◦ respectively. Each valid
point pi in Pt is now represented by a unique pixel in the range image. The range value ri that is associated with pi represents the Euclidean distance from the corresponding point pi to the sensor. Since sloped terrain is common in many environments, we do not assume the ground is flat. A column-wise evaluation of the range image, which can be viewed as ground plane estimation [22], is conducted for ground point extraction before segmentation. After this
process, points that may represent the ground are labeled as ground points and not used for segmentation.

开源地址：

GitHub - RobustFieldAutonomyLab/LeGO-LOAM: LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain