An Effective LiDAR-Inertial SLAM-Based Map Construction Method for Outdoor Environments

Abstract

SLAM (simultaneous localization and mapping) is essential for accurate positioning and reasonable path planning in outdoor mobile robots. LiDAR SLAM is currently the dominant method for creating outdoor environment maps. However, the mainstream LiDAR SLAM algorithms have a single point cloud feature extraction process at the front end, and most of the loop closure detection at the back end is based on RNN (radius nearest neighbor). This results in low mapping accuracy and poor real-time performance. To solve this problem, we integrated the functions of point cloud segmentation and Scan Context loop closure detection based on the advanced LiDAR-inertial SLAM algorithm (LIO-SAM). First, we employed range images to extract ground points from raw LiDAR data, followed by the BFS (breadth-first search) algorithm to cluster non-ground points and downsample outliers. Then, we calculated the curvature to extract planar points from ground points and corner points from clustered segmented non-ground points. Finally, we used the Scan Context method for loop closure detection to improve back-end mapping speed and reduce odometry drift. Experimental validation with the KITTI dataset verified the advantages of the proposed method, and combined with Walking, Park, and other datasets comprehensively verified that the proposed method had good accuracy and real-time performance.

1. Introduction

Map construction has been a primary research focus in the area of autonomous navigation for outdoor mobile robots [1]. Map construction approaches are mainly categorized into visual SLAM [2] and LiDAR SLAM [3]. The latter has gradually become the mainstream method of outdoor environment map construction because it uses LiDAR sensors with the advantages of long detection distance, high data accuracy, and no interference from light changes [4,5]. The LiDAR SLAM method is essentially separated into two parts: front end and back end [6]. The front-end component is primarily responsible for LiDAR data processing and point cloud feature extraction, which includes preprocessing, filtering, feature point extraction, and so on. The front end’s purpose is to improve the LiDAR odometry’s accuracy, as well as the map stability. ICP (iterative closest point) [7] and NDT (normal distribution transform) [8] are two widely used front-end methods for aligning point clouds. The back-end component is primarily in charge of optimizing the positioning and maps, which includes position optimization, map update, and loop closure detection. The back end’s purpose is to increase both the consistency and the accuracy of positioning and maps. Back-end techniques commonly utilized include pose graph optimization algorithms such as GTSAM [9], Ceres Solver [10], and so on, while loop closure detection algorithms typically use radius nearest neighbor (RNN) [11].

Currently, with the mainstream state-of-the-art LIO-SAM algorithm as the basis of research, there are still areas that need to be improved and areas for refinement in the front end and back end: (i) the front end should extract stable and reliable feature points as much as possible, so as to ensure the alignment accuracy of the LiDAR odometry and the stability of the map, and (ii) the back end should detect point cloud map loop closures as quickly and accurately as possible to avoid situations such as LiDAR odometry drift in large-scale scenarios.

As shown in Figure 1a, the feature point cloud is extracted directly by curvature calculation, and the result is very rough and cluttered. Moreover, due to the large scale of outdoor scenes, traditional loop closure detection methods based on odometry-measured distances (e.g., the RNN algorithm) are likely to produce cumulative drift and loop closure failures. As shown in Figure 1b, using the RNN algorithm for large-scale scene loop closure detection, the odometry drifts, and loop closure fails. In complex outdoor environments, directly relying on the curvature for feature extraction of point clouds is obviously single and unreasonable. Because the point cloud data acquired by LiDAR are too redundant, containing unnecessary distance maximum points, a large number of less reliable outlier points, etc., feature extraction and alignment of these points can easily degrade the odometry accuracy. The combination of the above leads to the low accuracy and poor reliability of the LiDAR SLAM algorithm in the outdoor mapping results, which brings unnecessary trouble to the subsequent robot re-localization and path planning. Therefore, there is an urgent need to optimize and improve the existing LiDAR SLAM front-end point cloud feature extraction module and back-end loop closure detection module, to further improve the accuracy and reliability of the map construction results, and to provide a guarantee for the subsequent autonomous navigation of mobile robots.

The LIO-SAM algorithm determines the LiDAR position by aligning two neighboring frames of the point cloud, a method known as scan-to-scan. It is important to note that LiDAR odometry based on scan-to-scan is less reliable since too little information is employed, resulting in lower position accuracy. Therefore, we integrated the distance from the laser point to the center of the LiDAR and the horizontal angle to extract the ground point, refine the front-end feature extraction module of the LIO-SAM algorithm, encode each frame of LiDAR point cloud as a range image, and cluster the non-ground point cloud. We decided to downsample the outlier points so that they could be included in the point cloud’s subsequent feature extraction because we could not discard all of the points that failed to cluster (outlier points); otherwise, the constructed maps would be too sparse, and the environment would have a single feature. By retaining just one column out of every five in the point cloud, we downsampled these outlier points. Next, planar points were extracted from ground points and corner points from clustered segmented non-ground points using the curvature. Both the downsampled outlier points and the successful clustering point cloud were contained in the non-ground points following clustering segmentation. The LiDAR measurement value is not always equal to the true value due to the existence of LiDAR measurement noise and other factors; this is known as “cumulative drift” and occurs each time the LiDAR odometry is solved. The error steadily increases over time. The localization of the LiDAR SLAM system may deviate from the true value due to cumulative drift, rendering the generated maps unusable. Therefore, loop closure detection is needed by the SLAM system in order to account for cumulative drift. In response to this problem, we present the Scan Context scene recognition method for loop closure detection. This method enhances the real-time mapping of the LIO-SAM algorithm’s back end and enables the algorithm to detect loop closure even in the case of a large odometry drift.

