Sensor Aware Lidar Odometry

TL;DR

This paper introduces a lidar odometry approach that incorporates sensor physics and measurement error modeling, achieving high accuracy and outperforming existing methods on standard benchmarks.

Contribution

It presents a novel lidar odometry method that models sensor measurement errors and uses beam adjacency for outlier rejection, improving accuracy over prior techniques.

Findings

Ranked 1.37% positioning error on KITTI leaderboard

Achieves 3.67% improvement over LOAM on internal dataset

Incorporates sensor physics into odometry estimation

Abstract

A lidar odometry method, integrating into the computation the knowledge about the physics of the sensor, is proposed. A model of measurement error enables higher precision in estimation of the point normal covariance. Adjacent laser beams are used in an outlier correspondence rejection scheme. The method is ranked in the KITTI's leaderboard with 1.37% positioning error. 3.67% is achieved in comparison with the LOAM method on the internal dataset.

Tables

Table 1. TABLE I: Mean Relative Positioning Error per 100 m

Training set of KITTI
Method	00		01		02		03		04		05		06		07		08		09		10		Overall
	$\mu \text{, m}$	$\sigma \text{, m}$	$\mu \text{, m}$	$\sigma \text{, m}$	$\mu \text{, m}$	$\sigma \text{, m}$	$\mu \text{, m}$	$\sigma \text{, m}$	$\mu \text{, m}$	$\sigma \text{, m}$	$\mu \text{, m}$	$\sigma \text{, m}$	$\mu \text{, m}$	$\sigma \text{, m}$	$\mu \text{, m}$	$\sigma \text{, m}$	$\mu \text{, m}$	$\sigma \text{, m}$	$\mu \text{, m}$	$\sigma \text{, m}$	$\mu \text{, m}$	$\sigma \text{, m}$	$\mu \text{, m}$	$\sigma \text{, m}$
LOAM	$2.71$	$2.39$	$32.64$	$40.19$	$5.83$	$14.16$	$2.30$	$1.33$	$1.12$	$0.70$	$2.02$	$1.64$	$2.53$	$2.63$	$2.75$	$2.13$	$3.05$	$2.47$	$2.18$	$1.46$	$2.43$	$1.90$	$4.68$	$12.78$
BL	$1.12$	$0.73$	$4.73$	$8.15$	$1.18$	$0.53$	$1.54$	$0.44$	$0.79$	$0.17$	$0.82$	$0.34$	$0.60$	$0.16$	$1.06$	$0.47$	$1.49$	$1.44$	$0.92$	$0.42$	$1.20$	$0.43$	$1.31$	$2.10$
NCF	$0.95$	$0.71$	$2.39$	$6.28$	$0.99$	$0.46$	$1.64$	$0.45$	$0.58$	$0.17$	$0.59$	$0.40$	$0.51$	$0.14$	$0.76$	$0.36$	$1.32$	$1.43$	$0.67$	$0.25$	$0.95$	$0.38$	$1.02$	$1.61$
SALO	$0.91$	$0.72$	$1.13$	$0.37$	$0.98$	$0.45$	$1.76$	$0.50$	$0.51$	$0.17$	$0.56$	$0.29$	$0.48$	$0.13$	$0.83$	$0.51$	$1.33$	$1.43$	$0.64$	$0.30$	$0.97$	$0.41$	$0.95$	$0.80$

Equations

$$\widetilde{X_i}={\cup }_{k=0}^N{p}_k{X}_i={\cup }_{k=0}^M(p_k,n_k,\text{Cov}[n_k])$$

$$\text{Cov}[V_2]=J^T\text{Cov}[D]JJ=\frac{\partial f(D)}{\partial D}(D)$$

$$J^{i,j}=\frac{\partial V_2}{\partial D_{i,j}}(D)J=\xi \sum _{i,j}^{k,3}{J}^{i,j}{J^{i,j}}^T$$

$${\Omega }_{V_2}^{i,j}=\left[\begin{array}{c}{\omega }_0\\ {\omega }_1\\ 0\end{array}\right]{\omega }_l=C_l\left[\begin{array}{c}[U^T{\Delta }^{(i,j)}V{]}_{l,2}\\ [U^T{\Delta }^{(i,j)}V{]}_{2,l}\end{array}\right]$$

$$J^{i,j}=-V{\Omega }_{V_2}^{i,j},$$

$$\text{Cov}[V_2]=QC{Q}^{-1}$$

$$||\widetilde{m_k}||<\underset{l\in \{0\dots 3\}}{\max }d(p_k,p_l)$$

