LOAM: Lidar Odometry and Mapping in Real-time

LOAM：实时激光雷达里程计与建图

Ji Zhang and Sanjiv Singh

J. Zhang and S. Singh are with the Robotics Institute at Carnegie Mellon University. Emails: zhangji@cmu.edu and ssingh@cmu.edu.
The paper is based upon work supported by the National Science Foundation under Grant No. IIS-1328930.

Abstract

摘要

We propose a real-time method for odometry and mapping using range measurements from a 2-axis lidar moving in 6-DOF. The problem is hard because the range measurements are received at different times, and errors in motion estimation can cause mis-registration of the resulting point cloud. To date, coherent 3D maps can be built by off-line batch methods, often using loop closure to correct for drift over time. Our method achieves both low-drift and low-computational complexity without the need for high accuracy ranging or inertial measurements. The key idea in obtaining this level of performance is the division of the complex problem of simultaneous localization and mapping, which seeks to optimize a large number of variables simultaneously, by two algorithms. One algorithm performs odometry at a high frequency but low fidelity to estimate velocity of the lidar. Another algorithm runs at a frequency of an order of magnitude lower for fine matching and registration of the point cloud. Combination of the two algorithms allows the method to map in real-time. The method has been evaluated by a large set of experiments as well as on the KITTI odometry benchmark. The results indicate that the method can achieve accuracy at the level of state of the art offline batch methods.

我们提出一种实时方法，用一台在六自由度中运动的两轴激光雷达的测距数据来做里程计与建图。这一问题之所以困难，是因为测距点在不同时刻到达，运动估计的误差会使点云对不齐。迄今为止，连贯的三维地图多由离线批处理方法建成，并常常借助闭环来修正随时间累积的漂移。我们的方法在不必依赖高精度测距或惯性测量的情况下，同时获得了低漂移和低计算复杂度。达到这一性能的关键，是把同时定位与建图这一需要同时优化大量变量的复杂问题，拆成两条算法。一条算法以高频但较低保真度做里程计，用来估计雷达速度；另一条算法的频率低一个数量级，用于点云的精细匹配与配准。二者结合，使该方法能够实时建图。方法已在大量实验以及 KITTI 里程计基准上评测。结果表明，其精度可以达到当时最先进的离线批处理方法的水平。

Fig. 1. The method aims at motion estimation and mapping using a moving 2-axis lidar. Since the laser points are received at different times, distortion is present in the point cloud due to motion of the lidar (shown in the left lidar cloud). Our proposed method decomposes the problem by two algorithms running in parallel. An odometry algorithm estimates velocity of the lidar and corrects distortion in the point cloud, then, a mapping algorithm matches and registers the point cloud to create a map. Combination of the two algorithms ensures feasibility of the problem to be solved in real-time.

图 1. 该方法旨在用一台运动中的两轴激光雷达做运动估计与建图。由于激光点在不同时刻到达，雷达运动会使点云出现扭曲（见左侧雷达点云）。我们提出的方法把问题分解为两条并行运行的算法。一条里程计算法估计雷达速度并纠正点云中的扭曲，随后一条建图算法匹配并配准点云以生成地图。二者结合，保证了这一问题可以实时求解。

I. INTRODUCTION

I. 引言

3D mapping remains a popular technology [1]–[3]. Mapping with lidars is common as lidars can provide high frequency range measurements where errors are relatively constant irrespective of the distances measured. In the case that the only motion of the lidar is to rotate a laser beam, registration of the point cloud is simple. However, if the lidar itself is moving as in many applications of interest, accurate mapping requires knowledge of the lidar pose during continuous laser ranging. One common way to solve the problem is using independent position estimation (e.g. by a GPS/INS) to register the laser points into a fixed coordinate system. Another set of methods use odometry measurements such as from wheel encoders or visual odometry systems [4], [5] to register the laser points. Since odometry integrates small incremental motions over time, it is bound to drift and much attention is devoted to reduction of the drift (e.g. using loop closure).

三维建图仍然是一项热门技术 [1]–[3]。用激光雷达建图很常见，因为雷达能提供高频测距，且误差相对稳定，并不随测距距离明显变化。若雷达的唯一运动只是旋转光束，点云配准很简单。然而，在许多我们关心的应用里，雷达自身也在运动，这时要准确建图，就必须知道连续测距过程中雷达的位姿。一种常见做法是用独立的位置估计（例如 GPS/INS）把激光点登记到固定坐标系中。另一类方法则用轮式编码器或视觉里程计等里程计测量 [4], [5] 来配准激光点。由于里程计是在时间上积分许多小的增量运动，必然会产生漂移，因此大量工作都花在减小漂移上（例如利用闭环）。

Here we consider the case of creating maps with low-drift odometry using a 2-axis lidar moving in 6-DOF. A key advantage of using a lidar is its insensitivity to ambient lighting and optical texture in the scene. Recent developments in lidars have reduced their size and weight. The lidars can be held by a person who traverses an environment [6], or even attached to a micro aerial vehicle [7]. Since our method is intended to push issues related to minimizing drift in odometry estimation, it currently does not involve loop closure.

这里我们考虑的情形是：用一台在六自由度中运动的两轴激光雷达，依靠低漂移里程计来建图。使用激光雷达的一个关键优点，是它对环境光照和场景中的光学纹理不敏感。近年来激光雷达的体积和重量都已减小，可以由人持着穿越环境 [6]，甚至安装在微型飞行器上 [7]。由于我们的方法意在推进与减小里程计估计漂移相关的问题，目前并不包含闭环。

The method achieves both low-drift and low-computational complexity without the need for high accuracy ranging or inertial measurements. The key idea in obtaining this level of performance is the division of the typically complex problem of simultaneous localization and mapping (SLAM) [8], which seeks to optimize a large number of variables simultaneously, by two algorithms. One algorithm performs odometry at a high frequency but low fidelity to estimate velocity of the lidar. Another algorithm runs at a frequency of an order of magnitude lower for fine matching and registration of the point cloud. Although unnecessary, if an IMU is available, a motion prior can be provided to help account for high frequency motion. Specifically, both algorithms extract feature points located on sharp edges and planar surfaces, and match the feature points to edge line segments and planar surface patches, respectively. In the odometry algorithm, correspondences of the feature points are found by ensuring fast computation. In the mapping algorithm, the correspondences are determined by examining geometric distributions of local point clusters, through the associated eigenvalues and eigenvectors.

该方法在不必依赖高精度测距或惯性测量的情况下，同时获得了低漂移和低计算复杂度。达到这一性能的关键，是把通常很复杂的同时定位与建图（SLAM）问题 [8]——也就是要同时优化大量变量——拆成两条算法。一条算法以高频但较低保真度做里程计，用来估计雷达速度；另一条算法的频率低一个数量级，用于点云的精细匹配与配准。虽然并非必要，但如果有 IMU，也可以提供运动先验，以帮助应对高频运动。具体而言，两条算法都提取位于尖锐边沿和平面上的特征点，并分别把它们匹配到边线线段和平面小面片上。在里程计算法中，特征点的对应关系以保证计算快速的方式寻找；在建图算法中，对应关系则通过考察局部点簇的几何分布，以及与之相关的特征值和特征向量来确定。

By decomposing the original problem, an easier problem is solved first as online motion estimation. After which, mapping is conducted as batch optimization (similar to iterative closest point (ICP) methods [9]) to produce high-precision motion estimates and maps. The parallel algorithm structure ensures feasibility of the problem to be solved in real-time. Further, since the motion estimation is conducted at a higher frequency, the mapping is given plenty of time to enforce accuracy. When running at a lower frequency, the mapping algorithm is able to incorporate a large number of feature points and use sufficiently many iterations for convergence.

通过分解原问题，首先把一个更容易的问题作为在线运动估计来求解。随后，建图作为批优化来进行（类似于迭代最近点（ICP）方法 [9]），以产生高精度的运动估计和地图。并行的算法结构保证了这一问题可以实时求解。此外，由于运动估计在更高频率上进行，建图就有充足的时间来保证精度。当以较低频率运行时，建图算法能够纳入大量特征点，并使用足够多的迭代以达到收敛。

II. RELATED WORK

II. 相关工作

Lidar has become a useful range sensor in robot navigation [10]. For localization and mapping, most applications use 2D lidars [11]. When the lidar scan rate is high compared to its extrinsic motion, motion distortion within the scans can often be neglected. In this case, standard ICP methods [12] can be used to match laser returns between different scans. Additionally, a two-step method is proposed to remove the distortion [13]: an ICP based velocity estimation step is followed by a distortion compensation step, using the computed velocity. A similar technique is also used to compensate for the distortion introduced by a single-axis 3D lidar [14]. However, if the scanning motion is relatively slow, motion distortion can be severe. This is especially the case when a 2-axis lidar is used since one axis is typically much slower than the other. Often, other sensors are used to provide velocity measurements, with which, the distortion can be removed. For example, the lidar cloud can be registered by state estimation from visual odometry integrated with an IMU [15]. When multiple sensors such as a GPS/INS and wheel encoders are available concurrently, the problem is usually solved through an extended Kalman filer [16] or a particle filter [1]. These methods can create maps in real-time to assist path planning and collision avoidance in robot navigation.

激光雷达已成为机器人导航中有用的测距传感器 [10]。在定位与建图中，大多数应用使用二维激光雷达 [11]。当雷达扫描频率相对其外部运动足够高时，扫描内部的运动扭曲往往可以忽略。这时可以用标准 ICP 方法 [12] 在不同扫描之间匹配激光回波。此外，也有人提出两步法来去除扭曲 [13]：先用基于 ICP 的速度估计，再用算得的速度做扭曲补偿。类似技术也被用来补偿单轴三维激光雷达引入的扭曲 [14]。然而，若扫描运动相对较慢，运动扭曲会很严重。两轴激光雷达尤其如此，因为通常有一根轴比另一根慢得多。人们常常用其他传感器提供速度测量，并据此去除扭曲。例如，可以用与 IMU 结合的视觉里程计的状态估计来配准雷达点云 [15]。当 GPS/INS 和轮式编码器等多种传感器同时可用时，问题通常通过扩展卡尔曼滤波 [16] 或粒子滤波 [1] 来求解。这些方法可以实时建图，以辅助机器人导航中的路径规划与避障。

If a 2-axis lidar is used without aiding from other sensors, motion estimation and distortion correction become one problem. A method used by Barfoot et al. is to create visual images from laser intensity returns, and match visually distinct features [17] between images to recover motion of a ground vehicle [18]–[21]. The vehicle motion is modeled as constant velocity in [18], [19] and with Gaussian processes in [20], [21]. Our method uses a similar linear motion model as [18], [19] in the odometry algorithm, but with different types of features. The methods [18]–[21] involve visual features from intensity images and require dense point cloud. Our method extracts and matches geometric features in Cartesian space and has a lower requirement on the cloud density.

如果两轴激光雷达不借助其他传感器，运动估计与扭曲校正就变成同一个问题。Barfoot 等人使用的一种方法，是由激光强度回波生成视觉图像，再在图像间匹配视觉上显著的特征 [17]，以恢复地面车辆的运动 [18]–[21]。车辆运动在 [18], [19] 中被建模为匀速，在 [20], [21] 中则用高斯过程建模。我们的方法在里程计算法中采用与 [18], [19] 类似的线性运动模型，但特征类型不同。[18]–[21] 使用来自强度图像的视觉特征，并需要稠密点云。我们的方法在笛卡尔空间中提取并匹配几何特征，对点云密度的要求更低。

The approach closest to ours is that of Bosse and Zlot [3], [6], [22]. They use a 2-axis lidar to obtain point cloud which is registered by matching geometric structures of local point clusters [22]. Further, they use multiple 2-axis lidars to map an underground mine [3]. This method incorporates an IMU and uses loop closure to create large maps. Proposed by the same authors, Zebedee is a mapping device composed of a 2D lidar and an IMU attached to a hand-bar through a spring [6]. Mapping is conducted by hand nodding the device. The trajectory is recovered by a batch optimization method that processes segmented datasets with boundary constraints added between the segments. In this method, the measurements of the IMU are used to register the laser points and the optimization is used to correct the IMU biases. In essence, Bosse and Zlot’s methods require batch processing to develop accurate maps and therefore are unsuitable for applications where maps are needed in real-time. In comparison, the proposed method in real-time produces maps that are qualitatively similar to those by Bosse and Zlot. The distinction is that our method can provide motion estimates for guidance of an autonomous vehicle. Further, the method takes advantage of the lidar scan pattern and point cloud distribution. Feature matching is realized ensuring computation speed and accuracy in the odometry and mapping algorithms, respectively.

与我们最接近的方法是 Bosse 与 Zlot 的工作 [3], [6], [22]。他们用两轴激光雷达获取点云，并通过匹配局部点簇的几何结构来配准 [22]。他们还用多台两轴激光雷达为地下矿井建图 [3]。该方法结合了 IMU，并利用闭环来生成大地图。同一作者提出的 Zebedee 是一种建图装置，由二维激光雷达和 IMU 经弹簧连接到手柄上构成 [6]。建图时用手点头式地摆动装置。轨迹由批优化方法恢复，该方法处理分段数据，并在各段之间加入边界约束。在这种方法中，IMU 测量用来配准激光点，优化则用来校正 IMU 偏差。本质上，Bosse 与 Zlot 的方法需要批处理才能做出准确地图，因此不适合需要实时地图的应用。相比之下，本文方法能够实时产生在外观上与 Bosse 和 Zlot 的结果定性相似的地图。区别在于，我们的方法还能提供用于自主车辆导引的运动估计。此外，该方法利用了激光雷达的扫描模式和点云分布。特征匹配分别在里程计算法中保证计算速度，在建图算法中保证精度。

III. NOTATIONS AND TASK DESCRIPTION

III. 符号与任务描述

The problem addressed in this paper is to perform ego-motion estimation with point cloud perceived by a 3D lidar, and build a map for the traversed environment. We assume that the lidar is pre-calibrated. We also assume that the angular and linear velocities of the lidar are smooth and continuous over time, without abrupt changes. The second assumption will be released by usage of an IMU, in Section VII-B.

本文要解决的问题是：利用三维激光雷达感知到的点云做自运动估计，并为所经过的环境建立地图。我们假定雷达已经预先标定。我们还假定雷达的角速度和线速度在时间上光滑连续，没有突变。第二个假定将在第 VII-B 节中通过使用 IMU 加以放松。

As a convention in this paper, we use right uppercase superscription to indicate the coordinate systems. We define a sweep as the lidar completes one time of scan coverage. We use right subscription $k$, $k \in Z^{+}$ to indicate the sweeps, and $\mathcal{P}_k$ to indicate the point cloud perceived during sweep $k$. Let us define two coordinate systems as follows.

按本文的约定，我们用右上标表示坐标系。我们把雷达完成一次扫描覆盖称为一个扫描周期（sweep）。用右下标 $k$，$k \in Z^{+}$ 表示各个扫描周期，并用 $\mathcal{P}_k$ 表示在扫描周期 $k$ 中感知到的点云。两个坐标系定义如下。

• Lidar coordinate system $\{L\}$ is a 3D coordinate system with its origin at the geometric center of the lidar. The $x$-axis is pointing to the left, the $y$-axis is pointing upward, and the $z$-axis is pointing forward. The coordinates of a point $i$, $i \in \mathcal{P}_k$, in $\{L_k\}$ are denoted as $X^L_{(k,i)}$.

• 雷达坐标系 $\{L\}$ 是一个三维坐标系，原点位于雷达的几何中心。$x$ 轴指向左，$y$ 轴指向上，$z$ 轴指向前。点 $i$，$i \in \mathcal{P}_k$，在 $\{L_k\}$ 中的坐标记为 $X^L_{(k,i)}$。

• World coordinate system $\{W\}$ is a 3D coordinate system coinciding with $\{L\}$ at the initial position. The coordinates of a point $i$, $i \in \mathcal{P}_k$, in $\{W_k\}$ are $X^W_{(k,i)}$.

• 世界坐标系 $\{W\}$ 是一个三维坐标系，在初始位置与 $\{L\}$ 重合。点 $i$，$i \in \mathcal{P}_k$，在 $\{W_k\}$ 中的坐标为 $X^W_{(k,i)}$。

With assumptions and notations made, our lidar odometry and mapping problem can be defined as

在给出假定和符号之后，我们的激光雷达里程计与建图问题可以定义为

Problem: Given a sequence of lidar cloud $\mathcal{P}_k$, $k \in Z^{+}$, compute the ego-motion of the lidar during each sweep $k$, and build a map with $\mathcal{P}_k$ for the traversed environment.

问题： 给定雷达点云序列 $\mathcal{P}_k$，$k \in Z^{+}$，计算雷达在每个扫描周期 $k$ 中的自运动，并用 $\mathcal{P}_k$ 为所经过的环境建立地图。

IV. SYSTEM OVERVIEW

IV. 系统概述

A. Lidar Hardware

A. 激光雷达硬件