The overall framework of the algorithm proposed in this paper on the basis of the LIO-SAM algorithm is shown in Figure 2. The green module is the improved module proposed in this paper, including front-end point cloud clustering segmentation, feature extraction, and back-end loop closure detection by using the Scan Context algorithm. Point cloud clustering segmentation includes ground point extraction and non-ground point clustering, etc. The main innovations of this study are as follows:

(1)

We projected the original LiDAR point cloud as a range image and performed ground point extraction on the range image. This method effectively reduced the dimensionality of the point cloud data, improved the efficiency of point cloud processing, and could accurately identify the ground points.

(2)

We used the BFS algorithm to cluster and segment the non-ground points and found the point cloud with the same label by traversing layer by layer, which improved the clustering speed of the point cloud, avoided the larger computation brought by the global search, and established the foundation for the lightweight front end of the algorithm.

(3)

We introduce the Scan Context loop closure detection method in the back end of the LIO-SAM algorithm, which effectively improves the accuracy of loop closure detection and enhances the robustness of loop closure detection in the back end of the algorithm.

2. Related Work

LiDAR point cloud feature extraction methods have advanced significantly in the previous few years. H. Hoppe et al. (1992) represented the curvature and orientation information of the point cloud by calculating the normal vector of each LiDAR point [12]. A. Golovinskiy et al. (2009) proposed a shape-based point cloud recognition method that used shape descriptors to represent objects in a point cloud and used machine learning algorithms to classify the point cloud [13]. B. Jian et al. (2011) used a Gaussian mixture model to represent the point set, which was aligned and filtered based on the distance between the Gaussian models [14]. The above method provides a good research idea for the point cloud feature extraction module of the subsequent LiDAR SLAM algorithm. Zhang J et al. (2014) published the LOAM algorithm with LiDAR as an odometry, which practically applied the LiDAR point cloud feature extraction method in the LiDAR SLAM algorithm [15]. The LOAM algorithm extracts feature points from LiDAR point cloud data using curvature, such as corner points and planar points, and then aligns the LiDAR point cloud by creating constraints between the feature points to achieve map construction and localization. Furthermore, the LOAM algorithm compensates for each feature point’s motion by calculating the relative position of the start and end moments of the LiDAR point cloud, which solves the problem of point cloud distortion caused by LiDAR motion. However, the LOAM algorithm lacks a loop closure detection module, resulting in easy drift and increased error accumulation in large scale scenes. Zhang J et al. (2015) proposed the V-LOAM algorithm to improve the robustness of LiDAR SLAM systems in the absence of visual features and challenging motion scenarios [16]. The V-LOAM algorithm uses high-speed but low-accuracy visual odometry to estimate robot motion and then optimizes the motion estimation and point cloud alignment using the LiDAR scan-match method, which is highly robust in high-speed motion and illumination change scenarios but is generally effective for outdoor mapping. It is worth mentioning that in recent years, LiDAR point cloud clustering segmentation methods based on deep learning have been significantly developed. The PointNet series of methods capture the local structure of the point cloud by designing a point net, which can learn the deep point set features for point cloud clustering segmentation [17,18]. The PointNet method can realize point cloud segmentation quickly and efficiently, but it cannot adequately capture the structural information in ground point cloud data and is sensitive to noise. The RandLA-Net method can directly segment the point cloud for clustering point by point, saving computational resources and memory consumption based on random point sampling [19]. The RandLA-Net method can perform point cloud segmentation efficiently, but in the process of ground point cloud segmentation, it is easy to lose part of the ground features due to the local feature aggregation module used in the method, and it is poorly adapted to sparse or dense point clouds.

In the course of the development of LiDAR point cloud feature extraction methods, the SLAM back-end loop closure detection methods have likewise been widely studied and progressed. G. Grisetti et al. (2010) proposed the GraphSLAM method, which uses a graph to represent the robot’s trajectory and a map of the environment and estimates the position and pose of the loop closure by optimizing the structure and edges of the graph [20]. Latif Y et al. (2013) proposed a method using a probabilistic graphical model that allows loop closure detection in different time steps [21]. The method estimates the position and attitude of the loop closure by minimizing the error between the loop closure constraints and other constraints, thus improving the accuracy and robustness of the SLAM system. ShanTixiao et al. (2018) formally integrated the loop closure detection module into the LiDAR SLAM back end and proposed the LeGO-LOAM algorithm [22]. The LeGO-LOAM algorithm enhances the LOAM algorithm framework by including a loop closure detection and optimization module based on the RNN algorithm.

