*Article* **LIDAR-Inertial Real-Time State Estimator with Rod-Shaped and Planar Feature**

**Hong Liu 1,2, Shuguo Pan 1,2,\*, Wang Gao 1,2, Chun Ma 1,2, Fengshuo Jia 1,2 and Xinyu Lu 1,2**


**Abstract:** State estimation and mapping based on Light Detection and Ranging (LIDAR) are important for autonomous systems. Point cloud registration is a crucial module affecting the accuracy and realtime performance of LIDAR simultaneous localization and mapping (SLAM). In this paper, a novel point cloud feature selection for LIDAR-inertial tightly coupled systems is proposed. In the front-end, a point cloud registration is carried out after marking rod-shaped and planar feature information which is different from the existing LIDAR and inertial measurement unit (IMU) integration scheme. This preprocessing method subsequently reduces the outliers. IMU pre-integration outputs highfrequency result and is used to provide the initial value for LIDAR solution. In the scan-to-map module, a computationally efficient graph optimization framework is applied. Moreover, the LIDAR odometry further constrains the IMU states. In the back-end, the optimization based on slidingwindow incorporates the LIDAR-inertial measurement and loop closure global constraints to reduce the cumulative error. Combining the front-end and back-end, we propose the low drift and high real-time LIDAR-inertial positioning system. Furthermore, we conducted an exhaustive comparison in open data sequences and real-word experiments. The proposed system outperforms much higher positioning accuracy than the state-of-the-art methods in various scenarios. Compared with the LIO-SAM, the absolute trajectory error (ATE) average RMSE (Root Mean Square Error) in this study increases by 64.45% in M2DGR street dataset (street\_01, 04, 07, 10) and 24.85% in our actual scene datasets. In the most time-consuming mapping module of each system, our system runtime can also be significantly reduced due to the front-end preprocessing and back-end graph model.

**Keywords:** tightly-coupled integration; LIDAR-inertial SLAM; rod-shaped and planar feature; sliding-window; graph optimization framework

**1. Introduction**

Accurate and reliable state estimation is a fundamental requirement of mobile robot and automatic driving. In urban environments, indoor environments and other complex scenes, it is difficult to achieve a high precision of positioning requirements with the traditional GNSS/INS integrated.

In recent years, visual/LIDAR simultaneous localization and mapping have made certain developments. On the one hand, visual slam can achieve six degrees-of-freedom state estimation just by camera, but it is seriously affected by the illumination and low texture feature [1]. On the other hand, the laser sensor directly obtains depth information and has high resolution, which can also work at night and achieve accurate pose estimation. Therefore, this research mainly focuses on LIDAR simultaneous localization and mapping.

LIDAR odometry and mapping (LOAM) [2] is an earlier proposed LIDAR slam algorithm. Iterative ICP algorithm is a common method for point cloud matching, which is time-consuming for registration, and it is easy to fall into a local minimum [3]. LOAM replaces ICP with point-to-line and point-to-plane matching. It consists of two subsystems.

**Citation:** Liu, H.; Pan, S.; Gao, W.; Ma, C.; Jia, F.; Lu, X. LIDAR-Inertial Real-Time State Estimator with Rod-Shaped and Planar Feature. *Remote Sens.* **2022**, *14*, 4031. https:// doi.org/10.3390/rs14164031

Academic Editor: Andrzej Stateczny

Received: 8 June 2022 Accepted: 16 August 2022 Published: 18 August 2022

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

The odometry system performs point to line/plane feature matching to calculate pose between scans. The features of line and plane are judged according to point curvature. At the same time, LOAM effectively eliminates the unreliable parallel points and occlusion points. And it performs the distortion compensation by motion interpolation. The mapping system performs scan to map matching and runs at lower frequency, which can perform higher accuracy state estimation. By combining these two systems, LOAM achieves low drift and low-computational complexity, which has been ranked as the top in the LIDAR based method on the KITTI odometry benchmark site [4]. However, LOAM still has some flaws; its point cloud is stored in global voxel. Without key frame selection, it is difficult to integrate observation information of other sensors and perform global optimization.