The study of this paper is validated on, but not limited to a custom built 3D lidar based on a Hokuyo UTM-30LX laser scanner. Through the paper, we will use data collected from the lidar to illustrate the method. The laser scanner has a field of view of $180^{\circ}$ with $0.25^{\circ}$ resolution and 40 lines/sec scan rate. The laser scanner is connected to a motor, which is controlled to rotate at an angular speed of $180^{\circ}/s$ between $-90^{\circ}$ and $90^{\circ}$ with the horizontal orientation of the laser scanner as zero. With this particular unit, a sweep is a rotation from $-90^{\circ}$ to $90^{\circ}$ or in the inverse direction (lasting for 1s). Here, note that for a continuous spinning lidar, a sweep is simply a semi-spherical rotation. An onboard encoder measures the motor rotation angle with a resolution of $0.25^{\circ}$, with which, the laser points are projected into the lidar coordinates, $\{L\}$.

本文的研究在一台基于 Hokuyo UTM-30LX 激光扫描器的自制三维激光雷达上验证，但不限于这一硬件。全文将用该雷达采集的数据来说明方法。该扫描器视场为 $180^{\circ}$，分辨率为 $0.25^{\circ}$，扫描速率为 40 线/秒。扫描器连接到一台电机，电机被控制以 $180^{\circ}/s$ 的角速度在 $-90^{\circ}$ 与 $90^{\circ}$ 之间旋转，并以扫描器的水平方位为零点。对这一特定装置，一个扫描周期是从 $-90^{\circ}$ 转到 $90^{\circ}$，或沿相反方向旋转（持续 1 秒）。这里需要说明的是，对连续旋转的激光雷达，一个扫描周期就是一次半球扫描。机载编码器以 $0.25^{\circ}$ 的分辨率测量电机转角，并据此把激光点投影到雷达坐标系 $\{L\}$ 中。

Fig. 2. The 3D lidar used in this study consists of a Hokuyo laser scanner driven by a motor for rotational motion, and an encoder that measures the rotation angle. The laser scanner has a field of view of $180^{\circ}$ with a resolution of $0.25^{\circ}$. The scan rate is 40 lines/sec. The motor is controlled to rotate from $-90^{\circ}$ to $90^{\circ}$ with the horizontal orientation of the laser scanner as zero.

图 2. 本研究使用的三维激光雷达由一台由电机带动旋转的 Hokuyo 激光扫描器和一台测量转角的编码器组成。扫描器视场为 $180^{\circ}$，分辨率为 $0.25^{\circ}$。扫描速率为 40 线/秒。电机被控制从 $-90^{\circ}$ 转到 $90^{\circ}$，并以扫描器的水平方位为零点。

B. Software System Overview

B. 软件系统概述

Fig. 3. Block diagram of the lidar odometry and mapping software system.

图 3. 激光雷达里程计与建图软件系统的框图。

Fig. 3 shows a diagram of the software system. Let $\hat{\mathcal{P}}$ be the points received in a laser scan. During each sweep, $\hat{\mathcal{P}}$ is registered in $\{L\}$. The combined point cloud during sweep $k$ forms $\mathcal{P}_k$. Then, $\mathcal{P}_k$ is processed in two algorithms. The lidar odometry takes the point cloud and computes the motion of the lidar between two consecutive sweeps. The estimated motion is used to correct distortion in $\mathcal{P}_k$. The algorithm runs at a frequency around 10Hz. The outputs are further processed by lidar mapping, which matches and registers the undistorted cloud onto a map at a frequency of 1Hz. Finally, the pose transforms published by the two algorithms are integrated to generate a transform output around 10Hz, regarding the lidar pose with respect to the map. Section V and VI present the blocks in the software diagram in detail.

图 3 给出了软件系统的框图。设 $\hat{\mathcal{P}}$ 为一次激光扫描中收到的点。在每个扫描周期内，$\hat{\mathcal{P}}$ 被登记到 $\{L\}$ 中。扫描周期 $k$ 内合并得到的点云记为 $\mathcal{P}_k$。随后，$\mathcal{P}_k$ 由两条算法处理。激光雷达里程计接收该点云，并计算相邻两个扫描周期之间雷达的运动。估计得到的运动用来纠正 $\mathcal{P}_k$ 中的扭曲。该算法的运行频率约为 10Hz。其输出再交给激光雷达建图：建图算法以 1Hz 的频率，把去扭曲后的点云匹配并配准到地图上。最后，把两条算法发布的位姿变换加以组合，以约 10Hz 给出雷达相对地图的位姿。第 V 节和第 VI 节将详细介绍软件框图中的各个模块。

V. LIDAR ODOMETRY

V. 激光雷达里程计

A. Feature Point Extraction

A. 特征点提取

We start with extraction of feature points from the lidar cloud, $\mathcal{P}_k$. The lidar presented in Fig. 2 naturally generates unevenly distributed points in $\mathcal{P}_k$. The returns from the laser scanner has a resolution of $0.25^{\circ}$ within a scan. These points are located on a scan plane. However, as the laser scanner rotates at an angular speed of $180^{\circ}/s$ and generates scans at 40Hz, the resolution in the perpendicular direction to the scan planes is $180^{\circ}/40 = 4.5^{\circ}$. Considering this fact, the feature points are extracted from $\mathcal{P}_k$ using only information from individual scans, with co-planar geometric relationship.

我们首先从雷达点云 $\mathcal{P}_k$ 中提取特征点。图 2 所示的激光雷达会在 $\mathcal{P}_k$ 中自然产生分布不均匀的点。激光扫描器在一次扫描内的回波分辨率为 $0.25^{\circ}$。这些点位于同一个扫描平面上。然而，扫描器以 $180^{\circ}/s$ 的角速度旋转，并以 40Hz 产生扫描，因此垂直于扫描平面方向上的分辨率是 $180^{\circ}/40 = 4.5^{\circ}$。考虑到这一点，特征点只利用各条扫描内部的信息，依据共面的几何关系，从 $\mathcal{P}_k$ 中提取。

We select feature points that are on sharp edges and planar surface patches. Let $i$ be a point in $\mathcal{P}_k$, $i \in \mathcal{P}_k$, and let $\mathcal{S}$ be the set of consecutive points of $i$ returned by the laser scanner in the same scan. Since the laser scanner generates point returns in CW or CCW order, $\mathcal{S}$ contains half of its points on each side of $i$ and $0.25^{\circ}$ intervals between two points. Define a term to evaluate the smoothness of the local surface,

我们选择位于尖锐边沿和平面小面片上的特征点。设 $i$ 为 $\mathcal{P}_k$ 中的一个点，$i \in \mathcal{P}_k$，并设 $\mathcal{S}$ 为激光扫描器在同一条扫描中返回的、与 $i$ 相邻的连续点集。由于扫描器按顺时针或逆时针顺序产生回波，$\mathcal{S}$ 在 $i$ 的两侧各包含一半的点，点与点之间间隔 $0.25^{\circ}$。定义一个量来评价局部表面的光滑度，

$$c = \frac{1}{|\mathcal{S}| \cdot \left\|\boldsymbol{X}^{L}_{(k,i)}\right\|} \left\| \sum_{j \in \mathcal{S},\, j \neq i} \bigl(\boldsymbol{X}^{L}_{(k,i)} - \boldsymbol{X}^{L}_{(k,j)}\bigr) \right\|. \quad (1)$$

The points in a scan are sorted based on the $c$ values, then feature points are selected with the maximum $c$'s, namely, edge points, and the minimum $c$'s, namely planar points. To evenly distribute the feature points within the environment, we separate a scan into four identical subregions. Each subregion can provide maximally 2 edge points and 4 planar points. A point $i$ can be selected as an edge or a planar point only if its $c$ value is larger or smaller than a threshold, and the number of selected points does not exceed the maximum.

一条扫描中的点按 $c$ 值排序，然后选取 $c$ 最大的点作为特征点，即边沿点，以及 $c$ 最小的点，即平面点。为了使特征点在环境中分布均匀，我们把一条扫描分成四个相同的子区。每个子区最多提供 2 个边沿点和 4 个平面点。点 $i$ 只有在其 $c$ 值大于或小于某一阈值、且已选点数尚未超过上限时，才能被选为边沿点或平面点。

While selecting feature points, we want to avoid points whose surrounded points are selected, or points on local planar surfaces that are roughly parallel to the laser beams (point $B$ in Fig. 4(a)). These points are usually considered as unreliable. Also, we want to avoid points that are on boundary of occluded regions [23]. An example is shown in Fig. 4(b). Point $A$ is an edge point in the lidar cloud because its connected surface (the dotted line segment) is blocked by another object. However, if the lidar moves to another point of view, the occluded region can change and become observable. To avoid the aforementioned points to be selected, we find again the set of points $\mathcal{S}$. A point $i$ can be selected only if $\mathcal{S}$ does not form a surface patch that is roughly parallel to the laser beam, and there is no point in $\mathcal{S}$ that is disconnected from $i$ by a gap in the direction of the laser beam and is at the same time closer to the lidar then point $i$ (e.g. point $B$ in Fig. 4(b)).