Although the LiDAR SLAM algorithm framework has been improved, the single use of 3D LiDAR for mapping and localization will inevitably lead to mapping drift and localization failure, etc. In order to solve this problem, the researchers introduced the LiDAR inertial fusion (LiDAR- Inertial) SLAM framework based on the characteristics of the complementary LiDAR and IMU sensing [23]. Ye Haoyang et al. (2019) proposed a LIO-mapping algorithm for tightly coupling LiDAR and IMU [24]. The algorithm minimizes the cost functions from LiDAR and IMU simultaneously to compensate for the shortcomings of a single LiDAR’s insufficient mapping and localization, and it provides more accurate state estimation for LiDAR odometry. Shibo Zhao et al. (2019) proposed the LIOM algorithm [25], which uses a CNN network to remove dynamic targets from each LiDAR scan, a Kalman filter to fuse the LiDAR and IMU data, and a high-frequency output for LiDAR position estimation. Shan Tixiao et al. (2020) proposed the LIO-SAM algorithm, which is more advanced and widely used today [26], based on research into the LeGO-LOAM algorithm. The LIO-SAM algorithm fuses multi-sensing data such as IMU, LiDAR, etc.; builds observation models such as absolute measurement, relative measurement, and loop closure detection into factors; and integrates these factors into a complete factor graph for SLAM back-end optimization, which greatly improves LiDAR SLAM mapping accuracy and robustness compared with other algorithms. However, due to the front-end point cloud feature extraction method, the LIO-SAM algorithm is relatively single and rough, resulting in the algorithm losing its lightweight characteristics, and there is still improvement space for the accuracy of mapping construction. In addition, the LIO-SAM algorithm itself continues the loop closure detection module of the LeGO-LOAM algorithm, and the LiDAR odometry is prone to cumulative drift in large-scale scenarios.

Aiming at the mainstream LiDAR SLAM algorithms, which have the problems of single and unreliable point cloud feature extraction methods, time-consuming back-end loop closure detection, and easy drift of the odometry, this paper proposes a LiDAR-inertial SLAM method using point cloud segmentation and Scan Context based on the state-of-the-art LIO-SAM algorithm. First, we projected and encoded each frame of the LiDAR point cloud to obtain a point cloud range image. Then, in order to extract reliable point cloud features more accurately, we extracted ground points based on range images. It is worth mentioning that this method can extract ground points better than the RANSAC algorithm in outdoor scenes [27,28]. The outdoor scene is large in scale, and the amount of point cloud data acquired by LiDAR is large. When using the RANSAC method for ground point cloud segmentation, the computational efficiency is low and easily falls into the local optimum, the algorithm is time-consuming, and at the same time, it is difficult to obtain ideal segmentation results. Then, we used the BFS method to cluster and label the non-ground points [29] and downsampled the points that failed to cluster (i.e., outlier points). By calculating the curvature, the point cloud clusters with the largest and smallest smoothness were selected from the point clouds under different labels for subsequent point cloud map alignment, which reduced the number of point cloud matches and realized the lightweight quality of the algorithm. Finally, we improved the speed and capability of the algorithm’s back-end loop closure detection through the Scan Context loop closure detection algorithm [30] and improved the algorithm’s odometry drift phenomenon in large-scale scenarios. In order to validate the effectiveness of our proposed method, we tested it on several real datasets such as the KITTI dataset, Walking, Park, etc., and the results showed that our method had better mapping accuracy and real-time performance compared with other state-of-the-art LiDAR SLAM algorithms.

The above research methods provide strong support and a strong reference for the improvement of the LIO-SAM algorithm, but the problems related to the front-end feature extraction and back-end loop closure of the algorithm are still not effectively solved. Therefore, based on the research of the above methods, this paper proposes a LiDAR-inertial SLAM method based on point cloud segmentation and Scan Context, so as to provide an effective solution for the construction of outdoor environment maps.

3. Point Cloud Clustering Segmentation and Feature Extraction

In this section, we propose a point cloud clustering segmentation method based on range image analysis, which uses the BFS algorithm to achieve ground point segmentation and non-ground point clustering. Figure 3 shows the method’s overall framework. First, we converted the LiDAR current frame point cloud into a range image and encoded a 2D image for each LiDAR point, including row and column information. Then, the ground points were segmented, and the non-ground points were clustered. Points where clustering failed (outlier points) were also reflective of environmental features and were large in proportion, so the outlier points were downsampled. Finally, we extracted planar points from ground points and corner points from non-ground points. The non-ground points were mainly composed of outlier points after downsampling and successfully clustered points.

3.1. Convert to Range Image

To significantly increase the efficiency of point cloud data processing, we employed a range image conversion method to transform the current frame LiDAR point cloud into a 2D picture [31]. We illustrate this method of range image conversion with the example of the Velodyne-16 LiDAR used. The Velodyne-16 LiDAR (Velodyne, San Jose, CA, USA) has an angular resolution of ${0.02}^{\circ }$ in the horizontal direction (${\theta }_{hd}$) and $2^{\circ }$ in the vertical direction (${\theta }_{vd}$). Figure 4 shows the specific approach for transforming the current frame LiDAR point cloud into a single range image, from which the coordinates (row and column numbers) of each LiDAR point could be determined using Equations (1)–(4). After the conversion, the current frame of the LiDAR point cloud was turned into a range image with a resolution of $16\times 1800$, with each pixel $r$ representing the target’s measured distance from the LiDAR’s center. Equations (1)–(4) are specified as follows:

$${\theta }_h=\arctan {\displaystyle \frac{y}{x}}$$

(1)

$${\theta }_v=\arctan {\displaystyle \frac{z}{\sqrt{x^2+y^2}}}$$

(2)

$$\mathrm{row}={\displaystyle \frac{{\theta }_{\mathrm{v}}-{\theta }_{\mathrm{v}\min }}{{\theta }_{vd}}}$$

(3)