F-LOAM adopts a two-stage distortion compensation method to reduce the computational cost and improve the real-time performance [5], but there are still no global optimization methods such as loop closure, resulting in large cumulative errors over a long period of time. Liu et al. propose a method based on deep learning for extracting feature points and obtaining the descriptors in LIDAR odometry. It also adopts the two-step state estimation for long distance experiment, which has a good performance for LIDAR of various resolutions [6]. V-LOAM introduces the visual odometry as the front-end of the laser odometry, further improving the accuracy of slam [7]. HDL\_GRAPH\_SLAM [8] is an algorithm that can fuse LIDAR, IMU and GNSS sensors, but the scan registration accuracy is low which is based on NDT [9]. It is also prone to drift in non-plane because of the flat ground constraint. LeGo-LOAM implements point cloud segmentation to reduce the number of features, and two-step registration provides the initial value for LIDAR mapping module. LeGo-LOAM firstly covers the key frame selection and loop detection [10]. However, there is obvious drift in the large scene testing experiment and the IMU is only used to remove distortion. LIO-mapping [11] is a joint state estimation problem based on the ideas of LOAM and VINS-Mono [12]. The front-end vision part is replaced by the LIDAR front-end for feature extraction and state estimation. However, the optimization problem is too large to be real-time, which makes it hard to apply in a mobile device. LINS is a tightly coupled LIDAR-inertial odometry (LIO) system based on the filter method [13]. The iterative error state Kalman Filter is used to correct the state estimation of the robot, but there is still a problem that the robot will drift when it runs for a long time without global constraints. LiLi-OM puts forward an adaptive keyframe selection for both solid-state and traditional LIDAR. It also introduces a metric weighting function during sensor fusion [14]. However, lacking a point cloud processing, the system stability is inadequate. LIO-SAM [15] is also a tightly-coupled LIO system, which is based on the incremental smoothing and mapping framework iSAM2 [16]. In addition, the loop closure factor and GPS factor can be added to the global optimization factor graph. In spite of this, its IMU constraints do not enter the factor graph optimization system, which may result in loss of constraint information between IMU and LIDAR measurements. In the actual scene test, LIO-SAM will appear at unstable states such as point cloud matching errors, especially when the carrier movement is in a large scene. Zhang et al. proposed the LIDAR-inertial odometry with an adaptive covariance estimation algorithm which is based on loosely-coupled method. It achieves better result compared to the tightly-coupled method [17].

In short, the existing LIDAR slam algorithms are mainly for small scenes. But for the complex scene or the great motion change, they are prone to cumulative errors and poor robustness.

Meanwhile, the processing of point cloud data affects the accuracy of point cloud registration for LIDAR slam. Douillard et al. introduced a method which jointly determines the ground and individual objects on the ground in three-dimensional space, including overhanging structures, but it requires a large amount of computation time, limiting online applications [17]. B. Douillard et al. proposed a priori ground extraction way. Segmentation of dense 3D data is optimized via a simple yet efficient voxel of the space. This approach provides near-real-time performance, but is not sufficient for real-time positioning scenarios [18]. M. Himmelsbach et al. proposed that 3D point clouds are

projected onto 2D grids on the ground plane, and then point clouds were segmented on the occupied grids [19]. The algorithm has fast speed and is suitable for online segmentation. However, the method tends to result in weak segmentation. When two objects are relatively close to each other, it is prone to misrecognition, especially in the z-axis direction. In 2019, Seungcheol Park et al. proposed Curved-Voxel Clustering. The point cloud coordinates are converted from cartesian coordinates to spherical coordinates, and each point cloud is assigned to the voxel in the corresponding spherical coordinate system. Hash tables establish associations between indexes and points. When clustering, lookup is implemented using the hash tables [20]. Chen et al. use IMU to assist the point cloud registration and introduce the inertial error model for mobile laser scanning, which could effectively reduce the error with low time cost [21].

This paper mainly aims to improve the accuracy of LIDAR point cloud registration under the condition of real-time positioning, so as to ensure the robustness of the system. The contributions of this paper are summarized as follows:

