Learning to Localize Using a LiDAR Intensity Map

Given a sequence of sensory inputs (e.g., LiDAR point clouds, color and/or depth images) simultaneous localization and mapping (SLAM) approaches [9, 10, 11] reconstruct a map of the environment and estimate the relative poses between each input and the map. Unfortunately, since the estimation error is usually biased, accumulated errors cause gradual estimation drift as the robot moves, resulting in large errors. Loop closure has been largely used to fix this issue. However, in many scenarios such as highways, it is unlikely that trajectories are closed. GPS measurements can help reduce the drift issue through fusion but commercial GPS sensors are not able to achieve centimeter-level accuracy.

Light-weight maps, such as Google maps and OpenStreetMap, draw attention for developing affordable localization efforts. While only requiring small amounts of storage, they encode both the topological structure of the road network, as well as its semantics. Recent approaches incorporated them to compensate for large drift [12, 13] and keep the vehicle associated with the road. However, these methods are still not yet able to achieve centimeter-level accuracy.

Exploiting high-definition maps (HD maps) has gained attention in recent years on both indoor and outdoor localization [4, 7, 8, 14, 15, 16, 17, 18]. The general idea is to build an accurate map of the environment offline through aligning multiple sensor passes over the same area. In the online stage the system is able to achieve sub-meter level accuracy by matching the sensory input against the HD-map. In their pioneering work, Levinson et al. [7] built a LiDAR intensity map offline using Graph-SLAM [19] and used particle filtering and Pearson product-moment correlation to localize against it. In a similar fashion, Wan et al. [18] use BEV LiDAR intensity images in conjunction with differential GPS and an IMU to robustly localize against a pre-built map, using a Kalman filter to track the uncertainty of the fused measurements over time. Uncertainty in intensity changes can be handled through online calibration and by building probabilistic map priors [8]. However, these methods require accurate calibration (online or offline) which is difficult to achieve. Yoneda et al. [17] proposed to align online LiDAR sweeps against an existing 3D prior map using ICP. However, this approach suffers in the presence of repetitive geometric structures, such as highways and bridges. The work of Wolcott and Eustice [14] combines height and intensity information against a GMM-represented height-encoded map and accelerates registration using branch and bound. Unfortunately, the inference speed cannot satisfy the real-time requirements of self-driving. Visual cues from a camera can also be utilized to match against 3D prior maps [3, 4, 15]. However, these approaches either require computationally demanding online 3D map rendering [15] or lack robustness to visual apperance changes due to the time of day, weather, and seasons [3, 4]. Semantic cues such as lane markings can also be used to build a prior map and exploited for localization [16]. However, the effectiveness of such methods depends on perception performance and does not work for regions where such cues are absent.

Convolutional matching networks that compute similarity between local patches have been exploited for tasks such as stereo [20, 21, 22], flow estimation [23, 24], global feature correspondence [25], and 3D voxel matching [26]. In this paper, we extend this line of work for the task of localizing in a known map.

Training machine learning models to conduct self-localization is an emerging field in robotics. The pioneering work of Shotton et al. [27] trained random forest to detect corresponding local features between a single depth image and a pre-scanned indoor 3d map. [28] utilized CNNs to detect text from large shopping malls to conduct indoor localization. Kendall et al. [29] proposed to directly regress a 6-DoF pose from a single image. Neural networks have also been used to learn representations for place recognition in outdoor scenes, achieve state-of-the-art performance in localization-by-retrieval [5]. Several algorithms utilize deep learning for end-to-end visual odometry [30, 31], showing promising results but still remaining behind traditional SLAM approaches in terms of performance.

Very recently, Bloesch et al. [32] learn a CNN-based feature representation from intensity images to encode depth, which is used to conduct incremental structure-from-motion inference. At the same time, the methods of Brachmann and Rother [33] and Radwan et al. [34] push the state of the art in learning-based localization to impressive new levels, reaching centimeter-level accuracy in indoor scenarios, such as those in the 7Scenes dataset, but not outdoor ones.

We collected a new dataset comprising over 4,000km of driving through a variety of urban and highway environments in multiple cities/states in North America, collected with two types of LiDAR sensors. According to the scenarios we split our dataset into Highway-LidarA and Misc-LidarB, where Highway-LidarA contains over 400 sequences for a total of over 3,000km of driving for training and validation. We select a representative and challenging subset of 282km of driving for testing, ensuring that there is no geographic overlap between the splits. All these sequences are collected by a LiDAR type A. Misc-LidarB contains 79 sequences with 200km of driving over a mix of highway and city collected by a different LiDAR type B in a different state. LiDARs A and B differ substantially in their intensity output profiles, as shown in  Fig. 6.