$$\mathrm{column}={\displaystyle \frac{{\theta }_{\mathrm{h}}+\pi }{{\theta }_{\mathrm{h}\mathrm{d}}}}$$

(4)

$P(x,y,z)$ represents the LiDAR point $P$ with coordinates $(x,y,z)$ in the LiDAR coordinate system. ${\theta }_h$ is the angle between the laser beam’s projection in the $xy$ plane and the $yz$ plane, ${\theta }_v$ is the angle between the laser beam and the $xy$ plane, and (row, column) is the row and column number of the range image to which the LiDAR points are transformed. ${\theta }_{v\min }$ represents the minimum value of the laser beam’s vertical field of view.

3.2. Ground Point Extraction

In this study, the 3D LiDAR sensor was placed horizontally, and the $16$ laser beams were numbered $0\to 15$ sequentially from bottom to top, so the extraction of ground points was limited to the $8$ laser beams numbered $0\to 7$. Figure 5 shows the ground point extraction procedure, in which two nearby laser beams intersected at ground points $A$ and $B$, with coordinates $(x_a,y_a,z_a)$ and $(x_b,y_b,z_b)$, respectively. The vertical height difference between grounding points $A$ and $B$ is $\Delta h$, whereas the plane distance is $d_{AB}$, and the horizontal angle is ${\alpha }_{AB}$. We used Equations (5)–(7) to calculate the horizontal angle ${\alpha }_{AB}$. If the horizontal angle ${\alpha }_{AB}<{10}^{\circ }$, then the grounding points $A$ and $B$ could be considered to be on the same horizontal plane, i.e., points $A$ and $B$ were ground points. Equations (5)–(7) are specified as follows:

$$d_{AB}=\sqrt{{(x_a-x_b)}^2+{(y_a-y_b)}^2}$$

(5)

$$\Delta h=\left|z_a-z_b\right|$$

(6)

$${\alpha }_{AB}=\arctan {\displaystyle \frac{\Delta h}{d_{AB}}}$$

(7)

Figure 6 shows that the above method could successfully extract a large number of ground points from the LiDAR point cloud.

3.3. Point Cloud Clustering Segment

First, we generated a labeled image of the same size as the range image and assigned class labels to the relevant point clouds in the range image. Then, we traversed the non-ground points to find the center point, followed by a search and angle calculation for the four domain points above, below, left, and right of the center point, as shown in Figure 7a. If the angle was greater than the threshold, the center point and neighborhood points were close and belonged to the same class of point cloud. We added labels of the same class to neighborhood points that met the requirements, as well as the current point, at the appropriate positions in the labeled image, until all traversals for such labels were completed. Finally, the non-ground points in the range image that did not have class labels were re-selected as new center points, and the preceding steps were repeated to complete the non-ground point clustering segmentation until all non-ground points in the range image had class labels in the labeled image.

The process of calculating the angle about the clustering decision is shown in Figure 7b. First, we calculated the distance from the center point and the neighborhood point to the LiDAR center; the larger value of the distance among the two was recorded as $d_1$, and the smaller value of the distance was recorded as $d_2$. We denoted $\alpha$ as the angle between the center point and the neighboring point; if the center point and the neighboring point were in the same laser beam, $\alpha ={0.2}^{\circ }$, i.e., the angular resolution of the LiDAR in the horizontal direction. If the center point and the neighboring point were in the upper and lower adjacent two laser beams, $\alpha =2^{\circ }$, i.e., the angular resolution of the LiDAR in the vertical direction. Finally, we wrote down the angle to be computed as $\beta$ and utilized Equation (8) for the calculation.

$$\beta =\arctan {\displaystyle \frac{d_1-d_2\times \cos \alpha }{d_2\times \sin \alpha }}$$

(8)

We can define an angle threshold $t{h}_{\beta }$. If $\beta >t{h}_{\beta }$, it means that the center point and the neighboring points are close to each other and belong to the same class, and should be added with the same class (number) label. After extracting the ground points, the non-ground points were clustered and segmented using the above method to obtain successfully cluster points and outlier points, as shown in Figure 8.

3.4. Corner and Planar Point Extraction

When the curvature exceeded a certain threshold, we chose the current point to be an edge feature point. When the curvature fell below a certain threshold, the current point was selected as a planar feature point. The specific steps were as follows: traversing from the $\mathrm{fifth}$ point on the first laser beam until the end of the penultimate $\mathrm{fifth}$ point of the last laser beam and for the current point $p_i$ taking each of the five points adjacent to the front and back of the same laser beam for the calculation of the curvature $c_i$. Specifically, the calculation was performed using Equation (9):

$$c_i={\displaystyle \frac{1}{\left|S\right|•‖p_{\left(k,i\right)}‖}}‖{\displaystyle \sum _{j\in S,j\ne i}\left(p_{\left(k,i\right)}-p_{\left(k,j\right)}\right)}‖$$

(9)

where $c_i$ denotes the curvature of the current point $p_i$, and $S$ is the set of points consisting of five points each neighboring the current point $p_i$ in front of and behind it. $p_{\left(k,i\right)}$ represents the coordinates of the $i$th LiDAR point in frame $k$ of the LiDAR point cloud. $p_{\left(k,j\right)}$ represents the neighboring point coordinates of point $p_i$ in the point cloud set $S$ in frame $k$ of the LiDAR point cloud.

After calculating the curvature, in order to extract the feature points uniformly, we divided the point cloud of each laser beam scanning for one week into six equal parts and then sorted each part of the point cloud in ascending order according to the curvature. We set the curvature threshold as $c_{th}$. We took $20$ points with curvature values greater than $c_{th}$ as the corner feature points according to the curvature value and took up to four points with curvature value less than $c_{th}$ as planar feature points according to the curvature value. Following clustering and segmentation, the returned ground and non-ground points were used to extract feature points via the previously stated procedure. The results are shown in Figure 9.

4. Loop Closure Detection Based on Scan Context

Figure 10 shows the Scan Context algorithm’s framework. The procedures of the Scan Context algorithm were as follows: first, a single-frame LiDAR point cloud was encoded into Scan Context. After that, a dimension vector was encoded using this Scan Context, and it was utilized to build the KD-Tree and obtain the closest candidate vectors. Finally, the current query’s Scan Context was compared with the searched candidate objects, and the candidate item that most closely matched the current query and met the threshold criterion was chosen as the loop closure.

4.1. The Concept of Scan Context

The reason for the name “Scan Context” is that much like when reading an article, you must relate the context in order to understand the content [32]. First, the LiDAR point cloud of the current frame was divided into $N_r$ and $N_s$ sub-areas according to both the radius and the azimuth, as shown in Figure 11. $N_r$ represents the number of circles divided along the radius direction, and $N_s$ represents the number of subregions divided by each circle according to the azimuthal angle. The farthest LiDAR detection distance used in this paper was $80 \mathrm{m}$, $N_r=20$, $N_s=60$ (default values in the algorithm), so each sub-area contained LiDAR point cloud data within a distance of $4 \mathrm{m}$ and an azimuth of $6^{\circ }$. These sub-areas were numbered sequentially in order, and the set of LiDAR point clouds that fell within the $j$th sub-area on the $i$th circle was denoted as $P_{ij}$.

Given that the height can effectively reflect the shape information of the environmental structure in the vertical direction, and the maximum height can reflect which parts of the environmental structure are visible and unobstructed for the sensors, we assigned a value to each sub-area based on the maximum height of the LiDAR point cloud, as shown in Equations (10) and (11).

$$\varphi :P_{ij}\to ℝ$$

(10)

where $\varphi$ denotes the mapping relationship from the LiDAR point cloud to real numbers in each sub-area.

$$\varphi \left(P_{ij}\right)=\underset{p\in P_{ij}}{\max }z\left(p\right)$$

(11)

where function $z\left(•\right)$ returns the $z$-axis value of the laser point $p$, which is directly assigned to $0$ for the empty sub-area.

After assigning values to each sub-area through Equations (10) and (11), the circle shown in Figure 11 can be expanded along the radius direction to obtain a rectangular matrix. The Scan Context matrix with information about the structure of the environment is shown in Figure 12.

4.2. Scan Context Similarity Calculation

The Scan Context matrices obtained by converting the current frame point cloud and the history frame point cloud by the above method were set to be $I^q$ and $I^c$, respectively, and we used the column-wise distance method to calculate the similarity between $I^q$ and $I^c$. Setting the $j$th column vectors of matrices $I^q$ and $I^c$ as $c_j^q$ and $c_j^c$, respectively, the similarity of matrices $I^q$ and $I^c$ can be calculated using Equation (12):

$$d\left(I^q,I^c\right)={\displaystyle \frac{1}{N_s}}{\displaystyle \sum _{j=1}^{N_s}\left(1-\frac{c_j^q•c_j^c}{‖c_j^q‖‖c_j^c‖}\right)}$$

(12)

The comparison between the history frame point cloud and the current frame point cloud using Equation (12) did not take into account the case where the viewing angle and position of the LiDAR observation had changed. Even though the viewing angle changed when the LiDAR returned to the same place, only the order of the column vectors would change in the Scan Context matrix for the current frame transformation, as shown in Figure 13.

In order to alleviate the problem of shifting the Scan Context matrix column vectors due to the change in the viewing angle of the LiDAR sensor, we translated the history frame $I^c$ by columns to obtain $N_s$ Scan Contexts and calculated the distance between the shifted Scan Contexts and the Scan Context of the current frame in turn, as shown in Equation (13), and then selected the frame with the smallest distance as the history frame for the loop closure matching. After finding the matching history frame, the column translations corresponding to the current frame and the history frame were calculated as shown in Equation (14):

$$D\left(I^q,I^c\right)=\underset{n\in N_s}{\min }d\left(I^q,I_n^c\right)$$

(13)

$$n^{*}=\underset{n\in N_s}{argmin}d\left(I^q,I_n^c\right)$$

(14)

where, $I_n^c$ is the new Scan Context matrix obtained by moving the last $n$ columns of the Scan Context matrix corresponding to the history frame $I^c$ to the top. $n^{*}$ is the best solution for $n$.

4.3. Two-Step Search Strategy

The direct search for $n$ according to the above method was computationally too large, so we used the following two-step search strategy. First, the $\mathrm{Ring}\hspace{0.33em}\mathrm{Key}$ descriptor was defined, and the function $\psi$ was used to encode one frame of the point cloud from the inside to the outside of one ring $r$, i.e., to encode the row vectors of the Scan Context matrix:

$$\psi \left(r_i\right)={\displaystyle \frac{{‖r_i‖}_1}{N_s}},i=1,2\cdots N_r$$