在选取特征点时，我们希望避开周围点已被选中的点，以及位于与激光束近乎平行的局部平面上的点（图 4(a) 中的点 $B$）。这类点通常被认为不可靠。我们还希望避开位于遮挡区域边界上的点 [23]。图 4(b) 给出了一个例子。点 $A$ 在雷达点云中是一个边沿点，因为它所连接的表面（虚线段）被另一物体挡住。然而，如果雷达换到另一个视角，被遮挡的区域可能发生变化并变得可见。为避免选中上述这些点，我们再次找出点集 $\mathcal{S}$。点 $i$ 只有在以下条件同时满足时才能被选中：$\mathcal{S}$ 不会构成一块与激光束近乎平行的小面片，并且 $\mathcal{S}$ 中不存在这样的点——它与 $i$ 在激光束方向上被间隙断开，同时又比点 $i$ 更靠近雷达（例如图 4(b) 中的点 $B$）。

In summary, the feature points are selected as edge points starting from the maximum $c$ value, and planar points starting from the minimum $c$ value, and if a point is selected,

综上所述，特征点按如下方式选取：边沿点从最大的 $c$ 值开始取，平面点从最小的 $c$ 值开始取；若某一点被选中，则须满足

• The number of selected edge points or planar points cannot exceed the maximum of the subregion, and

• 该子区中已选的边沿点或平面点数不能超过上限，并且

• None of its surrounding point is already selected, and

• 其周围点中没有已经被选中的点，并且

• It cannot be on a surface patch that is roughly parallel to the laser beam, or on boundary of an occluded region.

• 它不能位于与激光束近乎平行的小面片上，也不能位于遮挡区域的边界上。

Fig. 4. (a) The solid line segments represent local surface patches. Point $A$ is on a surface patch that has an angle to the laser beam (the dotted orange line segments). Point $B$ is on a surface patch that is roughly parallel to the laser beam. We treat $B$ as a unreliable laser return and do not select it as a feature point. (b) The solid line segments are observable objects to the laser. Point $A$ is on the boundary of an occluded region (the dotted orange line segment), and can be detected as an edge point. However, if viewed from a different angle, the occluded region can change and become observable. We do not treat $A$ as a salient edge point or select it as a feature point.

图 4. (a) 实线段表示局部小面片。点 $A$ 所在的小面片与激光束（橙色虚线段）成一定夹角。点 $B$ 所在的小面片与激光束近乎平行。我们把 $B$ 视为不可靠的激光回波，不把它选为特征点。(b) 实线段是激光能够观测到的物体。点 $A$ 位于遮挡区域的边界上（橙色虚线段），因而可能被检测为边沿点。然而，若从另一个角度观察，被遮挡的区域可能发生变化并变得可见。我们不把 $A$ 当作显著的边沿点，也不把它选为特征点。

An example of extracted feature points from a corridor scene is shown in Fig. 5. The edge points and planar points are labeled in yellow and red colors, respectively.

图 5 给出了从一个走廊场景中提取出的特征点示例。边沿点和平面点分别用黄色和红色标出。

Fig. 5. An example of extracted edge points (yellow) and planar points (red) from lidar cloud taken in a corridor. Meanwhile, the lidar moves toward the wall on the left side of the figure at a speed of 0.5m/s, this results in motion distortion on the wall.

图 5. 从走廊中采集的雷达点云里提取出的边沿点（黄）和平面点（红）示例。与此同时，雷达以 0.5m/s 的速度朝图中左侧的墙面运动，因而墙面上出现了运动扭曲。

B. Finding Feature Point Correspondence

B. 寻找特征点对应

The odometry algorithm estimates motion of the lidar within a sweep. Let $t_k$ be the starting time of a sweep $k$. At the end of each sweep, the point cloud perceived during the sweep, $\mathcal{P}_k$, is reprojected to time stamp $t_{k+1}$, illustrated in Fig. 6. We denote the reprojected point cloud as $\bar{\mathcal{P}}_k$. During the next sweep $k+1$, $\bar{\mathcal{P}}_k$ is used together with the newly received point cloud, $\mathcal{P}_{k+1}$, to estimate the motion of the lidar.

里程计算法估计雷达在一个扫描周期内的运动。设 $t_k$ 为扫描周期 $k$ 的起始时刻。在每个扫描周期结束时，该周期内感知到的点云 $\mathcal{P}_k$ 被重新投影到时间戳 $t_{k+1}$，如图 6 所示。我们把重投影后的点云记为 $\bar{\mathcal{P}}_k$。在下一个扫描周期 $k+1$ 中，$\bar{\mathcal{P}}_k$ 与新收到的点云 $\mathcal{P}_{k+1}$ 一起用来估计雷达运动。

Fig. 6. Reprojecting point cloud to the end of a sweep. The blue colored line segment represents the point cloud perceived during sweep $k$, $\mathcal{P}_k$. At the end of sweep $k$, $\mathcal{P}_k$ is reprojected to time stamp $t_{k+1}$ to obtain $\bar{\mathcal{P}}_k$ (the green colored line segment). Then, during sweep $k + 1$, $\bar{\mathcal{P}}_k$ and the newly perceived point cloud $\mathcal{P}_{k+1}$ (the orange colored line segment) are used together to estimate the lidar motion.

图 6. 把点云重投影到一个扫描周期的结束时刻。蓝色线段表示扫描周期 $k$ 中感知到的点云 $\mathcal{P}_k$。在扫描周期 $k$ 结束时，把 $\mathcal{P}_k$ 重投影到时间戳 $t_{k+1}$，得到 $\bar{\mathcal{P}}_k$（绿色线段）。随后，在扫描周期 $k+1$ 中，$\bar{\mathcal{P}}_k$ 与新感知到的点云 $\mathcal{P}_{k+1}$（橙色线段）一起用来估计雷达运动。

Let us assume that both $\bar{\mathcal{P}}_k$ and $\mathcal{P}_{k+1}$ are available for now, and start with finding correspondences between the two lidar clouds. With $\mathcal{P}_{k+1}$, we find edge points and planar points from the lidar cloud using the methodology discussed in the last section. Let $\mathcal{E}_{k+1}$ and $\mathcal{H}_{k+1}$ be the sets of edge points and planar points, respectively. We will find edge lines from $\bar{\mathcal{P}}_k$ as the correspondences for the points in $\mathcal{E}_{k+1}$, and planar patches as the correspondences for those in $\mathcal{H}_{k+1}$.

我们先假定 $\bar{\mathcal{P}}_k$ 和 $\mathcal{P}_{k+1}$ 目前都已齐备，并从寻找两片雷达点云之间的对应关系开始。对 $\mathcal{P}_{k+1}$，我们用上一节的方法从点云中找出边沿点和平面点。设 $\mathcal{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$ 分别为边沿点集和平面点集。我们将在 $\bar{\mathcal{P}}_k$ 中为 $\mathcal{E}_{k+1}$ 里的点寻找边线作为对应，为 $\mathcal{H}_{k+1}$ 里的点寻找平面小面片作为对应。

Note that at the beginning of sweep $k+1$, $\mathcal{P}_{k+1}$ is an empty set, which grows during the course of the sweep as more points are received. The lidar odometry recursively estimates the 6-DOF motion during the sweep, and gradually includes more points as $\mathcal{P}_{k+1}$ increases. At each iteration, $\mathcal{E}_{k+1}$ and $\mathcal{H}_{k+1}$ are reprojected to the beginning of the sweep using the currently estimated transform. Let $\tilde{\mathcal{E}}_{k+1}$ and $\tilde{\mathcal{H}}_{k+1}$ be the reprojected point sets. For each point in $\tilde{\mathcal{E}}_{k+1}$ and $\tilde{\mathcal{H}}_{k+1}$, we are going to find the closest neighbor point in $\bar{\mathcal{P}}_k$. Here, $\bar{\mathcal{P}}_k$ is stored in a 3D KD-tree [24] for fast index.

需要注意的是，在扫描周期 $k+1$ 开始时，$\mathcal{P}_{k+1}$ 是空集，随着更多点的到达，它在该周期过程中逐渐增大。激光雷达里程计递归地估计该周期内的六自由度运动，并随着 $\mathcal{P}_{k+1}$ 的增大逐步纳入更多的点。在每次迭代中，用当前估计的变换把 $\mathcal{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$ 重新投影到该扫描周期的起始时刻。设 $\tilde{\mathcal{E}}_{k+1}$ 和 $\tilde{\mathcal{H}}_{k+1}$ 为重投影后的点集。对 $\tilde{\mathcal{E}}_{k+1}$ 和 $\tilde{\mathcal{H}}_{k+1}$ 中的每个点，我们要在 $\bar{\mathcal{P}}_k$ 中寻找最近邻点。这里把 $\bar{\mathcal{P}}_k$ 存入三维 KD 树 [24]，以便快速检索。

Fig. 7(a) represents the procedure of finding an edge line as the correspondence of an edge point. Let $i$ be a point in $\tilde{\mathcal{E}}_{k+1}$, $i \in \tilde{\mathcal{E}}_{k+1}$. The edge line is represented by two points. Let $j$ be the closest neighbor of $i$ in $\bar{\mathcal{P}}_k$, $j \in \bar{\mathcal{P}}_k$, and let $l$ be the closest neighbor of $i$ in the two consecutive scans to the scan of $j$. $(j, l)$ forms the correspondence of $i$. Then, to verify both $j$ and $l$ are edge points, we check the smoothness of the local surface based on (1). Here, we particularly require that $j$ and $l$ are from different scans considering that a single scan cannot contain more than one points from the same edge line. There is only one exception where the edge line is on the scan plane. If so, however, the edge line is degenerated and appears as a straight line on the scan plane, and feature points on the edge line should not be extracted in the first place.

图 7(a) 表示寻找边线作为某个边沿点之对应的过程。设 $i$ 为 $\tilde{\mathcal{E}}_{k+1}$ 中的一个点，$i \in \tilde{\mathcal{E}}_{k+1}$。边线由两点表示。设 $j$ 为 $i$ 在 $\bar{\mathcal{P}}_k$ 中的最近邻，$j \in \bar{\mathcal{P}}_k$，再设 $l$ 为 $i$ 在与 $j$ 所在扫描相邻的两条扫描中的最近邻。$(j, l)$ 构成 $i$ 的对应。随后，为验证 $j$ 和 $l$ 都是边沿点，我们根据 (1) 式检查局部表面的光滑度。这里我们特别要求 $j$ 和 $l$ 来自不同扫描，因为同一条扫描不可能包含来自同一条边线的多于一个点。唯一的例外是边线就落在扫描平面上。若是这种情况，边线已经退化，在扫描平面上表现为一条直线，这条边线上的特征点本来就不应当被提取出来。

Fig. 7. Finding an edge line as the correspondence for an edge point in $\tilde{\mathcal{E}}_{k+1}$ (a), and a planar patch as the correspondence for a planar point in $\tilde{\mathcal{H}}_{k+1}$ (b). In both (a) and (b), $j$ is the closest point to the feature point, found in $\bar{\mathcal{P}}_k$. The orange colored lines represent the same scan of $j$, and the blue colored lines are the two consecutive scans. To find the edge line correspondence in (a), we find another point, $l$, on the blue colored lines, and the correspondence is represented as $(j, l)$. To find the planar patch correspondence in (b), we find another two points, $l$ and $m$, on the orange and blue colored lines, respectively. The correspondence is $(j, l, m)$.

图 7. 为 $\tilde{\mathcal{E}}_{k+1}$ 中的边沿点寻找边线作为对应 (a)，以及为 $\tilde{\mathcal{H}}_{k+1}$ 中的平面点寻找平面小面片作为对应 (b)。在 (a) 和 (b) 中，$j$ 都是在 $\bar{\mathcal{P}}_k$ 里找到的、距该特征点最近的点。橙色线表示 $j$ 所在的同一条扫描，蓝色线是相邻的两条扫描。在 (a) 中寻找边线对应时，我们在蓝色线上再找一个点 $l$，对应表示为 $(j, l)$。在 (b) 中寻找平面小面片对应时，我们再找两个点 $l$ 和 $m$，分别位于橙色线和蓝色线上。对应为 $(j, l, m)$。

Fig. 7(b) shows the procedure of finding a planar patch as the correspondence of a planar point. Let $i$ be a point in $\tilde{\mathcal{H}}_{k+1}$, $i \in \tilde{\mathcal{H}}_{k+1}$. The planar patch is represented by three points. Similar to the last paragraph, we find the closest neighbor of $i$ in $\bar{\mathcal{P}}_k$, denoted as $j$. Then, we find another two points, $l$ and $m$, as the closest neighbors of $i$, one in the same scan of $j$, and the other in the two consecutive scans to the scan of $j$. This guarantees that the three points are non-collinear. To verify that $j$, $l$, and $m$ are all planar points, we check again the smoothness of the local surface based on (1).

图 7(b) 给出了寻找平面小面片作为某个平面点之对应的过程。设 $i$ 为 $\tilde{\mathcal{H}}_{k+1}$ 中的一个点，$i \in \tilde{\mathcal{H}}_{k+1}$。平面小面片由三个点表示。与上一段类似，我们在 $\bar{\mathcal{P}}_k$ 中找出 $i$ 的最近邻，记为 $j$。然后另外找两个点 $l$ 和 $m$，作为 $i$ 的最近邻：一个在 $j$ 所在的同一条扫描上，另一个在与 $j$ 所在扫描相邻的两条扫描上。这样就保证了三点不共线。为验证 $j$、$l$ 和 $m$ 都是平面点，我们再次根据 (1) 式检查局部表面的光滑度。

With the correspondences of the feature points found, now we derive expressions to compute the distance from a feature point to its correspondence. We will recover the lidar motion by minimizing the overall distances of the feature points in the next section. We start with edge points. For a point $i \in \tilde{\mathcal{E}}_{k+1}$, if $(j, l)$ is the corresponding edge line, $j, l \in \bar{\mathcal{P}}_k$, the point to line distance can be computed as

在找到特征点的对应关系之后，我们接下来推导特征点到其对应的距离表达式。下一节将通过最小化特征点的总体距离来恢复雷达运动。先看边沿点。对点 $i \in \tilde{\mathcal{E}}_{k+1}$，若 $(j, l)$ 是对应的边线，$j, l \in \bar{\mathcal{P}}_k$，则点到直线的距离可写为

$$d_{\mathcal{E}} = \frac{\left| \bigl(\tilde{\boldsymbol{X}}^{L}_{(k+1,i)} - \bar{\boldsymbol{X}}^{L}_{(k,j)}\bigr) \times \bigl(\tilde{\boldsymbol{X}}^{L}_{(k+1,i)} - \bar{\boldsymbol{X}}^{L}_{(k,l)}\bigr) \right|}{\left| \bar{\boldsymbol{X}}^{L}_{(k,j)} - \bar{\boldsymbol{X}}^{L}_{(k,l)} \right|}, \quad (2)$$

where $\tilde{\boldsymbol{X}}^{L}_{(k+1,i)}$, $\bar{\boldsymbol{X}}^{L}_{(k,j)}$, and $\bar{\boldsymbol{X}}^{L}_{(k,l)}$ are the coordinates of points $i$, $j$, and $l$ in $\{L\}$, respectively. Then, for a point $i \in \tilde{\mathcal{H}}_{k+1}$, if $(j, l, m)$ is the corresponding planar patch, $j, l, m \in \bar{\mathcal{P}}_k$, the point to plane distance is

其中 $\tilde{\boldsymbol{X}}^{L}_{(k+1,i)}$、$\bar{\boldsymbol{X}}^{L}_{(k,j)}$ 和 $\bar{\boldsymbol{X}}^{L}_{(k,l)}$ 分别是点 $i$、$j$ 和 $l$ 在 $\{L\}$ 中的坐标。然后，对点 $i \in \tilde{\mathcal{H}}_{k+1}$，若 $(j, l, m)$ 是对应的平面小面片，$j, l, m \in \bar{\mathcal{P}}_k$，则点到平面的距离为

$$d_{\mathcal{H}} = \frac{\left| \begin{array}{c}(\tilde{\boldsymbol{X}}^{L}_{(k+1,i)} - \bar{\boldsymbol{X}}^{L}_{(k,j)}) \\ \bigl((\bar{\boldsymbol{X}}^{L}_{(k,j)} - \bar{\boldsymbol{X}}^{L}_{(k,l)}) \times (\bar{\boldsymbol{X}}^{L}_{(k,j)} - \bar{\boldsymbol{X}}^{L}_{(k,m)})\bigr)\end{array} \right|}{\left| (\bar{\boldsymbol{X}}^{L}_{(k,j)} - \bar{\boldsymbol{X}}^{L}_{(k,l)}) \times (\bar{\boldsymbol{X}}^{L}_{(k,j)} - \bar{\boldsymbol{X}}^{L}_{(k,m)}) \right|}, \quad (3)$$

where $\bar{\boldsymbol{X}}^{L}_{(k,m)}$ is the coordinates of point $m$ in \{L}.

其中 $\bar{\boldsymbol{X}}^{L}_{(k,m)}$ 是点 $m$ 在 $\{L\}$ 中的坐标。

C. Motion Estimation

C. 运动估计

The lidar motion is modeled with constant angular and linear velocities during a sweep. This allows us to linear interpolate the pose transform within a sweep for the points that are received at different times. Let $t$ be the current time stamp, and recall that $t_{k+1}$ is the starting time of sweep $k+1$. Let $\boldsymbol{T}^{L}_{k+1}$ be the lidar pose transform between $[t_{k+1}, t]$. $\boldsymbol{T}^{L}_{k+1}$ contains rigid motion of the lidar in 6-DOF, $\boldsymbol{T}^{L}_{k+1} = [t_x, t_y, t_z, \theta_x, \theta_y, \theta_z]^{T}$, where $t_x$, $t_y$, and $t_z$ are translations along the $x$-, $y$-, and $z$- axes of $\{L\}$, respectively, and $\theta_x$, $\theta_y$, and $\theta_z$ are rotation angles, following the right-hand rule. Given a point $i$, $i \in \mathcal{P}_{k+1}$, let $t_i$ be its time stamp, and let $\boldsymbol{T}^{L}_{(k+1,i)}$ be the pose transform between $[t_{k+1}, t_i]$. $\boldsymbol{T}^{L}_{(k+1,i)}$ can be computed by linear interpolation of $\boldsymbol{T}^{L}_{k+1}$,

雷达运动被建模为在一个扫描周期内角速度和线速度保持恒定。这样就可以对在不同时刻到达的点，在扫描周期内对位姿变换做线性插值。设 $t$ 为当前时间戳，并回顾 $t_{k+1}$ 是扫描周期 $k+1$ 的起始时刻。设 $\boldsymbol{T}^{L}_{k+1}$ 为区间 $[t_{k+1}, t]$ 上的雷达位姿变换。$\boldsymbol{T}^{L}_{k+1}$ 包含雷达的六自由度刚体运动，$\boldsymbol{T}^{L}_{k+1} = [t_x, t_y, t_z, \theta_x, \theta_y, \theta_z]^{T}$，其中 $t_x$、$t_y$ 和 $t_z$ 分别是沿 $\{L\}$ 的 $x$、$y$ 和 $z$ 轴的平移，$\theta_x$、$\theta_y$ 和 $\theta_z$ 是按右手定则的转角。给定点 $i$，$i \in \mathcal{P}_{k+1}$，设 $t_i$ 为其时间戳，并设 $\boldsymbol{T}^{L}_{(k+1,i)}$ 为区间 $[t_{k+1}, t_i]$ 上的位姿变换。$\boldsymbol{T}^{L}_{(k+1,i)}$ 可由 $\boldsymbol{T}^{L}_{k+1}$ 线性插值得到，

$$\boldsymbol{T}^{L}_{(k+1,i)} = \frac{t_i - t_{k+1}}{t - t_{k+1}} \boldsymbol{T}^{L}_{k+1}. \quad (4)$$

Recall that $\mathcal{E}_{k+1}$ and $\mathcal{H}_{k+1}$ are the sets of edge points and planar points extracted from $\mathcal{P}_{k+1}$, and $\tilde{\mathcal{E}}_{k+1}$ and $\tilde{\mathcal{H}}_{k+1}$ are the sets of points reprojected to the beginning of the sweep, $t_{k+1}$. To solved the lidar motion, we need to establish a geometric relationship between $\mathcal{E}_{k+1}$ and $\tilde{\mathcal{E}}_{k+1}$, or $\mathcal{H}_{k+1}$ and $\tilde{\mathcal{H}}_{k+1}$. Using the transform in (4), we can derive,

回顾 $\mathcal{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$ 是从 $\mathcal{P}_{k+1}$ 中提取的边沿点集和平面点集，$\tilde{\mathcal{E}}_{k+1}$ 和 $\tilde{\mathcal{H}}_{k+1}$ 则是被重投影到该扫描周期起始时刻 $t_{k+1}$ 的点集。为了求解雷达运动，我们需要在 $\mathcal{E}_{k+1}$ 与 $\tilde{\mathcal{E}}_{k+1}$ 之间，或 $\mathcal{H}_{k+1}$ 与 $\tilde{\mathcal{H}}_{k+1}$ 之间建立几何关系。利用 (4) 式中的变换，可以推出

$$\boldsymbol{X}^{L}_{(k+1,i)} = \mathbf{R}\tilde{\boldsymbol{X}}^{L}_{(k+1,i)} + \boldsymbol{T}^{L}_{(k+1,i)}(1 : 3), \quad (5)$$

where $\boldsymbol{X}^{L}_{(k+1,i)}$ is the coordinates of a point $i$ in $\mathcal{E}_{k+1}$ or $\mathcal{H}_{k+1}$, $\tilde{\boldsymbol{X}}^{L}_{(k+1,i)}$ is the corresponding point in $\tilde{\mathcal{E}}_{k+1}$ or $\tilde{\mathcal{H}}_{k+1}$, $\boldsymbol{T}^{L}_{(k+1,i)}(a : b)$ is the $a$-th to $b$-th entries of $\boldsymbol{T}^{L}_{(k+1,i)}$, and $\mathbf{R}$ is a rotation matrix defined by the Rodrigues formula [25],

其中 $\boldsymbol{X}^{L}_{(k+1,i)}$ 是 $\mathcal{E}_{k+1}$ 或 $\mathcal{H}_{k+1}$ 中点 $i$ 的坐标，$\tilde{\boldsymbol{X}}^{L}_{(k+1,i)}$ 是 $\tilde{\mathcal{E}}_{k+1}$ 或 $\tilde{\mathcal{H}}_{k+1}$ 中的对应点，$\boldsymbol{T}^{L}_{(k+1,i)}(a : b)$ 是 $\boldsymbol{T}^{L}_{(k+1,i)}$ 的第 $a$ 至第 $b$ 个分量，$\mathbf{R}$ 则是由罗德里格斯公式 [25] 定义的旋转矩阵，

$$\mathbf{R} = e^{\hat{\omega}\theta} = \mathbf{I} + \hat{\omega}\sin\theta + \hat{\omega}^{2}(1 - \cos\theta). \quad (6)$$

In the above equation, $\theta$ is the magnitude of the rotation,

在上式中，$\theta$ 是转角的大小，

$$\theta = ||\boldsymbol{T}^{L}_{(k+1,i)}(4 : 6)||, \quad (7)$$

$\omega$ is a unit vector representing the rotation direction,

$\omega$ 是表示旋转方向的单位向量，

$$\omega = \boldsymbol{T}^{L}_{(k+1,i)}(4 : 6)/||\boldsymbol{T}^{L}_{(k+1,i)}(4 : 6)||, \quad (8)$$

and $\hat{\omega}$ is the skew symmetric matrix of $\omega$ [25].

而 $\hat{\omega}$ 是 $\omega$ 的反对称矩阵 [25]。

Recall that (2) and (3) compute the distances between points in $\tilde{\mathcal{E}}_{k+1}$ and $\tilde{\mathcal{H}}_{k+1}$ and their correspondences. Combining (2) and (4)-(8), we can derive a geometric relationship between an edge point in $\mathcal{E}_{k+1}$ and the corresponding edge line,

回顾 (2) 式和 (3) 式计算的是 $\tilde{\mathcal{E}}_{k+1}$ 和 $\tilde{\mathcal{H}}_{k+1}$ 中的点到其对应的距离。把 (2) 式与 (4)–(8) 式结合起来，可以推出 $\mathcal{E}_{k+1}$ 中一个边沿点与对应边线之间的几何关系，

$$f_{\mathcal{E}}(\boldsymbol{X}^{L}_{(k+1,i)}, \boldsymbol{T}^{L}_{k+1}) = d_{\mathcal{E}},\ i \in \mathcal{E}_{k+1}. \quad (9)$$

Similarly, combining (3) and (4)-(8), we can establish another geometric relationship between a planar point in $\mathcal{H}_{k+1}$ and the corresponding planar patch,

类似地，把 (3) 式与 (4)–(8) 式结合起来，可以建立 $\mathcal{H}_{k+1}$ 中一个平面点与对应平面小面片之间的几何关系，

$$f_{\mathcal{H}}(\boldsymbol{X}^{L}_{(k+1,i)}, \boldsymbol{T}^{L}_{k+1}) = d_{\mathcal{H}},\ i \in \mathcal{H}_{k+1}. \quad (10)$$

Finally, we solve the lidar motion with the Levenberg-Marquardt method [26]. Stacking (9) and (10) for each feature point in $\mathcal{E}_{k+1}$ and $\mathcal{H}_{k+1}$, we obtain a nonlinear function,

最后，我们用 Levenberg-Marquardt 方法 [26] 求解雷达运动。把 $\mathcal{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$ 中每个特征点的 (9) 式和 (10) 式堆叠起来，得到一个非线性函数，

$$\boldsymbol{f}(\boldsymbol{T}^{L}_{k+1}) = \boldsymbol{d}, \quad (11)$$

where each row of $\boldsymbol{f}$ corresponds to a feature point, and $\boldsymbol{d}$ contains the corresponding distances. We compute the Jacobian matrix of $\boldsymbol{f}$ with respect to $\boldsymbol{T}^{L}_{k+1}$, denoted as $\mathbf{J}$, where $\mathbf{J} = \partial\boldsymbol{f}/\partial\boldsymbol{T}^{L}_{k+1}$. Then, (11) can be solved through nonlinear iterations by minimizing $\boldsymbol{d}$ toward zero,

其中 $\boldsymbol{f}$ 的每一行对应一个特征点，$\boldsymbol{d}$ 则包含相应的距离。我们计算 $\boldsymbol{f}$ 关于 $\boldsymbol{T}^{L}_{k+1}$ 的雅可比矩阵，记为 $\mathbf{J}$，其中 $\mathbf{J} = \partial\boldsymbol{f}/\partial\boldsymbol{T}^{L}_{k+1}$。然后，通过非线性迭代把 $\boldsymbol{d}$ 向零最小化，即可求解 (11) 式，

$$\boldsymbol{T}^{L}_{k+1} \leftarrow \boldsymbol{T}^{L}_{k+1} - \bigl(\mathbf{J}^{T}\mathbf{J} + \lambda\mathrm{diag}(\mathbf{J}^{T}\mathbf{J})\bigr)^{-1}\mathbf{J}^{T}\boldsymbol{d}. \quad (12)$$

$\lambda$ is a factor determined by the Levenberg-Marquardt method.

$\lambda$ 是由 Levenberg-Marquardt 方法确定的因子。

Algorithm 1: Lidar Odometry

1 input : $\bar{\mathcal{P}}_k$, $\mathcal{P}_{k+1}$, $\boldsymbol{T}^{L}_{k+1}$ from the last recursion
2 output : $\bar{\mathcal{P}}_{k+1}$, newly computed $\boldsymbol{T}^{L}_{k+1}$
3 begin
4 　if at the beginning of a sweep then
5 　　$\boldsymbol{T}^{L}_{k+1} \leftarrow \boldsymbol{0}$;
6 　end
7 　Detect edge points and planar points in $\mathcal{P}_{k+1}$, put the points in $\mathcal{E}_{k+1}$ and $\mathcal{H}_{k+1}$, respectively;
8 　for a number of iterations do
9 　　for each edge point in $\mathcal{E}_{k+1}$ do
10 　　　Find an edge line as the correspondence, then compute point to line distance based on (9) and stack the equation to (11);
11 　　end
12 　　for each planar point in $\mathcal{H}_{k+1}$ do
13 　　　Find a planar patch as the correspondence, then compute point to plane distance based on (10) and stack the equation to (11);
14 　　end
15 　　Compute a bisquare weight for each row of (11);
16 　　Update $\boldsymbol{T}^{L}_{k+1}$ for a nonlinear iteration based on (12);
17 　　if the nonlinear optimization converges then
18 　　　Break;
19 　　end
20 　end
21 　if at the end of a sweep then
22 　　Reproject each point in $\mathcal{P}_{k+1}$ to $t_{k+2}$ and form $\bar{\mathcal{P}}_{k+1}$;
23 　　Return $\boldsymbol{T}^{L}_{k+1}$ and $\bar{\mathcal{P}}_{k+1}$;
24 　end
25 　else
26 　　Return $\boldsymbol{T}^{L}_{k+1}$;
27 　end
28 end

算法 1： 激光雷达里程计

1 输入：上一轮递归得到的 $\bar{\mathcal{P}}_k$，$\mathcal{P}_{k+1}$，$\boldsymbol{T}^{L}_{k+1}$
2 输出：$\bar{\mathcal{P}}_{k+1}$，新计算的 $\boldsymbol{T}^{L}_{k+1}$
3 begin
4 　if 处于一个扫描周期的起始时刻 then
5 　　$\boldsymbol{T}^{L}_{k+1} \leftarrow \boldsymbol{0}$;
6 　end
7 　在 $\mathcal{P}_{k+1}$ 中检测边沿点和平面点，分别放入 $\mathcal{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$；
8 　for 若干次迭代 do
9 　　for $\mathcal{E}_{k+1}$ 中的每个边沿点 do
10 　　　寻找一条边线作为对应，再根据 (9) 式计算点到直线的距离，并把该方程堆叠到 (11) 式；
11 　　end
12 　　for $\mathcal{H}_{k+1}$ 中的每个平面点 do
13 　　　寻找一块平面小面片作为对应，再根据 (10) 式计算点到平面的距离，并把该方程堆叠到 (11) 式；
14 　　end
15 　　为 (11) 式的每一行计算一个双平方权；
16 　　根据 (12) 式更新 $\boldsymbol{T}^{L}_{k+1}$，完成一次非线性迭代；
17 　　if 非线性优化已经收敛 then
18 　　　Break;
19 　　end
20 　end
21 　if 到达一个扫描周期的结束时刻 then
22 　　把 $\mathcal{P}_{k+1}$ 中的每个点重投影到 $t_{k+2}$，形成 $\bar{\mathcal{P}}_{k+1}$；
23 　　返回 $\boldsymbol{T}^{L}_{k+1}$ 和 $\bar{\mathcal{P}}_{k+1}$；
24 　end
25 　else
26 　　返回 $\boldsymbol{T}^{L}_{k+1}$；
27 　end
28 end

D. Lidar Odometry Algorithm

D. 激光雷达里程计算法

The lidar odometry algorithm is shown in Algorithm 1. The algorithm takes as inputs the point cloud from the last sweep, $\bar{\mathcal{P}}_k$, the growing point cloud of the current sweep, $\mathcal{P}_{k+1}$, and the pose transform from the last recursion, $\boldsymbol{T}^{L}_{k+1}$. If a new sweep is started, $\boldsymbol{T}^{L}_{k+1}$ is set to zero (line 4-6). Then, the algorithm extracts feature points from $\mathcal{P}_{k+1}$ to construct $\mathcal{E}_{k+1}$ and $\mathcal{H}_{k+1}$ in line 7. For each feature point, we find its correspondence in $\bar{\mathcal{P}}_k$ (line 9-19). The motion estimation is adapted to a robust fitting [27]. In line 15, the algorithm assigns a bisquare weight for each feature point. The feature points that have larger distances to their correspondences are assigned with smaller weights, and the feature points with distances larger than a threshold are considered as outliers and assigned with zero weights. Then, in line 16, the pose transform is updated for one iteration. The nonlinear optimization terminates if convergence is found, or the maximum iteration number is met. If the algorithm reaches the end of a sweep, $\mathcal{P}_{k+1}$ is reprojected to time stamp $t_{k+2}$ using the estimated motion during the sweep. Otherwise, only the transform $\boldsymbol{T}^{L}_{k+1}$ is returned for the next round of recursion.

激光雷达里程计算法如算法 1 所示。算法的输入是上一扫描周期的点云 $\bar{\mathcal{P}}_k$、当前扫描周期中正在增长的点云 $\mathcal{P}_{k+1}$，以及上一轮递归得到的位姿变换 $\boldsymbol{T}^{L}_{k+1}$。若新的扫描周期刚开始，则把 $\boldsymbol{T}^{L}_{k+1}$ 置为零（第 4–6 行）。随后，算法在第 7 行从 $\mathcal{P}_{k+1}$ 中提取特征点，构造 $\mathcal{E}_{k+1}$ 和 $\mathcal{H}_{k+1}$。对每个特征点，我们在 $\bar{\mathcal{P}}_k$ 中寻找其对应（第 9–19 行）。运动估计采用稳健拟合 [27]。在第 15 行，算法为每个特征点赋予一个双平方权：到其对应距离较大的特征点获得较小的权，距离超过阈值的特征点被视为离群点并赋零权。然后，在第 16 行对位姿变换做一次迭代更新。非线性优化在发现收敛、或达到最大迭代次数时终止。若算法到达一个扫描周期的结束时刻，则用该周期内估计得到的运动，把 $\mathcal{P}_{k+1}$ 重投影到时间戳 $t_{k+2}$。否则，只返回变换 $\boldsymbol{T}^{L}_{k+1}$，供下一轮递归使用。

VI. LIDAR MAPPING

VI. 激光雷达建图

The mapping algorithm runs at a lower frequency then the odometry algorithm, and is called only once per sweep. At the end of sweep $k + 1$, the lidar odometry generates a undistorted point cloud, $\bar{\mathcal{P}}_{k+1}$, and simultaneously a pose transform, $\boldsymbol{T}^{L}_{k+1}$, containing the lidar motion during the sweep, between $[t_{k+1}, t_{k+2}]$. The mapping algorithm matches and registers $\bar{\mathcal{P}}_{k+1}$ in the world coordinates, $\{W\}$, illustrated in Fig. 8. To explain the procedure, let us define $\mathcal{Q}_k$ as the point cloud on the map, accumulated until sweep $k$, and let $\boldsymbol{T}^{W}_{k}$ be the pose of the lidar on the map at the end of sweep $k$, $t_{k+1}$. With the outputs from the lidar odometry, the mapping algorithm extents $\boldsymbol{T}^{W}_{k}$ for one sweep from $t_{k+1}$ to $t_{k+2}$, to obtain $\boldsymbol{T}^{W}_{k+1}$, and projects $\bar{\mathcal{P}}_{k+1}$ into the world coordinates, $\{W\}$, denoted as $\bar{\mathcal{Q}}_{k+1}$. Next, the algorithm matches $\bar{\mathcal{Q}}_{k+1}$ with $\mathcal{Q}_k$ by optimizing the lidar pose $\boldsymbol{T}^{W}_{k+1}$.

建图算法的运行频率比里程计算法低一个档次，每个扫描周期只调用一次。在扫描周期 $k+1$ 结束时，激光雷达里程计生成一份去扭曲的点云 $\bar{\mathcal{P}}_{k+1}$，同时给出位姿变换 $\boldsymbol{T}^{L}_{k+1}$，其中包含该周期内、区间 $[t_{k+1}, t_{k+2}]$ 上的雷达运动。建图算法把 $\bar{\mathcal{P}}_{k+1}$ 匹配并配准到世界坐标系 $\{W\}$ 中，如图 8 所示。为说明这一过程，我们把截至扫描周期 $k$ 累积在地图上的点云记为 $\mathcal{Q}_k$，并把扫描周期 $k$ 结束时刻 $t_{k+1}$ 雷达在地图上的位姿记为 $\boldsymbol{T}^{W}_{k}$。利用里程计的输出，建图算法把 $\boldsymbol{T}^{W}_{k}$ 从 $t_{k+1}$ 延伸一个扫描周期到 $t_{k+2}$，得到 $\boldsymbol{T}^{W}_{k+1}$，再把 $\bar{\mathcal{P}}_{k+1}$ 投影到世界坐标系 $\{W\}$ 中，记为 $\bar{\mathcal{Q}}_{k+1}$。随后，算法通过优化雷达位姿 $\boldsymbol{T}^{W}_{k+1}$，把 $\bar{\mathcal{Q}}_{k+1}$ 与 $\mathcal{Q}_k$ 匹配。

Fig. 8. Illustration of mapping process. The blue colored curve represents the lidar pose on the map, $\boldsymbol{T}^{W}_{k}$, generated by the mapping algorithm at sweep $k$. The orange color curve indicates the lidar motion during sweep $k+1$, $\boldsymbol{T}^{L}_{k+1}$, computed by the odometry algorithm. With $\boldsymbol{T}^{W}_{k}$ and $\boldsymbol{T}^{L}_{k+1}$, the undistorted point cloud published by the odometry algorithm is projected onto the map, denoted as $\bar{\mathcal{Q}}_{k+1}$ (the green colored line segments), and matched with the existing cloud on the map, $\mathcal{Q}_k$ (the black colored line segments).

图 8. 建图过程示意。蓝色曲线表示建图算法在扫描周期 $k$ 生成的、雷达在地图上的位姿 $\boldsymbol{T}^{W}_{k}$。橙色曲线表示里程计算法算出的、扫描周期 $k+1$ 内的雷达运动 $\boldsymbol{T}^{L}_{k+1}$。利用 $\boldsymbol{T}^{W}_{k}$ 和 $\boldsymbol{T}^{L}_{k+1}$，把里程计算法发布的去扭曲点云投影到地图上，记为 $\bar{\mathcal{Q}}_{k+1}$（绿色线段），并与地图上已有的点云 $\mathcal{Q}_k$（黑色线段）相匹配。

The feature points are extracted in the same way as in Section V-A, but 10 times of feature points are used. To find correspondences for the feature points, we store the point cloud on the map, $\mathcal{Q}_k$, in 10m cubic areas. The points in the cubes that intersect with $\bar{\mathcal{Q}}_{k+1}$ are extracted and stored in a 3D KD-tree [24]. We find the points in $\mathcal{Q}_k$ within a certain region around the feature points. Let $\mathcal{S}'$ be a set of surrounding points. For an edge point, we only keep points on edge lines in $\mathcal{S}'$, and for a planar point, we only keep points on planar patches. Then, we compute the covariance matrix of $\mathcal{S}'$, denoted as $\mathbf{M}$, and the eigenvalues and eigenvectors of $\mathbf{M}$, denoted as $\mathbf{V}$ and $\mathbf{E}$, respectively. If $\mathcal{S}'$ is distributed on an edge line, $\mathbf{V}$ contains one eigenvalue significantly larger than the other two, and the eigenvector in $\mathbf{E}$ associated with the largest eigenvalue represents the orientation of the edge line. On the other hand, if $\mathcal{S}'$ is distributed on a planar patch, $\mathbf{V}$ contains two large eigenvalues with the third one significantly smaller, and the eigenvector in $\mathbf{E}$ associated with the smallest eigenvalue denotes the orientation of the planar patch. The position of the edge line or the planar patch is determined by passing through the geometric center of $\mathcal{S}'$.

特征点的提取方式与第 V-A 节相同，但使用的特征点数量约为原来的 10 倍。为了给这些特征点寻找对应，我们把地图上的点云 $\mathcal{Q}_k$ 按 10m 边长的立方区域存放。与 $\bar{\mathcal{Q}}_{k+1}$ 相交的立方体中的点被取出，并存入一棵三维 KD 树 [24]。我们在 $\mathcal{Q}_k$ 中寻找特征点周围一定区域内的点。设 $\mathcal{S}'$ 为周围点的集合。对边沿点，我们只在 $\mathcal{S}'$ 中保留位于边线上的点；对平面点，则只保留位于平面小面片上的点。随后，计算 $\mathcal{S}'$ 的协方差矩阵，记为 $\mathbf{M}$，以及 $\mathbf{M}$ 的特征值和特征向量，分别记为 $\mathbf{V}$ 和 $\mathbf{E}$。若 $\mathcal{S}'$ 分布在一条边线上，$\mathbf{V}$ 中有一个特征值明显大于另外两个，与最大特征值相对应的、$\mathbf{E}$ 中的特征向量表示这条边线的方向。另一方面，若 $\mathcal{S}'$ 分布在一块平面小面片上，$\mathbf{V}$ 中有两个较大的特征值，第三个则明显更小，与最小特征值相对应的特征向量给出该小面片的法向。边线或小面片的位置由通过 $\mathcal{S}'$ 几何中心来确定。

To compute the distance from a feature point to its correspondence, we select two points on an edge line, and three points on a planar patch. This allows the distances to be computed using the same formulations as (2) and (3). Then, an equation is derived for each feature point as (9) or (10), but different in that all points in $\bar{\mathcal{Q}}_{k+1}$ share the same time stamp, $t_{k+2}$. The nonlinear optimization is solved again by a robust fitting [27] through the Levenberg-Marquardt method [26], and $\bar{\mathcal{Q}}_{k+1}$ is registered on the map. To evenly distribute the points, the map cloud is downsized by a voxel grid filter [28] with the voxel size of 5cm cubes.

为计算特征点到其对应的距离，我们在边线上取两个点，在平面小面片上取三个点。这样就可以用与 (2) 式和 (3) 式相同的公式来计算距离。随后，为每个特征点写出形如 (9) 式或 (10) 式的方程，不同之处在于 $\bar{\mathcal{Q}}_{k+1}$ 中的所有点共享同一个时间戳 $t_{k+2}$。非线性优化同样采用稳健拟合 [27]，通过 Levenberg-Marquardt 方法 [26] 求解，并把 $\bar{\mathcal{Q}}_{k+1}$ 配准到地图上。为使点的分布更均匀，用边长为 5cm 的体素栅格滤波器 [28] 对地图点云降采样。