We randomly extracted 230k training samples from the training sequences. For each training sample, we aggregate the five most recent online LiDAR sweeps to generate the BEV intensity image using vehicle dynamics, corresponding to 0.5 seconds of LiDAR scan. In such a short time drift is negligible. Our ground-truth poses are acquired through an expensive high precision offline matching procedure with up to several centimeter uncertainty. We rasterize the aggregated LiDAR points to create a LiDAR intensity image. Both the online intensity image and the intensity map are discretized at a spatial resolution of 5cm covering a $30m\times 24m$ region. During matching, we use the same spatial resolution, plus a rotational resolution of 0.5^∘, with a total search range of $1m\times{}1m\times{}2.5$^∘around the dead reckoning pose. We report the median error as well as the failure rate. The median error reflects how accurate the localization is in the majority of cases while the failure rates reflect the worst case performance In particular, we define “failure” if there is at least a frame with localization error over 1m. In addition to these per-sequence metrics, we also plot the per-frame cumulative localization error curve in Fig. 5.

We manually chose the following hyper-parameters through validation, namely the motion model variance $\Sigma=\mathrm{diag}([3.0,3.0,3.0])$, GPS’s observation variance $\sigma_{gps}=10.0$, temperature constant $\alpha=2.0$. We also conduct two ablation studies. Our first ablation verifies whether the motion prior defined in Eq. (4) is helpful. We evaluate algorithm with and without this term, denoted as $\mathrm{Motion}$ in Table 1. Our second ablation evaluates whether a probabilistic MLE proposed in Eq. (7) helps improve performance, denoted as $\mathrm{Prob}$. The none-probabilistic is achieved through changing the soft-argmax in Eq. (7) to a hard argmax. We implement our full inference algorithm in PyTorch 0.4. The networks are trained using Adam over four NVIDIA 1080Ti GPUs with initial learning rate at 0.001.

We compare our algorithm against several baselines. The raw matching consists of performing the matching-based localization in a similar manner to our method but only use the raw intensity BEV online and map images, instead of the learned embeddings. The ICP baseline conducts point-to-plane ICP between the raw 3D LiDAR points against the 3D pre-scanned localization prior at 10Hz, initialized in a similar manner as us using the previous estimated location plus the vehicle dynamics. This ensures good quality of initialization, as required by algorithms from ICP family.

As shown in Table 1, our approach achieves the best performance among all the competing algorithms in terms of failure rate. Both probabilistic inference and motion prior further improves the robustness of our method. Our ICP baseline is competitive in terms of median error, especially along lateral direction, but the failure rate is significantly higher. It is also more computationally demanding and requires 3D maps. Both dynamics-only and raw intensity matching result in large drift. Moreover, we have observed that deeper architectures and the probabilistic inference are generally helpful. Fig. 4 shows the localization error as a function of the travel distance aggregated across all sequences from the Highway-LidarA test set. The solid line denotes the median and the shaded region denotes the $95\%$ area, together with the distribution of lateral and longitudinal errors per frame. Fig. 5 compares our approach to ICP in terms of cumulative errors with $95\%$-percentile error reported. From this we can see our method significantly outperforms ICP in terms of the worst-case behavior.

In order to show that our approach generalize well across LiDAR sensors, we conduct a second experiment, where we train our network on Highway-LidarA, which is purely highway, collected using LiDAR A, and test on the test set of Misc-LidarB which is highway + city, collected by a different LiDAR (type B) in a different state. In order to better highlight the difference, in Fig. 6 we show two LiDARs intensity value distributions and their raw intensity images, collected at the same location. Table 2 showcases the results of this experiment. From the table we can see our neural network is able to generalize both across LiDAR models and across environment types.

We conduct a runtime analysis over both embedding networks and matching. Our LinkNet based embedding networks take less than 10ms each for a forward pass over both online and map images. We also compare the cuDNN implementation of FFT-conv and standard spatial convolution. FFT reduces the run time of the matching by an order of magnitude bringing it down from $27ms$ to $1.4ms$ for a single-channel embedding. This enables us to run the localization algorithm at 15 Hz, thereby achieving our real-time operation goal.