(15)

where, $r_i$ denotes the $i$th row vector of the Scan Context matrix. Equation (15) denotes the encoding of a row vector as the average of all elements of that row vector. An $N_r$-dimensional column vector $k$ can be obtained after encoding the $N_r$ circles according to Equation (15).

$$k={\left[\psi \left(r_1\right),\cdots ,\psi \left(r_{N_r}\right)\right]}^T,\psi :r_i\to ℝ$$

(16)

The KD-Tree was constructed by vector $k$. The $\mathrm{Ring}\hspace{0.33em}\mathrm{Key}$ descriptor of the current frame was used to take the KD-Tree to search for the $m$ most similar LiDAR point cloud scans. Then, we found $n$ to solve the above problem of column vector shift of Scan Context caused by the change in the LiDAR viewing angle. The similarity between the Scan Context matrix corresponding to the current frame and the Scan Context matrix corresponding to these $m$ scans was compared one by one by (12). In order to improve the computational efficiency of finding $n$, $\mathrm{S}\mathrm{e}\mathrm{c}\mathrm{t}\mathrm{o}\mathrm{r}\hspace{0.33em}\mathrm{K}\mathrm{e}\mathrm{y}$ descriptors are defined by Equations (17) and (18), and $\mathrm{S}\mathrm{e}\mathrm{c}\mathrm{t}\mathrm{o}\mathrm{r}\hspace{0.33em}\mathrm{K}\mathrm{e}\mathrm{y}$ descriptors were constructed for the Scan Context matrices of both the current frame and the candidate frames:

$$\zeta \left(c_i\right)={\displaystyle \frac{{‖c_i‖}_1}{N_r}},i=1,2\cdots N_s$$

(17)

$$l=\left[\xi \left(c_1\right),\cdots ,\xi \left(c_{N_s}\right)\right],\xi :c_i\to ℝ$$

(18)

where $c_i$ denotes the $i$th column vector of the Scan Context matrix. Equation (17) denotes the encoding of a column vector as the average of all elements of that column vector. An $N_s$-dimensional row vector $l$ can be obtained after encoding the $N_s$ circles according to Equation (17), i.e., the $\mathrm{S}\mathrm{e}\mathrm{c}\mathrm{t}\mathrm{o}\mathrm{r}\hspace{0.33em}\mathrm{K}\mathrm{e}\mathrm{y}$ descriptor.

Solve $n^{*}$ by traversing the Sector Key descriptor of the candidate frame after how many column shifts to obtain the row vector $l_n^c$ most similar to the Sector Key descriptor $l^q$ of the current frame. The difference between the two row vectors was measured by the two-paradigm measure as shown in Equation (19). The computation of A is shown in Equation (20):

$$d^{\prime }\left(l^q,l_n^c\right)={‖l_n^c-l^q‖}_2$$

(19)

$$n^{*}=\underset{n\in N_s}{argmin}d\left(l^q,l_n^c\right)$$

(20)

It should be noted that Equations (13) and (14) are general expressions for solving the optimal column shift $n^{*}$, and Equations (19) and (20) are specific forms for solving the optimal column shift $n^{*}$ by means of the $\mathrm{S}\mathrm{e}\mathrm{c}\mathrm{t}\mathrm{o}\mathrm{r}\hspace{0.33em}\mathrm{K}\mathrm{e}\mathrm{y}$ descriptor.

After solving the optimal column shift number for each candidate frame by the $\mathrm{S}\mathrm{e}\mathrm{c}\mathrm{t}\mathrm{o}\mathrm{r}\hspace{0.33em}\mathrm{K}\mathrm{e}\mathrm{y}$ descriptor, the column vector of each candidate frame was optimally adjusted. Utilizing Equation (12) to compare the similarity between the Scan Context matrix corresponding to the current frame and the Scan Context matrix corresponding to the above $m$ scans one by one, the minimum distance between the adjusted candidate frame and the current frame was denoted as $d_i$, $i=1,\cdots ,m$. If $d_i$ was less than a set threshold, the candidate frame after column shifting was the optimal candidate frame, and the number of moving columns $n^{*}$ of this candidate frame was recorded, then the angle at which the LiDAR of the current frame was turned relative to the LiDAR at the candidate frame was

$$\stackrel{\wedge }{\theta }={\displaystyle \frac{2\pi n^{*}}{N_s}}$$

(21)

We could obtain the index of the current frame and the loop closure frame using the Scan Context method, as well as a better initial value of the relative pose estimation between the two frames, and then use the ICP algorithm to obtain the precise pose constraints between the two frames, so we will not go into detail about the specific introduction.

5. Experimental Results

In this section, we present experimental validation of map construction accuracy and back-end mapping in real time. We tested the accuracy of our algorithm based on the KITTI dataset with a given ground truth and compared it with other state-of-the-art algorithms. To ensure adequate validation of the algorithms, we compared and evaluated our algorithm based on the most recently collected real-world datasets from Walking, Park, and others. The controller was a computer and was equipped with an i7-10710U processor (Intel Corporation, Santa Clara, CA, USA), which used the melodic version of the Robot Operating System (ROS) in Ubuntu 18.04, and the algorithms involved in this experiment were based on C++ implementation.

5.1. Dataset

