# Fast Lidar Inertial Odometry and Mapping for Mobile Robot SE(2) Navigation
## 1 Introduction  
Simultaneous localization and mapping (SLAM) plays a crucial role in diverse navigation missions and is indispensable for autonomous mobile robots [1; 2]. It provides reliable state estimation and high-precision maps required for collision-free path planning and real-time obstacle avoidance. Over the past thirty years, numerous excellent visual SLAM techniques, Lidar SLAM techniques, and back-end optimization methods have been proposed to support 6-DOF state estimation for mobile robots. Vision SLAM methods, including monocular-vision-based [3; 4], RGB-D-based [5; 6], and vision-inertial-based [7; 8; 9; 10] SLAM frameworks, usually employ a camera to obtain a semantic interpretation of the scene and have advantages in place recognition; at the same time, they cannot work in poor lighting situations. Lidar SLAM [11; 12; 13; 14; 15] is invariant to illumination changes, which is essential for obstacle detection and tracking to achieve autonomous navigation. Therefore, this paper mainly focuses on mobile robot pose estimation and mapping based on 3D Lidar. In 3D Lidar-based SLAM, one of the goals is to estimate Lidar odometry (LO), or Lidar inertial odometry (LIO) if the inertial measurement unit (IMU) is employed. LIO is usually represented by SE(3) [16], involving translation terms for the x, y, and z axes and the rotation of the three axes (roll, pitch, and yaw). A robot's changes in the z direction and pitch and roll dimensions are insignificant for navigation on the ground, especially in indoor or outdoor flat terrain. In these cases, the traditional SLAM system can be almost equivalently converted to estimate only the x, y, and yaw angle, and mobile robots move in SE(2). Although these perturbations are trivial, they can still represent the state changeof the robot and may corrupt the measurements, and it is not wise to ignore them directly during the state estimation process. For mobile robots, the state space in terms of extended and minimal representations is SE(2) and can be conveniently optimized by incremental smoothing and mapping with a Bayes tree (iSAM2) [17]. These perturbations are considered as a noise term and integrated into the model to construct a new constraint in the factor graph. In addition, loop closure, as one of the indispensable components of SLAM, is used to correct the accumulated error. The smoothness information calculated in the feature extraction process can not only be used to distinguish edge or plane features [18] but can also be used for the front-end place recognition algorithm to detect loop closures. More importantly, it integrates the geometry and environmental texture information of the point cloud over a local region and requires less memory. This study proposes a fast Lidar inertial odometry and mapping (F-LIOM) method for real-time local accuracy and the global drift-free state estimation of mobile robots in flat terrain. F-LIOM contains all the essential elements required for a SLAM system, including point-cloud de-skewing, pose estimation, and loop closure. We first redefine the data structure of the point cloud in the feature extraction stage for later place recognition and jointly apply the Gaussian process (GP) and IMU measurements to correct the point-cloud distortion in a non-linear form, thereby replacing the traditional assumption of uniform motion. Next, this paper assumes that the robots perform SE(2) motion and proposes a novel constraint model integrating the perturbations as noises to optimize the state estimation by associating the extended pose and the 3D points via the Lidar measurements. Last, for the front-end place recognition algorithm, we construct a new global descriptor, the smoothness scanning context, to detect loop closure and construct a globally consistent trajectory and map. The main contributions of our work can be summarized as follows:  
* An efficient, fast F-LIOM method is developed that integrates the perturbations as weights in noise terms. It enables the system to be computationally tractable.