A Benchmark for Multi-Modal Lidar SLAM with Ground Truth in GNSS-Denied Environments

The primary types of 3D lidar SLAM algorithms today are lidar-only [11], and loosely-coupled [12] or tightly-coupled [13] with IMU data. Tightly-coupled approaches integrate the lidar and IMU data at an early stage, in opposition to SLAM methods that loosely fuse the lidar and IMU outputs towards the end of their respective processing pipelines.

In terms of lidar-only methods, an early work by Zhang et al. on Lidar Odometry and Mapping (LOAM) introduced a method that can achieve low-drift and low-computational complexity already in 2014 [14]. Since then, there have been multiple variations of LOAM that enhance its performance. By incorporating a ground point segmentation and a loop closure module, LeGO-LOAM is more lightweight with the same accuracy but improved computational expense and lower long-term drift [15]. However, lidar-only approaches are mainly limited by a high susceptibility to featureless landscapes [16, 17]. By incorporating IMU data into the state estimation pipeline, SLAM systems naturally become more precise and flexible.

In LIOM [13], the authors proposed a novel tightly-coupled approach with lidar-IMU fusion based on graph optimization which outperformed the state-of-the-art lidar-only and loosely coupled. Owing to the better performance of tightly-coupled approaches, subsequent studies have focused in this direction. Another practical tightly-coupled method is Fast-LIO [18], which provides computational efficiency and robustness by fusing the feature points with IMU data through a iterated extended Kalman filter. By extending FAST-LIO, FAST-LIO2 [19] integrated a dynamic structure ikd-tree to the system that allows for the incremental map update at every step, addressing computational scalability issues while inheriting the tightly-coupled fusion framework from FAST-LIO.

The vast majority of these algorithms function well with spinning lidars. Nonetheless, new approaches are in demand since new sensors such as solid-state Livox lidars have emerged novel sensing modalities, smaller FoVs and irregular samplings have emerged [9]. Multiple existing studies using enhanced SLAM algorithms are being researched to fit these new lidar characteristics. Loam livox  [20] is a robust and real-time LOAM algorithm for these types of lidars. LiLi-OM [6] is another tightly-coupled method that jointly minimizes the cost derived from lidar and IMU measurements for both solid-state Lidars and conventional Lidars.

It is worth mentioning that there are other studies addressing lidar odometry and mapping by fusing not only IMU but also visual information or other ranging data for more robust and accurate state estimation [21, 22].

There are various multi-sensor datasets available online. We had a systematic comparison of the popular datasets in the Table III of our former work [9]. Among these datasets, not all of them have an analytical benchmark of 3D Lidar SLAM based on multi-modality Lidars. KITTI benchmark [23] is the most significant one with capabilities of evaluating several tasks including odometry, SLAM, objects detection, tracking ans so alike.

The data collection platform contains various lidar sensors, from traditional spinning lidars with different resolutions to novel solid-state lidar featured with non-repetition scanning patterns. A lidar camera and stereo fisheye camera are also included. There are three spinning lidars, a 16-channel Velodyne lidar (VLP-16), a 64-channel Ouster lidar (OS1), and a 128-channel Ouster lidar (OS0). The OS0 and OS1 sensors were mounted left and right sides, where the OS1 is turned 45 degrees clockwise, and the OS0 is turned 45 degrees anticlockwise. The Velodyne lidar is at the top-most position. Two solid-state lidars, Horizon and Avia, were installed in the center of the frame. The Optitrack marker set for the MOCAP-based and the antenna for GNSS/RTK ground truth are both fixed on the top of the aluminum stick to maximize its visibility and detection range. All sensors are connected to a computer, featuring an Intel i7-10750h processor, 64 GB of DDR4 RAM memory and 1 TB SSD storage, through a Gigabit Ethernet router. The data collection system, including sensor drivers and online calibration scripts, are running on ROS Melodic under Ubuntu 18.04 entirely owing to the wider variety of ROS-based lidar SLAM methods available for Melodic.

Efficient extrinsic parameters calibration is crucial to multi-sensor platforms, especially for handmade devices where the extrinsic parameters may change due to unstable connections or distortion of the material during transit. Similar to our previous work [9], we calculated the extrinsic parameter of sensors before each data collecting process. Fig. 2 shows the calibration result of sample lidar data from one of the indoor data sequences.

Different to our previous work [9], where the timestamp of Ouster and Livox lidars are kept based on their own clock, we synchronized all lidar sensors in ethernet mode via the software-based precise timestamp protocol (PTP) [24]. We compared the orientation estimation between the sensor’s built IMUs, and SLAM results with lidars and concluded that the latency of our system is below 5 ms.