We fully validated the algorithm’s accuracy through extensive testing on the KITTI dataset [33]. The KITTI dataset was converted to a packet in the form of a $.\mathrm{bag}$, which contained a $10 \mathrm{Hz}$ LiDAR point cloud, $100 \mathrm{Hz}$ IMU data, and $100 \mathrm{Hz}$ GPS absolute measurements. Additionally, we fully validated the algorithm’s real-time performance based on the most recent actual collected datasets, such as Walking and Park, provided by the LIO-SAM algorithm.

5.2. Evaluation

To accurately measure the accuracy of the algorithms in this study, an experimental comparison of the absolute trajectory error (ATE), which focuses on globally consistency in algorithm outcomes, was performed using the EVO evaluation tool. ATE compares the estimated position values output from the algorithm with the ground truth of the dataset according to the time stamps on a frame-by-frame basis, and then calculates the position difference between the estimated values and the ground truth on each set of corresponding timestamps, so as to realize an important measure of the global consistency of the trajectory, which is very suitable for evaluating the performance of the SLAM system and the accuracy of the trajectory.

Assuming that the trajectory position estimated by the SLAM algorithm at moment $t$ is $P_t$ and the position of the real trajectory at that moment is $G_t$, the ATE at that moment is defined as follows:

$$F_t=G_t^{-1}{P}_t$$

(22)

In order to be able to better reflect the relative error situation, with the total number of positions $T$ and the time interval $\Delta$ known, the root mean squared error (RMSE) was utilized to count the errors of all frames to obtain an overall value.

$$RMSE\left(F\right)={\left({\displaystyle \frac{1}{n}}{\displaystyle \sum _{t=1}^n{‖trans\left(F_t\right)‖}^2}\right)}^{{\displaystyle \frac{1}{2}}}$$

(23)

where $n=T-\Delta$, and $trans$ denotes that the translation portion was taken for the variables inside the parentheses.

In conclusion, we used the KITTI dataset to evaluate the absolute trajectory errors of the algorithms in this research.

5.3. Results

In order to evaluate the effectiveness of our method, we performed various tests of the proposed algorithm on the KITTI dataset, with a special focus on the sequences 0016 (No. 04), 0027 (No. 07), 0034 (No. 02), and 0042 (No. 01). We performed a careful comparison with several state-of-the-art and typical methods through these sequences in the order of KITTI dataset numbering. The results of the trajectory accuracy comparison are shown in Figure 14 and Figure 15.

As seen in Figure 14 and Figure 15, compared with A-LOAM, LeGO-LOAM, and LIO-SAM algorithms, the overall match between the trajectories estimated by our method and the ground truth of the dataset was higher, and the fitting results were better. The LeGO-LOAM algorithm data were not counted in Figure 14a because the LeGO-LOAM algorithm drifted during the KITTI-0042 sequence mapping process. The trajectories estimated by the four algorithms had some deviation from the ground truth, but as can be seen by the localized enlarged view of the complex comparison of the trajectories of sequence 0034 and sequence 0027, our method was closer to the ground truth of the trajectories and achieved a better positional estimation result both in straight lines and at corners. Since the features extracted by our method in the front end were more accurate and reliable, the point cloud had less error in the map matching process, thus reducing the mapping error. Second, the back end of our method had a robust loop closure detection capability, which was not affected by the cumulative error of the LiDAR odometry and could further eliminate the cumulative error in the mapping process. As a result, our mapping results fit the ground truth more satisfactorily.

As can be seen in Figure 16, our algorithm obtained the smallest ATE.Max results in the KITTI-0042, KITTI-0034, and KITTI-0016 datasets, which were 36.54 m, 19.54 m, and 0.864 m, respectively. It can be seen by fitting the color results with the ground truth that the algorithm in this paper had a better overall fit with the ground truth and a lower error level compared with the A-LOAM, LeGO-LOAM, and LIO-SAM algorithms. It directly illustrates that the algorithm in this paper was much closer to the ground truth, and the maximal error was smaller. The ATE error data are shown in

Table 1. ATE results of the four algorithms on the KITTI dataset 0042 sequence (m).

	ATE.Max	ATE.Mean	ATE.Median	ATE.Min	ATE.RMSE	ATE.SSE	ATE.STD
A-LOAM	47.832483	18.72789	19.434571	0.224138	20.374159	265753.488	9.77424
LeGO-LOAM	fail	fail	fail	fail	fail	fail	fail
LIO-SAM	38.701464	18.453796	19.317356	3.063197	20.258078	226124.735	8.35746
Ours	36.542054	17.765455	18.670118	3.468393	19.443158	208298.043	7.90095

,

Table 2. ATE results of the four algorithms on the KITTI dataset 0034 sequence (m).

	ATE.Max	ATE.Mean	ATE.Median	ATE.Min	ATE.RMSE	ATE.SSE	ATE.STD
A-LOAM	56.972621	27.81383	26.041961	1.247682	31.10942	3166628.01	13.935097
LeGO-LOAM	183.97580	59.16251	52.638437	21.49668	65.23284	4953196.65	27.479466
LIO-SAM	19.93554	8.443412	7.577774	0.583131	9.308295	201967.999	3.91831
Ours	19.540422	8.283499	7.911282	0.7737	9.013449	189375.728	3.553295

,

Table 3. ATE results of the four algorithms on the KITTI dataset 0016 sequence (m).

	ATE.Max	ATE.Mean	ATE.Median	ATE.Min	ATE.RMSE	ATE.SSE	ATE.STD
