## 3 LiDAR-Inertial odometry
### LiDAR Odometry  
The front-end of the LIO subsystem is mainly processed sensor data. Following LOAM and Lego-LOAM. Firstly, the distorted point cloud is corrected, and then feature extraction is started to extract edge points and plane points by calculating the curvature of the point cloud in the frame. \(\mathrm{F}_{i}^{\mathrm{e}}\) and \(\mathrm{F}_{i}^{\mathrm{p}}\) are the sets of edge features and planar features extracted at moment \(i\), respectively, and they, together, form the LiDAR frame sets \(\mathrm{F}_{i}\) at moment \(i\), \(\mathrm{F}_{i}=\left\{\mathrm{F}_{i}^{\mathrm{e}},\mathrm{F}_{i}^{\mathrm{p}}\right\}\), where the LiDAR frame F is represented under B. After obtaining the LiDAR frames, to avoid the factor graph being too large, which leads to data complexity and cumulative errors, each LiDAR frame extracted cannot be directly added to the factor graph as a factor. The widely used keyframe strategy is used to select the most representative LiDAR frames at each moment as keyframes. If the robot's pose \(x_{i}\) exceeds the defined threshold at moment \(i\), the LiDAR frame \(\mathrm{F}_{i+1}\) of the next moment will be used as the keyframe and the LiDAR frames between keyframes \(\mathrm{F}_{i}\) and \(\mathrm{F}_{i+1}\) will be discarded. The latest saved keyframe \(\mathrm{F}_{i+1}\) will be associated with the new robot state \(x_{i+1}\) for data association. Then, the set \(\left\{\mathrm{F}_{i-n},\ldots,\mathrm{F}_{i}\right\}\) of \(n\) closest keyframes at moment \(i\), also known as subkeyframes, are extracted and transformed to W through the corresponding transformation matrix set \(\left\{\mathrm{T}_{i-n},\ldots,\mathrm{T}_{i}\right\}\). This results in a voxel map \(\mathrm{M}_{i}\) consisting of a edge voxel map \(\mathrm{M}_{i}^{\mathrm{e}}\) and a planar voxel map \(\mathrm{M}_{i}^{\mathrm{p}}\). The relationship between the LiDAR frame and voxel map is  
\[\mathrm{M}_{i}=\left\{\mathrm{M}_{i}^{\mathrm{e}},\mathrm{M}_{i}^{\mathrm{p}}\right\} \tag{2}\]  
Figure 2: Factor graph of the system.The system receives detection data from LiDAR, camera, and IMU. The factor graph contains five types of constraints: imu preintegration constraints, LiDAR odometry constraints, visual-inertial odometry constraints, and loop closure constraints. \[\mathrm{M}_{i}^{e} = {}^{\prime}\mathrm{F}_{i}^{e}\cup{}^{\prime}\mathrm{F}_{i-1}^{e} \cup\ldots\cup{}^{\prime}\mathrm{F}_{i-n}^{e} \tag{3}\] \[\mathrm{M}_{i}^{p} = {}^{\prime}\mathrm{F}_{i}^{p}\cup{}^{\prime}\mathrm{F}_{i-1}^{p} \cup\ldots\cup{}^{\prime}\mathrm{F}_{i-n}^{p} \tag{4}\]  
In order to preserve the basic shape and geometric structure of the original point cloud as much as possible, it is necessary to divide the map into several voxels and keep only one representative point in each voxel to realize the downsampling of point cloud data. After filtering out the redundant point cloud features, the new keyframe set F\({}_{i+1}\) is feature-matched to the voxel map M\({}_{i}\) that has already been created. Registration methods of Point-to-line and point-to-plane are adopted [24; 25], F\({}_{i+1}\) is converted from B to W to obtain F\({}_{i+1}\), and the distance equation from the point cloud feature to the corresponding edge or plane block is  
\[d_{e_{k}}=\frac{\left|\left(\mathrm{P}_{i+1,k}^{e}-\mathrm{P}_{i,u}^{e}\right) \times\left(\mathrm{P}_{i+1,k}^{e}-\mathrm{P}_{i,u}^{e}\right)\right|}{\left| \mathrm{P}_{i,u}^{e}-\mathrm{P}_{i,v}^{e}\right|} \tag{5}\]  
\[d_{p_{k}}=\frac{\left|\left(\mathrm{P}_{i+1,k}^{p}-\mathrm{P}_{i,u}^{p}\right) \left(\mathrm{P}_{i,u}^{p}-\mathrm{P}_{i,v}^{p}\right)\times\left(\mathrm{P}_ {i,u}^{p}-\mathrm{P}_{i,w}^{p}\right)\right|}{\left|\left(\mathrm{P}_{i,u}^{p} -\mathrm{P}_{i,w}^{p}\right)\times\left(\mathrm{P}_{i,u}^{p}-\mathrm{P}_{i,w}^ {p}\right)\right|} \tag{6}\]  
where \(k,u,v,w\) denotes the index in the corresponding feature set.