$$\begin{array}{rl}& p_k^{\text{ground}}=[{p_k}_x{p_k}_{y}0{]}^{T}u=p_k\times p_k^{\text{ground}}v=p_k\times u\\ & p_{k,l}=[\varphi \cdot u\pm {\theta }_{j\pm 1,j}\cdot v]\cdot ||p_k||\end{array}$$

$$d_{k,l}=\frac{||p_{k,l}|{|}^2}{\sqrt{||p_{k,l}|{|}^2-||⟨p_{k,l},n_p⟩|{|}^2}}$$

Full text

††publicationid: pubid: 978-1-7281-3605-9/19/$31.00 ©2019 IEEE

1 Introduction

The driverless transportation is becoming a technological dream on a verge of turning a day-to-day reality, as more competitors are joining to [1] the pursuit. Early prototypes appeared decades ago [2], while DARPA Urban Challenge could be considered an important milestone. AV (Autonomous Vehicle), hardware and models of the victorious team are highlighted in [3] with an emphasis on the crucial role that a lidar plays in providing that level of autonomy. The comprehensive analysis [4] of a low-speed collision between MIT’s AV "Talos" and Cornell’s AV "Skynet" occured during the challenge puts the positioning error as a one of causes. The lidar odometry is a subject of this work and our main contributions are:

• •

point cloud filtering by the covariance of point normal, informed of the measurement error, specific for a lidar sensor

• •

scheme for the false matches rejection between consecutive point clouds, based on a neighbor beam distance

• •

reliable performance in the realistic environment is demonstrated on KITTI [5] and on the internal dataset, collected from the autonomous fleet facing intense modes of operation every day (Fig. 1) in comparison with LOAM, state-of-the-art competitor [6]

2 Related work

Driver-assist systems (autonomy level $\leq$ 3 [7]) tend to avoid 3D SLAM (Simultaneous Localization and Mapping) by reliance on GNSS, odometry and the lane detection. However, level 4 AVs often benefit from data stored in a HD map. This study is concerned with a level 4 setup, which dictates the separation of localization and mapping because doing the latter repetitively on an every car in a fleet in a same location is wasteful.

A substantial body of reseach exists on a problem of egomotion estimation and SLAM, yet the application to AVs imposes an unique set of challenges, as the environment is:

• •

unprepared for autonomous navigation and is unfeasible to be retrofitted with markers

• •

feature-poor: highways, tunnels, wastelands

• •

dynamic: vegetation growth, snowbanks accumulation, construction works

• •

hard to perceive: the weather, time of a day and an annual season change influence

• •

rich with occlusions of background features by other vehicles

The degenerative effect of dense urbanisation on the SLAM performance was shown in [8]. This challenge is addressed by graph optimization [9], achieving the map global consistency. However, to reduce the computation time and decrease the chance of convergence to a suboptimal local minima, the better maps for optimization are called for. The input maps are built with odometry, which is the objective of our study.

The comparison [10] of lidar SLAM with the visual counterparts favours the former. The top of the KITTI’s leaderboard is also occupied by lidar methods. Laser range measurements tend to show a higher reliability and lower dependence on luminance than the depth measurements produced by cameras, which drives our focus on a lidar.

The seminal contribution is Iterative Closest Point [11], a method to register a relative 3D transformation between point clouds of arbitrary shape, based on the least squares. The closest point correspondence estimation is replaced by a grid lookup, and Euclidian loss is replaced by Mahalanobis in Normal Distributions Transform [12]. Novel learning-based methods are proposed [13], [14], but do not perform on par yet. Reflectance intensity images with depth are reconstructed from range scans in [15] and then used as an input for the visual odometry algorithm. Contrary to that, many works [16], [17],  [18],  [19] rely solely on the geometric properties of registered laser point clouds.

The continuous time was found benefitial by H. Alismail et al in [20] as it allows to solve simultaneously and explicitely for the pose and the point cloud drift, which happens due to an egomotion during the sensor spin time. The proposed method does not estimate the point cloud drift correction, as the wheel odometry data could be employed for that.

Optimal registration algorithms [21] guarantee convergence without initialization, but are prohibitively expensive to compute. The landmark-based registration is advocated in [22] on grounds of decreasing a SLAM graph density. In spite of this valid concern our method operates directly on point clouds, because the dense approach provides a greater robustness in feature-poor environments.

Experimental results are evaluated against an another feature-based approach, LOAM [6], which holds the top result on KITTI’s leaderboard111http://www.cvlibs.net/datasets/kitti/eval_odometry.php at the time of writing this paper. LOAM makes use of two types of features (corner-like and plane-like) associated with dedicated losses (point-to-line, point-to-plane). These point-to-plane losses may include outlier entries because plane normal estimates are computed with three points each and could contain noise, while the proposed method estimates normals robutly and filters point clouds by normal covariances.

The rest of the paper is organized as follows: Sec. 3 introduces the mathematical model of the proposed method, experimental results are layed down in Sec. 4, then Sec. 5 concludes with the summary and further work.

3 Lidar Odometry

The odometry task could be stated formally as the problem of determining a relative rigid-body 3D transformation $T_{i}^{i-1}\in SE(3)$, which brings the sensor coordinate frame at a time of the current range scan (source point cloud $\widetilde{X}_{i}$) registration to the sensor coordinate frame at a time of the previous range scan (target point cloud $\widetilde{X}_{i-1}$) registration.

The proposed method is built on the ICP framework [11]. We proceed with detailing specific choices made for every subroutine mentioned in Alg. 1 without a dive into the discussion of available alternatives, which are covered by the excellent review [23] and the best-practices for choosing among them [24].

Initialization of the transformation (initial guess) practically is derived from the independent sensors available: IMU, wheel odometry. However, this work considers the lidar-only odometry and uses the linear extrapolation from past states for initialization.

Filtration of Points is crucial as the data rate of a modern lidar would hinder the computational performance. Depending on the lidar model and the configuration of the environment 30K-150K points are registered within one range scan $\widetilde{X}_{i}$, while only $5-15\%$ are passed on through the filter (Alg. 1, line 1) to the model as the point cloud $X_{i}$. The filtration steps are:

• •

voxel grid filter, passing on centroids of occupied voxels

• •

normal estimation and normal covariance filtration, covered in detail in Sec. 3.1

• •

local curvature filter: points with a higher curvature are discarded (estimated as in [25])

Matching of Points is done with a k-d tree, selecting a closest point in the target cloud. These preliminary correspondences undergo the rejection of outliers:

• •

geometric correspondence rejector, highlighted in Sec. 3.2

• •

remove $20\%$ of matches having the largest distance (introduced by D. Chetverikov et al in [26])

Transformation Estimation is performed by the uniformly weighted linearized least squares optimization with the point-to-plane metric, which was shown to be generally superior to point-to-point by F. Pomerleau et al in [24].

Termination Criteria is a combination of thresholds on the absolute and relative magnitude of transformation increment $\Delta$ with the threshold on an iterations count.

With that the ICP framework is defined and the proposed models for point filtration and matching follow.

3.1 Normal Covariance Filtration

The filter introduced in Alg. 1, line 1, takes a raw range scan $\widetilde{X}_{i}$ and outputs $X_{i}$, where each point has an estimated normal associated with it, as well as the normal covariance:

[TABLE]

$M\leq N$, as the points having high-uncertainty normals are rejected. The reason is miscalculated normals impair transformation estimation precision, as individual losses would be off.

The normal $n_{k}$ is estimated using SVD (Singular Value Decomposition) in a $k$-neighborhood of the point $p_{k}$, so when a substantial fraction of points is sampled from an another surface, which could happen in many boundary cases, SVD would yeild a normal vector, that is not describing a true plane well. The higher values of the last singular value are indicative of the lower confidence in the normal orientation. However, such a model discounts the fact that the point coordinates are not certain and are subject to the measurement error.

We propose a normal covariance filter (NCF), which accounts for the uncertainty in point coordinates, treating it as Gaussian random variables. The measurement error is approximated by a sphere with the standard deviation $\xi$.

For the notation clarity, until the rest of the section the lower indices are used (as in $D_{i,j}$) to access a matrix entry and a single lower index to denote a matrix column $D_{i}$, counting indices from [math].

The SVD input is an array of unbiased local points, sampled around $p_{k}$ from $\widetilde{X}_{i}$, i.e. the data matrix $D\in\mathbb{R}^{\{k\times 3\}}$. The SVD output is $D=USV$, where $U\in\mathbb{R}^{\{k\times 3\}}$, $S\in\mathbb{R}^{\{3\times 3\}}$, $\forall i,j:\{i\neq j;i,j<3;S_{i,j}=0\}$, $V\in\mathbb{R}^{\{3\times 3\}}$ and $U$, $V$ are orthogonal.

When entries of $D$ are random variables, $V_{2}$ is as well. The covariance of the plane normal is estimated by the propagation of uncertainty technique:

[TABLE]

The data matrix covariance was informally introduced above and is $\text{Cov}[D]=\xi\cdot I^{\{3\times 3\}}$. The function $f$ is a sequence of computations, yielding $V_{2}$ from $D$. To avoid the appearance of 3-dimensional Jacobian matrix, $J$ is reformulated w.r.t. $D_{i,j}$, simplified further, given the form of $\text{Cov}[D]$:

[TABLE]

Then the intermediate matrix $\Omega^{i,j}_{V_{2}}\in\mathbb{R}^{\{3\times 1\}}$ is defined:

[TABLE]

where $l\in\{0,1\}$, $C_{l}=\frac{1}{S_{l,l}^{2}-S_{2,2}^{2}}\cdot\begin{bmatrix}S_{l,l}&S_{2,2}\end{bmatrix}$ and $\Delta^{(i,j)}$ defines a matrix, having zeros everywhere except $(i,j)$-th element. With Eq. 3 the Jacobian w.r.t. to $D_{i,j}$ is obtained:

[TABLE]

For the full derivation of the SVD Jacobian please refer to [27],  [28].

Recovered in Eq. 1, $\text{Cov}[V_{2}]$ lies in the sensor coordinate frame, hence it should be rotated into an alignment with $V_{2}$ before the threshold may be applied:

[TABLE]

The matrix $C$ is a covariance of $V_{2}$ in a normal-aligned coordinate frame computed in Eq. 5 through eigendecomposition. Having $C$ for an each point in a range scan $\widetilde{X}_{i}$, the points associated with the higher normal uncertainty are discarded: $C_{2,2}\geq c_{\tau}$. The filtration result is a range scan $X_{i}$ with precise normals, utilized on later stages of the ICP framework.

3.2 Geometric Correspondence Rejector

For a current range scan $X_{i}$ the set of correspondences $\widetilde{M}$ is found by a search in the previous range scan $X_{i-1}$ (Alg. 1, line 5). The proposed model yields filtered correspondences $M=\{\forall\widetilde{m}_{k}\in\widetilde{M}:\text{isInlier}(p_{k},p^{\prime}_{k},\tilde{m}_{k})\}$, where $p_{k}\in X_{i}$ is a point in the current range scan, participating in the correspondence $\widetilde{m}_{k}$ with a point $p^{\prime}_{k}\in X_{i-1}$ from the previous range scan.

The test isInlier is conducted independently for an every correspondence $\widetilde{m}_{k}$, characterized by the Euclidian distance between matched points: $||\widetilde{m}_{k}||=||p_{k}-p^{\prime}_{k}||$. The test description follows:

[TABLE]

The matching distance $||\widetilde{m}_{k}||$ in compared to an upper bound of neighbor beam distances, $d(p_{k},p_{l})$ or, shortly, $d_{k,l}$, highlighted red in Fig. 2.

The neighbors of $p_{k}$, $p_{l}$, $l\in\{0\dots 3\}$ are four hypothetical points, which might have been registered by lasers, vertically adjacent to the one registering $p_{k}$. Conventionally for spinning lidars, these laser levels are called rings, so if $p_{k}$ is a part of the ring $r_{j}$, then it’s neighbors $p_{l}$ are on the adjacent rings $r_{j-1}$, $r_{j+1}$, shown in purple and orange in Fig. 2.

The model introduces two neighbors on an each of the adjacent rings assuming that "left" neighbors were registered at a previous increment of lidar azimutal rotation, $p_{k}$ at a current, and "right" neighbors at a next one.

For the calibrated lidar, all angular distances between $p_{k}$ and it’s neighbors $p_{l}$ are easily found. The calibration parameters are: $\phi$ (an angular increment of lidar azimutal rotation), $\theta_{j-1,j}$, $\theta_{j,j+1}$ are angular distances in pitch between rings.

Hence, $p_{k}$ with it’s neighbors $p_{l}$ define a segment of the sphere. This segment could be approximated by a gray plane on Fig. 2, in turn assuming neighbor lidar beams parallel. This assumption was found admissable for lidars in our use, for the hardware parameters are close to [math]: $\phi\approx 0.08^{\circ}$, $\theta\approx 0.26^{\circ}$.

The diagonal vector $p_{k,l}$ between $p_{k}$ and $p_{l}$ on the approximated plane (shown in cyan in Fig. 2) is obtained as follows:

[TABLE]

The vectors $u$, $v$ define absciss and ordinate axes of $p_{k,l}$ coordinate frame. With $p_{k,l}$ proceed to the neighbor beam distance:

[TABLE]

where $d_{k,l}$ is a projection of $p_{k,l}$ onto the reflecting surface orthogonal to $n_{k}$ (green surface in Fig. 2) along the laser beam flight direction. Matches having $||\widetilde{m_{k}}||$ higher than that are probably made with points laying even further away from $p_{k}$ than $p_{l}$ and should be discarded.