A-LOAM	1.137166	0.305874	0.274523	0.100661	0.359983	23.97372	0.189812
LeGO-LOAM	2.074986	0.553511	0.475358	0.161224	0.639678	27.4155552	0.320645
LIO-SAM	0.867199	0.256773	0.228004	0.112968	0.289565	11.403665	0.133858
Ours	0.864057	0.256678	0.226332	0.112585	0.288599	11.327375	0.131932

and

Table 4. ATE results of the four algorithms on the KITTI dataset 0027 sequence (m).

	ATE.Max	ATE.Mean	ATE.Median	ATE.Min	ATE.RMSE	ATE.SSE	ATE.STD
A-LOAM	1.04283	0.540099	0.551983	0.135299	0.768797	274.677392	0.17839
LeGO-LOAM	5.249547	3.218281	3.063042	0.641702	3.408125	3182.597489	1.1216
LIO-SAM	1.445858	0.677021	0.638876	0.13041	0.763134	320.887515	0.352158
Ours	1.158319	0.533615	0.507074	0.081021	0.584182	188.03915	0.237748

.

It can be seen from Table 1, Table 2, Table 3 and Table 4 that the A-LOAM and LIO-SAM algorithms achieved better results in a small number of indicators. However, compared with algorithms such as A-LOAM, LeGO-LOAM, and LIO-SAM, our method achieved better results in most of the indicators of ATE. Our algorithm had obvious advantages in the evaluation of metrics such as ATE.Mean, ATE.RMSE, and ATE.SSE, which further validated that our method predicted trajectories that were closer to the ground truth with less error. The local point cloud features were too sparse, resulting in a large ATE.Min value for our method. However, the ATE.Max and ATE.Mean values were smaller, which also proved that our method had a better consistency of mapping results and higher accuracy.

Next, in order to effectively verify the loop closure detection effect of our method, we conducted a loop closure detection comparison experiment with the RNN algorithm, and the experimental results are shown in Figure 17. For ease of observation, we rendered the two mapping results using different color schemes.

As can be seen through Figure 17a,b, the traditional RNN-based loop closure detection had several loop closure failures, whereas the loop closure detection results based on our method were better, and the desired loop closure was achieved. The main reason for the difference in this result was that the traditional RNN-based loop closure detection method was susceptible to the influence of the cumulative error of the LiDAR odometry, whereas the Scan Context-based loop closure detection method had rotational invariance, which avoided the influence of the cumulative error of the LiDAR odometry on loop closure detection. In addition, by comparing Figure 17a and Figure 17b, it can be seen that the map construction results of our method were clearer, which indirectly verified the map construction accuracy of our method. Compared with several other algorithms, the method in this paper extracted reliable point cloud features at the front end and therefore had a high accuracy of map construction.

In order to fully validate the real-time performance of our method, in addition to the KITTI dataset, we also used the Walking and Park datasets collected in real scenarios provided by the LIO-SAM algorithm. The mapping results of our method based on the Walking and Park datasets are shown in Figure 18, with localized mapping zoom results appended to the mapping results.

As can be seen in Figure 18, the mapping results of our method did not show large or obvious drift, and the local mapping results can clearly show the environmental structure and details when enlarged. Among them, Figure 18a can show clear localized building structures when compared with the results of LOAM, LIOM, and LIO-SAM mapping in the literature [23]. This also directly indicated that our method built maps with high accuracy.

Table 5. Runtime of mapping for processing one scan (ms).

	A-LOAM	LeGO-LOAM	LIO-SAM	Ours
KITTI-0042	226.2	fail	81.3	73.7
KITTI-0034	195.5	45.6	66.9	58.7
KITTI-0016	180.9	50.9	48	42.1
KITTI-0027	104	37.1	22.5	19.8
Walking	253.6	fail	58.4	50.2
Park	266.4	fail	100.5	92.1

shows the average running time of the four algorithms to register a LiDAR frame on the KITTI dataset and the Walking and Park datasets.

As shown in Table 5, our method’s runtime decreased when compared with the A-LOAM, LeGO-LOAM, and LIO-SAM algorithms, making it more suitable for deployment in low-powered situations. In the above dataset, the minimum construction time for our method to process a single-frame point cloud was 19.8 ms, and the maximum was 92.1 ms. The mapping time was reduced by an average of 69.57% compared with the A-LOAM algorithm and even 11.38% compared with the state-of-the-art LIO-SAM algorithm.

6. Conclusions

Based on the research of the LIO-SAM algorithm, we proposed a map construction method suitable for an outdoor environment. First, we realized ground point extraction based on the range image transformation method, which can reduce the point cloud dimension and improve the reliability of ground point extraction. Second, we traversed and clustered the segmentation of non-ground point clouds by a BFS algorithm, which effectively avoided the increased computation of a global search and could significantly improve the clustering speed when the distribution of point clouds was not uniform. Finally, we introduced the Scan Context loop closure detection method in the back end of the algorithm, which improved the accuracy of the algorithm’s loop closure detection. We fully validated the overall mapping accuracy and real-time performance of the method in this paper with a variety of datasets such as KITTI, Walking, and Park.

In the future work, we will refer to the deep learning method to adaptively adjust the angle threshold in the point cloud clustering segmentation process and construct a new type of loop closure detection descriptor to further improve the robustness of the algorithm in the process of mapping in outdoor environments.