To provide accurate ground truth for large-scale indoor and outdoor environments, where the MOCAP system is unavailable or GNSS/RTK positioning result becomes unreliable due to the multi-path effect, we propose a SLAM-assisted solid-state lidar-based ground map generation framework.

Inspired by the prior map generation methods in  [25], where a survey-grade 3D imaging laser scanner Leica BLK360 scanner is unitized to obtain static pointclouds of the target environment, we employed a low-cost solid-state lidar Livox Avia and high resolution spinning lidar to collect undistorted pointclouds from environments. According to the Livox Avia datasheet,the range accuracy of the Avia sensor is 2 cm with a maximum detection range of 480 m. Due to the non-repetitive scanning pattern, the environment coverage of the pointcloud within the FoV increases with time. Therefore, we integrated multiple frames when the platform is stationary to get more detailed undistorted environmental sampling. Each integrated pointcloud contains more than 240,000 points. The Livox built-in IMU is used to detect the stationary state of the platform when the acceleration values are smaller than 0.01 $m/s^{2}$ along all axes. After gathering multiple undistorted pointcloud submaps from the target environment, the next step is to match and merge all submap into a global map by ICP. As the ICP process requires a good initial guess, we employ a high resolution spinning lidar os0 with a 360-degree horizontal FOV to provide raw position by performing real-time SLAM algorithms. This process is outlined in Algorithm 1. A dense and high-definition ground truth map can be obtained by denoising the map generated by the algorithm described above to remove noise. Fig. 1(a) shows ground truth map of sequence indoor08 generated based on Algorithm 1

Let $\mathcal{P}_{sk}$ be the pointcloud produced by the spinning lidar, $\mathcal{P}_{dk}$ be the pointcloud generated by solid-state lidar, and $\mathcal{I}_{k}$ be the IMU data from built-in IMU. Our previous work has shown high resolution spinning lidar has the most robust performance in diverse environments. Therefore, LeGo-LOAM [15] is performed with a high resolution spinning lidar (OS0-128) and outputs the estimated pose for each submap.

The cached data $\mathcal{S}_{cache}$ stores submaps and the related poses. Let $\mathcal{P}_{i}$ be the pointcloud and related pose $\textbf{p}_{i}$ in $\mathcal{S}_{cache}[i]$. The submap $\mathcal{P}_{i}$ will be first transformed to map coordinate as $\mathcal{P}^{m}_{i}$ based on estimated pose $\textbf{p}_{i}$; then GICP methods are employed on $\mathcal{P}^{m}_{i}$ to minimize the Euclidean distance between closest points against pointcloud $\mathcal{M}_{ap}$ iteratively; $\mathcal{P}^{m}_{i}$ will be transformed by the transformation matrix generated from GICP process, then merged to the map $\mathcal{M}_{ap}$. The result map $\mathcal{M}_{ap}$ is treated as ground truth map.

After the ground truth map generated, we employ normal NDT method in  [26] to match the real-time pointcloud data from spinning lidar against the HR map as the Figure 4 shows to get the platform position in ground truth map. The matching result from the NDT localizer is treated as the ground truth.

The evaluation of the accuracy of the proposed ground truth prior map method is challenging for some scenes in the dataset, as both GNSS and MOCAP systems are not available in indoor environments such as long corridors. Figure 5 (a),(b),(c) shows the standard deviations of the ground truth generated by the proposed method during the first 10 seconds when the device is stationary from sequence Indoors09. The standard deviations along the $X$, $Y$, and $Z$ axes are 2.2 cm, 4.1 cm, and 2.5 cm, respectively, or about 4.8 cm overall. However, evaluating localization performance when the device is in motion is more difficult. To better understand the order of magnitude of the accuracy, we compare the NDT-based ground truth $Z$ values with the MOCAP-based ground truth $Z$ values in the sequence Indoor06 environment. The results in Fig.  5 (d) show that the maximum difference does not exceed 5 cm.