4 Experiments

The thorough evaluation of the proposed method is provided. Two parts of this exist: the dedicated study of the geometric correspondence rejector and the integrated test of the proposed lidar odometry. The former is a comparison of the proposed rejector with the Euclidian distance rejector while the latter is a test with three ICP odometry variants and the competitor apporach, LOAM, on two real-world AV benchmarks, renown KITTI and the internal dataset, taken during Yandex own AV fleet operation.

4.1 Outlier Correspondence Rejection

The geometric correspondence rejector (GCR) is evaluated against the baseline, which is the Euclidian distance rejector. The test is run on $100$ point cloud pairs from the internal dataset. The ground truth for the test is relative transformations between pairs, refined by the graph optimization.

In an every test instance the initial guess is the composition of the ground truth transformation with a random noise. The method’s ability to recover the correct transformation is evaluated for several noise levels. The proposed method have shown the performance, superior to the baseline, as in Fig. 3. Only distributions of translation estimation errors are included as the rotation errors were found to be very correlated with it.

The noise transformation have two parameters $l_{r}$, $l_{t}$ (magnitude limits in rotation and translation) and is sampled as follows: generate two random unit vectors on a sphere, where the former is the rotation axis $v_{r}$ and the latter is the translation axis $v_{l}$, rotate for $m_{r}\sim U[-l_{r}\dots l_{r}]$ about $v_{r}$ and translate for $m_{t}\sim U[-l_{t}\dots l_{t}]$ along $v_{t}$.

4.2 Lidar Odometry Evaluation

The study of the fully integrated odometry method follows with the baseline (BL), where the Euclidian distance rejector is used within the ICP framework, described in Alg. 1 along with the common approach for points filtration, following Sec. 3.

The baseline method is used as a control in a performance evaluation of LOAM, the proposed method (SALO) and an intermediate variant, integrating NCF into BL. GCR was not evaluated separately from NCF, because it relies on the quality of point cloud normals, which is ensured by NCF.

The internal Yandex dataset of total length of 16 minutes, 12 km was compiled for this study. It was taken in different cities and seasons of the year with a 32-beam lidar, mid-grade single-band GNSS with RTK. GNSS signal frequency was increased by the Kalman filter interpolation with IMU and wheel odometry to be used as the ground truth in this evaluation. The removal of dynamic objects from lidar scans was not performed.

Detailed results for all three methods and the competitor are showcased in Tab. I with trajectory visualizations in Fig. 4.

A. Geiger argues in [5] that the absolute metric accounts errors non-uniformly based on a time passed since the start of the sequence. We adhere to KITTI’s standard and use the same metric and the official devkit in all evaluations, with a minor change: the reference pose is set $100$ metres away instead of averaging across $50$, $100$ …$800$ to foster the results interpretability. The other motivation for that is space constraints and we may suggest readers to refer to our submission on the benchmark website [5] for more results.

Along with error averages, standard deviations are listed and it may be noted that the lower mean error always coincides with the lower deviation.

It was shown before [29] and is confirmed now on the internal dataset and KITTI (Fig. 4.d) that the community-supported implementation of LOAM [30] does not perform accordingly to the version, competing in the KITTI’s leaderboard and tends to degrade in many cases. Contrary to that, the proposed method produces a trajectory close to the ground truth, accumulating little drift over 7 minutes (Fig. 4.d).

The proposed method achieves 27% and 45% drift reduction on KITTI and internal dataset respectively along with an even stronger error variance reduction.

The avarage processing time of a point cloud is $0.6$ s. Studied variants do not vary substantially in computational cost, which mostly comes from the normal estimation.

5 Conclusions

The MIT/Cornell collision during DARPA Grand Urban Challenge is the first AV road incident, but regretfully would not be the last. We share our experience in designing odometry solution for self-driving car hoping to foster safety and reliabilty of the autonomous fleets across the globe. In this work ICP lidar odometry performance boosted by the normal covariance point cloud filter and by the geometric outlier correspondence rejector. These routines jointly enable for cleaner point clouds with better correspondences yielding better convergence properties, which is demonstrated on KITTI and internal dataset with relative positioning errors of $1.37\%$ and $3.67\%$ respectively. Further pursuit is an implementation of a local map and fast map query mechanisms to allow for implicit loop closures. Another conceived improvement is mixing point-to-point and point-to-plane losses depending on the local structure, as well as the preparation of internal dataset for public release.

We are grateful to our colleague Sergey Krutikov for his invaluable contribution into this project.