Different types of SLAM algorithms are selected and tested in our experiment. Lidar-only algorithms LeGo-LOAM (LEGO)  ¹¹1https://github.com/RobustFieldAutonomyLab/LeGO-LOAM and Livox-Mapping (LVXM) ²²2https://github.com/Livox-SDK/livox_mapping are applied on data from the VLP-16 and Horizon separately; Tightly-coupled iterated extended Kalman filter-based methods, FAST-LIO (FLIO) ³³3https://github.com/hku-mars/FAST_LIO  [27], are applied on both spinning lidar and solid-state lidar with built-in IMUs; A tightly coupled lidar inertial SLAM system based on sliding window optimization, LiLi-OM ⁴⁴4https://github.com/KIT-ISAS/lili-om [28] is tested with OS1 and Horizon. Furthermore, a tightly coupled method featuring sliding window optimization developed for Horizon lidar, LIO-LIVOX (LIOL)⁵⁵5https://github.com/Livox-SDK/LIO-Livox has also been tested on Horizon lidar data.

We provide a quantitative analysis of the odometry error based on the ground truth in Table III. To compare the trajectories in the same coordinate, we treat the coordinate of OS0 as a reference coordinate and transformed all trajectories generated by selected SLAM methods to reference coordinate. The absolute pose errors (APE) [29] is employed as the core evaluation metric. We calculated the error of each trajectory with the open-source EVO toolset ⁶⁶6https://github.com/MichaelGrupp/evo.git.

From the result, we can conclude that FAST_LIO with high resolution spinning lidar OS0 and OS1 has the most robust performance that can complete all the trajectories on different sequences with promising accuracy. Especially for sequence Indoor09 showcasing a long corridor, all other methods failed and Fast_LIO with high resolution lidar remain survived. Solid-state lidar-based SLAM systems such as LIOL_Hori perform as well or even better in outdoor environments than rotating lidars with appropriate algorithms, but perform significantly more poorly in the indoor environments. For the open road sequences Road03, all SLAM methods perform well, and the trajectories are completed without major disruptions. For the indoor sequence Indoor06, Avia-based and Horizon-based FLIO are able to reconstruct the sensor trajectory but significant drift accumulates. In all of these sequences, all the methods applied to spinning lidars perform satisfactorily. This result can be expected as they have full view of the environment, which has a clear geometry. For the sequence Indoor10 showcasing a long corridor, almost all methods can reconstruct the complete trajectory again. The best performance comes from OS0-FLIO and OS1-FLIO with correct alignment between the first and last positions. We hypothesize that this occurs because OS0 has more channels than OS1, leading to lower accumulated cumulative angular drift.

In addition to the quantitative trajectory analysis, we visualize trajectories generated by selected methods in 3 representative environments (indoors, outdoors, forest) in Fig. 6. Full reconstructed paths are available in the dataset repository.

We conducted this experiment on 4 different platforms. First, a Lenovo Legion Y7000P with 16 GB RAM, a 6-core Intel i5-9300H (2.40 GHz) and an Nvidia GTX 1660Ti (1536 CUDA cores, 6 GB VRAM). Then, the Jetson Xavier AGX, a popular computing platform for mobile robots, has an 8-core ARMv8.2 64-bit CPU (2.25 GHz), 16 GB RAM and 512-core Volta GPU. From its 7 power modes, we chose MAX and 30 W (6 core only) modes. The Nvidia Xavier NX is also a common embedded computing platform with a 6-core ARM v8.2 64-bit CPU, 8 GB RAM, and 384-core Volta GPU with 48 Tensor cores. For the NX, we choose the 15 W power mode (all 6 cores). Finally, the UP Xtreme board features an 8-core Intel i7-8665UE (1.70 GHz) and 16 GB RAM.

These platforms all run ROS Melodic on Ubuntu 18.04. The CPU and memory utilization is measured with a ROS resource monitor tool ⁷⁷7https://github.com/alspitz/cpu_monitor. Additionally, for minimizing the difference of the operating environment, we unified the dependencies used in each SLAM system into same version, and each hyperparameter in the SLAM system is configured with the default values. The results are shown in Table IV.

The memory utilization of each selected SLAM approach among the two processor architectures platforms are roughly equivalent. However, the CPU utilization of the same SLAM algorithm running on Intel processors is generally higher than the other algorithms, and also the highest publishing frequency is obtained. LeGO_LOAM has the lowest CPU utilization but its accuracy is towarsd the low end (see Table III), and has a very low pose publishing frequency. Fast-LIO performs well, especially on embedded computing platforms, with good accuracy, low resource utilization, and high pose publishing frequency. In contrast, LIO_LIVOX has the highest CPU utilization due to the computational complexity of the frame-to-model registration method applied to estimate the pose.

A final takeaway is in the generalization of the studied methods. Many state-of-the-art methods are only applicable to a single lidar modality. In addition, those that have higher flexibility (e.g., FLIO) still lack the ability to support a point-cloud resulting from the fusion of both types of lidars.