An Offline Coarse-To-Fine Precision Optimization Algorithm for 3D Laser SLAM Point Cloud

Abstract

3D laser simultaneous localization and mapping (SLAM) technology is one of the most efficient methods to capture spatial information. However, the low-precision of 3D laser SLAM point cloud limits its application in many fields. In order to improve the precision of 3D laser SLAM point cloud, we presented an offline coarse-to-fine precision optimization algorithm. The point clouds are first segmented and registered at the local level. Then, a pose graph of point cloud segments is constructed using feature similarity and global registration. At last, all segments are aligned and merged into the final optimized result. In addition, a cycle based error edge elimination method is utilized to guarantee the consistency of the pose graph. The experimental results demonstrated that our algorithm achieved good performance both in our test datasets and the Cartographer public dataset. Compared with the reference data obtained by terrestrial laser scanning (TLS), the average point-to-point distance root mean square errors (RMSE) of point clouds generated by Google’s Cartographer and LOAM laser SLAM algorithms are reduced by 47.3% and 53.4% respectively after optimization in our datasets. And the average plane-to-plane distances of them are reduced by 50.9% and 52.1% respectively.

1. Introduction

Three-dimensional spatial information plays an important role in many applications such as indoor navigation [1], building information model (BIM) construction [2], and forest inventory [3]. Recently, light detection and ranging (LiDAR) technology has become one of the most important methods to efficiently capture 3D spatial information. Through integration with the global navigation satellite system (GNSS)/ inertial navigation system (INS), mobile laser scanning (MLS) has achieved great success in the field of 3D spatial information collection. However, it cannot work in a GNSS signal denied environment [4]. Meanwhile, the conventional 3D data collection means in this environment is terrestrial laser scanning (TLS) which is less efficient than dynamic laser scanning and requires artificial targets sometimes. Consequently, for the advantages of having no reliance on GNSS signal and efficiency, laser simultaneous localization and mapping (SLAM) technology has become an important development direction in indoor and underground environments mapping [5,6].

The concept of SLAM technology originated in the field of robotics [7,8]. The purpose of SLAM is to realize the navigation of mobile robots in unknown environments in the absence of external referencing systems. Thus, the early laser SLAM algorithms usually used 2D LiDAR as the main sensor, and the final mapping result was a 2D occupancy grid map [9,10,11]. With the development of multi-beam LiDAR and increasing demands for 3D spatial information, researchers proposed more and more 3D laser SLAM algorithms [12,13,14,15,16,17,18]. Lidar odometry and mapping (LOAM) is one of the most popular laser SLAM algorithms, which can use a rotation 2D LiDAR or multi-beam LiDAR to generate 3D point cloud [19]. It divides the SLAM algorithm into two modules: high-frequency LiDAR odometry and low-frequency LiDAR mapping. The odometry module extracts feature points on sharp edges and planar surface patches in the point cloud and then finds the corresponding points in the previous point cloud to estimate the motion of the sensor. The mapping module registers and adds the feature points accumulated by odometry module into a global point cloud map for the next iteration. Visual-LOAM combines visual odometry and LiDAR odometry to improve accuracy and robustness [20]. Another variant of LOAM, lightweight and ground optimized LOAM (LeGO-LOAM), leverages the presence of the ground plane to estimate the translation along elevation axis and roll, pitch angle [21]. Lidar inertial odometry (LIO) is the combination of LOAM and visual-inertial systems (VINS) [22] that introduces inertial measurement unit (IMU) pre-integration to address the problem under fast motion and insufficient features conditions [23]. Cartographer is another state-of-the-art laser SLAM proposed by Google [24]. It is a graph-based SLAM algorithm that consists of a front-end and back-end [25]. In the front-end, the new point cloud is aligned to gravity first and registered into a probability grid as a node. Then the probability grid will be saved as a submap periodically. The constraints between nodes and submaps are computed by the loop closure detection module. The back-end uses nonlinear optimization algorithms to estimate all poses of nodes and submaps simultaneously to optimize the global point cloud map.

Limited by the real-time requirement of navigation application, these SLAM algorithms have to put more emphasis on efficiency than accuracy. This means that the internal conformity of the point cloud obtained by laser SLAM cannot be ensured. The disadvantage in precision prevents the further application of the laser SLAM point cloud. Therefore, some algorithms are proposed to improve the precision of the laser SLAM point cloud. The fundamental idea of these algorithms is using the registration results of overlapped point clouds to reduce cumulative errors. In addition, the constraint relationship between point clouds is usually represented as a pose graph. For instance, to improve the mapping result of Cartographer, Nüchter et al. [26,27] first split the trajectory into sections and used a heuristic that measures the overlap of sections using the number of closest point pairs to estimate the constraints between sections. Then a pose graph was constructed by the registration results. Using the solution of pose graph as the initial value, a semi-rigid matching described in [28] was applied to optimize the global point cloud. This algorithm did not utilize the features of point cloud, which makes it able to work in a featureless environment. For example, in [29], Lauterbach et al. applied this continuous-time optimization algorithm to correct the result of Hector SLAM in lava tube [30] successfully. For the same reason, it cannot detect loop closure using features, which means it cannot correct the severely distorted trajectories. Kukko et al. [31] first extracted tree stem features, then used them to recognize the point clouds of the same trees and match point clouds to construct a pose graph. As the tree stem features can only be used in the forest environment, the application environment of the algorithm is limited. Zhu et al. [32] introduced visual information to detect loop closure. In that study, point clouds were segmented by the image keyframes and a bag-of-words model was used to decide loop candidates. Vlaminck et al. [33] presented a novel registration algorithm to determine loop candidates using a signature based on the projection of the point cloud on several 2D planes. Then, the candidates were verified by the residual of iterative closest point (ICP) and the positions of object clusters. All of the above algorithms can only reject error loop candidates in pairwise registration phase.

In this paper, we propose an offline coarse-to-fine optimization algorithm for laser SLAM point cloud based on feature registration and a novel pose graph validation method. First, the raw point cloud of laser SLAM is segmented by time and the point cloud in each segment is registered sequentially to reduce online registration errors inside the segment. Then, the features of each segment are computed and a pose graph is constructed by registering the point cloud segment pairs with high similarity. In next phase, we leveraged the cycle struct in the pose graph to eliminate error edges. This method can use the information of point clouds belongs to one cycle, instead of only the information of pairwise point clouds. After removing the error edges, we solved the pose graph and optimized the global point cloud.

The rest of the paper is organized as follows: Section 2 presents the proposed coarse-to-fine optimization algorithm. Section 3 introduces the experiment datasets and evaluates the algorithm performance qualitatively and quantitatively. Section 4 discusses the experimental results and future research directions. Finally, we conclude our works in Section 5.

2. Methods

We used pose graph-based global point cloud registration method to improve the precision of laser SLAM point cloud. Figure 1 illustrates an overview of the proposed optimization algorithm framework. The inputs of our algorithm are raw point cloud and trajectory generated by laser SLAM algorithm. The main phases of the algorithm are as follows:

• Local optimization: the raw point cloud is first segmented by time and processed to remove noise. The normal vector of each point is estimated. Then, in each segment, setting the first frame point cloud as reference, all point clouds are registered sequentially by point-to-plane ICP [34] and merged.
• Pose graph construction: we use the features of each point cloud segment to calculate the similarity between different segments. The point cloud segments with high similarity are registered based on features matching and ICP. Lastly, a pose graph is constructed according to the obtained registration result.
• Global optimization: a cycle based error edge elimination method is performed to ensure the consistency of the pose graph. Then, the poses of each point cloud segment are computed by solving the pose graph. Finally, all segments are registered using the estimated pose.

The following sections will provide a detailed description of each phase.

2.1. Local Optimization

Due to the huge number of frames in laser SLAM point cloud and the complicated overlapping relationship between them, it is hard to register all point cloud frames at the same time like multi-view registration of TLS point clouds. We adopted a coarse-to-fine optimization strategy to register these point clouds. Thus, we divided these point clouds into different segments according to time interval $T_s$ to register them in local level firstly.

In pre-processing, the points whose distance to scan center exceeds the threshold $D_{max}$ were removed for the reason that the measurement accuracy of LiDAR and point density decrease as the scanning distance increases. Then, the noise in point cloud was removed by statistical outlier removal filter [35]. The filter trims the points whose mean distances with neighborhood points are outside an interval defined by the global distances mean and standard deviation. The normal vectors of the remaining points were calculated by performing principal component analysis (PCA) on the covariance matrix of their k-nearest neighbors [36].

In the sequential registration, all point clouds are aligned with the first frame point cloud in the segment. However, if only using the previous frame point cloud as the reference, the registration error will accumulate rapidly, which means the point cloud segment after registration still cannot be treated as a rigid body. Using all registered point cloud frames as the reference point cloud will slow down the speed of registration, while the point clouds that have no overlapping with current point cloud have no contribution to registration. Therefore, to strike a balance between accuracy and efficiency, we used point clouds in sliding window as the reference. The new registered point cloud was added to the sliding window continuously, and when the size of point cloud in it exceeded the threshold $W_{size}$, the oldest point cloud was removed to prevent the reference point cloud from being too large. In addition, to accelerate the registration, the point clouds were resampled with voxel size $V_{size}$ previously.

We chose the point-to-plane ICP as the registration algorithm, for it achieved the best performance in comparison with standard ICP and generalized ICP [37] in our datasets. Figure 2 shows the registration result of point-to-plane ICP, and it can be seen that the error inside the segment is obviously reduced, for example, the thickness of the wall decreases. Different to the standard ICP which uses the point to point metric, it minimizes a point to plane metric based objective function as following equation:

$$E={\displaystyle \sum }_{i=1}^n{\Vert ({RP}_i+T-Q_i)·N_i\Vert }^2,$$

(1)

where $P_i$ and $Q_i$ are the correspondence point in source and reference point cloud respectively; $N_i$ is the normal vector of point $Q_i$; $n$ is the number of correspondence points; $R\in {ℝ}^{3\times 3}$ is an orthonormal matrix representing the rotation part of transformation; $T\in {ℝ}^{3\times 1}$ is a vector representing the translation part of transformation.

2.2. Pose Graph Construction

There are two types sources of overlapping relationship in the laser SLAM point cloud: (1) consecutive point cloud; (2) revisit of the same place. As the point cloud with large deviation cannot be corrected only using the first type of adjacent relationship, it is important to find the second type of overlapping relationship. Performing the exhaustive pairwise registration between point cloud segments to retrieve the underlying overlapping relationship is extraordinarily time-consuming, and will produce a large number of false overlapping relationships. Therefore, based on the hypothesis that overlapping point cloud segments have higher similarity, we used the feature similarity as criteria to find the candidates of overlapping segments.

2.2.1. Feature Similarity Computation

The feature similarity between point cloud segments was computed using the vector of locally aggregated descriptors (VLAD) [38]. It is a feature encoding method that aggregates local descriptors into a vector as global descriptor. In this section, the VLAD descriptor of each point cloud segment is computed by following steps:

• Extracting the keypoints in each point cloud segment by intrinsic shape signatures (ISS) keypoint detector [39];
• Computing the fast point feature histograms (FPFH) [40] local descriptors of each keypoint extracted in the last step;
• Aggregating the FPFH descriptor to compute the global descriptor of each segment using following equation:

  $$\{\begin{array}{c}{V}_i={\displaystyle {\displaystyle \sum }_{f \in NN\left(f\right) = c_i }}f- c_i\\ D_{vlad}=\frac{\left[V_1, V_2, \dots ,V_k\right]}{{\Vert \left[V_1, V_2, \dots ,V_k\right]\Vert }_2} \end{array},$$

  (2)

  where $c_i$ belongs to the set of visual word $C=\left\{c_1, c_2, \dots ,c_k\right\}$; the visual words are the k-means clustering centers of all FPFH descriptors; $k$ is the number of cluster centers; $NN\left(f\right)$ is a function that finds the nearest visual word of the local descriptor $f$; $\left[V_1, V_2, \dots ,V_k\right]$ represents the concatenation of vectors.

After the computation of VLAD descriptors is finished, the similarity between two point cloud segments can be calculated by the method in [41] as

$$s\left(D_{vlad}^i, D_{vlad}^j\right)= \frac{1}{K}{\displaystyle {\displaystyle \sum }_{k=1, 2 \dots , K}}{w}_k\frac{1}{\Vert D_{vlad}^i\left(k\right)- D_{vlad}^j\left(k\right)\Vert },$$

(3)

where $D_{vlad}^i$ and $D_{vlad}^j$ are the VLAD descriptors of different segments; $D_{vlad}\left(k\right)$ is the $k$th component of descriptor; $w_k=1- \frac{k}{K}$ is the weight of $k$th component, and the component pairs are sorted in descending order with respect to their similarity. The function of this weight is amplifying the influence of the more similar component pairs, for the overlapping part of two segments is more important than the other parts.

2.2.2. Point Cloud Segments Global Registration

After obtaining the similarity between different segments, we only used the most similar n pairs and consecutive pairs for global registration to avoid redundant computation. As there is only small drift between two consecutive segment pairs, it is feasible to perform the point-to-plane ICP directly to register them. However, this method does not work for the inconsecutive segment pairs. Thus, performing the fast global registration [42] algorithm first to provide the initial value for ICP is necessary. It is a high computational efficiency coarse registration algorithm for it only establishes correspondences once during the optimization. The objective function of fast global registration algorithm is

$$E\left(R,T\right)= {\displaystyle {\displaystyle \sum }_{\left(p,q\right)\in C}}\rho \left(\Vert \left(Rp+T-q\right)\Vert \right),$$

(4)

$$\rho \left(x\right)= \frac{\mu x^2}{\mu +x^2},$$

(5)

where $p$ and $q$ are the correspondence point in source and reference point cloud respectively; $\rho \left(x\right)$ is a robust penalty function; $\mu$ controls the range within which residuals have a significant effect on the objective function.

The initial correspondences were established by reciprocity search using the FPFH feature point obtained in Section 2.2.1. Only the correspondence pair that both points are the nearest point each other was used. Then a tuple test was used to improve the quality of the correspondences. Three correspondence pairs were randomly picked to test their compatibility using following condition:

$$\forall i\ne j, \tau <\frac{\Vert p_i-p_j\Vert }{\Vert q_i-q_j\Vert }<\frac{1}{ \tau },$$

(6)

where $\tau$ = 0.9, the tuples that do not pass the test were removed from initial correspondences.

2.2.3. Pose Graph Model

In this section, we constructed a pose graph for registering all point cloud segments using the results in the previous section. As shown in Figure 3, a pose graph consists of nodes and edges: the nodes represent the poses of point cloud segments, and the edges represent the constraints between the point cloud segments. We used the relative pose of two point cloud segments after registration as constraints and minimize the sum of residuals in the pose graph to get the optimal pose of each segment.

The node $x_i= {\left[t_i^T,q_i^T\right]}^T$, where $t_i= {\left[x_i, y_i,z_i\right]}^T$ is the translation component of pose, $q_i= {\left[q{w}_i, q{x}_i,q{y}_i,q{z}_i\right]}^T$ is a quaternion that represents the rotation component of pose. The edge $e_{ij}=\left[t_{e_{ij}},q_{e_{ij}}\right]$ connecting node $x_i$ and $x_j$ is formed as

$$e_{ij}=\left[\begin{array}{c}{q}_i^{-1}\left(q_{i,j}{t}_j+t_{i,j}-t_i\right)\\ q_i^{-1}{q}_{i,j}{q}_j\end{array}\right],$$

(7)

where $t_i$ and $q_i$ represent the pose of the reference segment; $t_j$ and $q_j$ represent the pose of the source segment; $t_{i,j}$ and $q_{i,j}$ are the registration result of these two segments. Besides, the residual $r\left(x_i, x_j,e_{ij}\right)$ of the edge $e_{ij}$ is

$$r\left(x_i, x_j,e_{ij}\right)=\left[\begin{array}{c}{q}_i^{-1}\left(t_j-t_i\right)- t_{e_{ij}}\\ 2·\mathrm{vec}\left(q_{e_{ij}}^{-1}\left(q_i^{-1}{q}_j\right)\right)\end{array}\right],$$

(8)

where $\mathrm{vec}\left(q\right)=\left[q{x}_i,q{y}_i,q{z}_i\right]$, $2·\mathrm{vec}\left(q\right)$ is the approximate conversion from quaternion to Euler angle when rotation angle is close to zero.

The objective function for calculating the optimal pose variables $\widehat{X} = \left\{{\widehat{x}}_1,{\widehat{x}}_2\dots {\widehat{x}}_{\mathrm{n}}\right\}$ is

$$\widehat{X}= {\displaystyle \underset{X}{\mathrm{argmin}}}\left[\left(x_0-x_0^{\prime }\right){\mathsf{\Omega }}_0\left(x_0-x_0^{\prime }\right)+ {\displaystyle {\displaystyle \sum }_{e_{ij}\in E}}r\left(x_i, x_j,e_{ij}\right){\mathsf{\Omega }}_{ij}r\left(x_i, x_j,e_{ij}\right)\right],$$

(9)

where $x_0^{\prime }$ is the initial value of $x_0$; ${\mathsf{\Omega }}_0$ is a diagonal matrix and its diagonal element is positive infinity;${\mathsf{\Omega }}_{ij}$ is the covariance matrix derived by point-to-plane ICP [43]; $E$ is the set of registration results in previous section.

2.3. Global Optimization

Since it is crucial to eliminate the error edges in a pose graph to guarantee the consistency of pose graph, we performed our cycle based error edge elimination method before optimization. Unlike most existing methods that verify the validity of the registration result by just leveraging the information of pairwise point cloud, our method validates the edges belonging to one cycle simultaneously. The basic idea of our method is that the correct registration results should be compatible with each other. In this section, all cycles in the pose graph are enumerated first; then a consistency validation is conducted for these cycles to detect the error edge; finally, the pose graph is solved to optimize the global point cloud.

2.3.1. Enumerating Cycles in the Pose Graph

A pose graph is a kind of undirected graph, and all cycles in an undirected graph can be found by the permutation and combination of fundamental cycles [44]. As shown in Figure 4, adding a missing edge to minimal spanning tree can construct a fundamental cycle. For minimal spanning the tree of a graph is not unique, the fundamental cycle set is also not unique, but the number of fundamental cycles $N_{fc}$ is same and can be calculated using

$$N_{fc}=E-V+1,$$

(10)

where $E$ is the number of edges in graph; $V$ is the number of nodes in graph. In addition, all fundamental cycle sets are equivalent. In this section, we use the fundamental cycle set constructed by depth-first search minimal spanning tree.

Having established the fundamental cycle set, Algorithm 1 describes how to enumerate all remaining cycles. The operator $Xor$ in line 11 removes the edges which are present in both input sub-graphs and then merges the rest of sub-graphs as shown in Figure 5. The function isCycle() in line 13 judges whether the input graph is a cycle. It is implemented by counting the number of edges during a deep-first traversal. If the result is equal to the total number of edges, the input graph can be confirmed as a cycle.

Algorithm 1: Enumerating cycles in pose graph

2.3.2. Consistency Validation

Algorithm 2: Cycle based error edge elimination

The cycle struct can be used to validate the edges in pose graph, for its optimal solution should satisfy all constraints when all edges are correct, and conversely, there must be at least one edge formed by spurious registration result inside. We used the residual of corresponding points in ICP registration as the criteria whether the constraint of the edge is satisfied. If it increases sharply after the corresponding points are transformed by the optimal solution of cycle, it can be inferred that this cycle contains an error edge. The calculation process is detailed in Algorithm 2. The residual consistency threshold $\mathsf{\alpha }$ in line 8 determines whether the constraint of edge is satisfied, and this parameter should be set to close to 1 at the beginning and increased gradually if there are edges that remain unvalidated after consistency validation. However, according to our experience, this value should not exceed 1.6, otherwise, some error edges will pass the test.

After the consistency validation, the objective function (9) of the pose graph was minimized using the Levenberg–Marquardt algorithm, and all point segments were transformed and merged into one using the optimized pose.

3. Results

3.1. Experimental Platform and Data Information

To validate the effectiveness of the proposed laser SLAM point cloud precision improvement algorithm, we tested our algorithm with a variety of datasets collected by a customer hand-held laser SLAM device. As shown in Figure 6, this device consists of a laser scanner and an IMU. Both of them are mounted horizontally, and the relative position of the laser scanner and IMU was calibrated manually. We chose two state-of-the-art laser SLAM algorithms, Cartographer and LOAM, to produce the raw SLAM result and used the point cloud and trajectory as the input of our optimization algorithm.

The experimental datasets contain three different environments shown in Figure 7 to test the robustness of the algorithm. Dataset 1 is an indoor office contains a long corridor. Dataset 2 is an underground garage. Dataset 3 is the road around an office building. Figure 8 illustrates the SLAM device trajectory of each dataset, and the basic information of trajectories are listed in

Table 1. Experimental data collection time and trajectory length.

Dataset	Time (s)	Trajectory Length (m)
Dataset 1	184.4	130.9
Dataset 2	278.1	245.5
Dataset 3	283.6	286.8

.

The experiments were performed using a computer with Intel Core i7-3770K CPU @ 3.4 GHz and 8.0 GB of RAM. We implemented the proposed algorithm in C++ language with the use of VLFeat library and PCL (Point Cloud Library).

3.2. Parameter Setting

The default values of parameters were used for Cartographer, LOAM, and fast global registration algorithms.

Table 2. Overview of the parameters applied in optimization.

	Parameters	Value
Local optimization	Time interval $T_s$	10 s
	Maximum distance $D_{max}$	30 m
	Sliding window size $W_{size}$	10
	Number of neighbors	20
	Voxel resolution $V_{size}$	0.1 m
Pose graph construction	Salient radius	0.3 m
	Non maxima radius	0.2 m
	Sphere radius	0.5 m
	Top n similar segment pairs	3
Global optimization	Residual consistency threshold $\mathsf{\alpha }$	1.2
	Maximum iteration	500

lists the remaining main parameters used by the proposed algorithm. The value of time interval for dividing point cloud into segments was 10 s, and the value of sliding window size for local ICP registration was 10. Before registered by the point-to-plane ICP, the point cloud was downsampled using the voxel filter with 0.1 m resolution, and the number of neighbors for normal vector computing was set to 20. After the local optimization phase finished, the parameters for ISS keypoint detecting that salient radius and non maxima radius were set to 0.3 m and 0.2 m. The value of sphere radius for FPFH feature computing was 0.5 m. These parameters were tuned for the best performance. More details about these parameters can be found in [39,40]. We tried to register the top three similar segment pairs to construct the edges in the pose graph. During the global optimization phase, the consistency threshold was set to 1.2. At last, the max iteration times for L-M algorithm was set to 500 to ensure convergence.

3.3. Optimization Results

Figure 9, Figure 10 and Figure 11 illustrate the qualitative comparison results of the point clouds before and after optimization. In these figures, the point clouds are rendered based on time, and the earlier a point is obtained, the closer its color to red, on the contrary, the closer its color to blue. It can be seen that although there remain minor inconsistencies in some datasets as shown in Figure 9e after optimization, our optimization algorithm successfully corrects the point clouds generated by both Cartographer and LOAM in all three datasets. More details of comparing results are shown in Figure 12 and Figure 13. In these figures, the point clouds are rendered based on height, and we highlight where the improvement is obvious after optimization using the ellipses. For example, it is shown that the object contours in point cloud are clearer after optimization in Figure 12b,f, and the internal consistency of point cloud is improved significantly in Figure 13f.

In order to make a quantitative analysis of the precision improvement of point cloud after optimization, we used the TLS point cloud as reference data. These data were captured by a Rigel VZ400 laser scanner and registered manually as shown in Figure 14. The evaluation metrics are as follows:

1. Point-to-point Distance RMSE. For each dataset, we selected 10 pairs of corresponding points and show the distribution of them in Figure 15. The RMSE of their distance are calculated by

$$\sigma =\sqrt{\frac{1}{n}{\displaystyle \sum }_{i=1}^n{\left(\Vert p_i^s-p_i^t\Vert {}_2\right)}^2},$$

(11)

where $p_i^s$ is the point in SLAM point cloud; $p_i^t$ is the point in TLS point cloud; $n$ is the number of corresponding point pairs.

2. Plane-to-plane Distance. We randomly selected 50 1 m × 1 m planes from point clouds in each dataset and calculate the distances of corresponding plane pairs. And we limit the minimum distance between the planes to make them are uniformly distributed and do not overlap with each other.

Table 3. The RMSE estimation result of datasets. For the raw output point cloud of LOAM is distorted severely in dataset 1, the initial RMSE values are empty.

Dataset		Min Distance (m)			Max Distance (m)			RMSE (m)
		Initial	Optimized		Initial	Optimized		Initial	Optimized
Dataset 1	Cartographer	0.034	0.024		0.180	0.100		0.109	0.064
	LOAM	/	0.052		/	0.227		/	0.151
Dataset 2	Cartographer	0.043	0.021		0.348	0.097		0.158	0.059
	LOAM	0.035	0.023		0.134	0.094		0.095	0.062
Dataset 3	Cartographer	0.071	0.057		0.210	0.124		0.154	0.102
	LOAM	0.095	0.068		0.422	0.185		0.277	0.125

and

Table 4. The plane-to-plane distance estimation result of datasets. For the raw output point cloud of LOAM is distorted severely in dataset 1, the initial values are empty.

Dataset		Min Distance (m)			Max Distance (m)			Mean Distance (m)
		Initial	Optimized		Initial	Optimized		Initial	Optimized
Dataset 1	Cartographer	0.037	0.012		0.156	0.138		0.103	0.056
	LOAM	/	0.028		/	0.171		/	0.074
Dataset 2	Cartographer	0.021	0.016		0.508	0.091		0.133	0.054
	LOAM	0.019	0.014		0.170	0.103		0.084	0.050
Dataset 3	Cartographer	0.028	0.022		0.169	0. 119		0.105	0.059
	LOAM	0.034	0.024		0.210	0.102		0.122	0.066

list the precision estimation results. The results show a vast overall improvement in the precision of the point clouds in all datasets. It also can be seen that the higher the precision of the initial result, the more probable a satisfactory optimized point cloud can be obtained. The distorted results of LOAM in dataset 1 and 3 lead to the most residuals remaining off. The most significant improvement happens in Cartographer’s result in dataset 2 where the RMSE and plane-to-plane distance of it are reduced 63% and 59% respectively, since the drift in the initial result is corrected as shown in Figure 10b,c. The average RMSE of Cartographer’s results is 0.144 m, and it reduces to 0.076 m after optimization. The average RMSE of LOAM’s results is 0.206 m, and it reduces to 0.096 m after optimization. They are reduced by 47.3% and 53.4% respectively. The average plane-to-plane distance of Cartographer’s results is 0.114 m, and it reduces to 0.056 m after optimization. The average plane-to-plane distance of LOAM’s results is 0.119 m, and it reduces to 0.057 m after optimization. They are reduced by 50.9% and 52.1% respectively.

4. Discussion

For time performance analysis, we recorded the computing time of each phase and list the results in

Table 5. The time cost of the proposed algorithm.

Dataset		Local Optimization (s)	Pose Graph Construction (s)	Global Optimization (s)	Total (s)
Dataset 1	Cartographer	1033.8	56.3	12.6	1102.7
	LOAM	1167.9	63.9	12.1	1243.9
Dataset 2	Cartographer	1704.3	166.5	36.2	1907.0
	LOAM	1840.2	189.4	40.9	2070.5
Dataset 3	Cartographer	1768.4	225.3	54.5	2048.2
	LOAM	1892.2	231.1	56.7	2180

. It shows that the mean ratio of optimization time and data acquisition time is approximately 8:1. In addition, the local optimization phase occupies more than 85% of the total time cost for the ICP registration is time-consuming. It is noted that the time spent in the proposed cycle based error edge elimination method is almost negligible. Considering it is essential to ensure the consistency of pose graph, the application of it is very cost-effective.

Table 6. Pose graph consistency validation results.

Dataset		Nodes	Constructed Edges	Validated Edges	Error Edges
Dataset 1	Cartographer	16	27	25	2
	LOAM	16	25	24	1
Dataset 2	Cartographer	24	46	40	6
	LOAM	24	49	41	8
Dataset 3	Cartographer	26	49	45	4
	LOAM	26	50	44	6

shows the characterization of the pose graph constructed from all datasets. There are 246 edges constructed and 27 error edges detected in the pose graphs of all datasets, and we inspected all these edges manually. The result is that all of the detected edges are error edges indeed and no false-negative occurs, which indicates our consistency validation method is necessary and effective. For further analysis, we visualized the pose graph and show the results in Figure 16. Since the difference between the pose graphs constructed from two algorithms is slight, we only show the results of LOAM. It is found that there are two types of these error edges:

• Edges are formed by unconverged ICP registration results;
• Edges are formed by wrong feature similarity match results.

The formation of first type error edges illustrated in Figure 17a,b is caused by the fact that ICP algorithm cannot converge to the optimal solution. We inspected the related point cloud and found the reason is that the accuracy of feature registration is not high enough, which leads to the initial value of the ICP algorithm is inaccurate when refining the coarse registration result. The formation of second type error edges illustrated in Figure 17c,d is caused by the unrelated locations having high feature similarities for the repeated similar structures in the environment. In particular, the second type error edges, as long as there is one in the pose graph, will bring about a serious wrong result in the final optimization, so the elimination of these edges is essential. The underground garage is a typical scene with a lot of similar structures, which explains why the proportion of the error edges in dataset 2 is higher than the other two datasets. This also leads to the error in loop closure detection in the Cartographer algorithm, resulting in the inconsistency in Figure 10b.

Except for validated edge and error edge, there are edges that cannot be validated in theory. This will happen when an edge does not belong to any cycles or there is more than one error edge in all cycles this edge belongs to. A pose graph structure that consists of enough small cycles can help avoid these situations. Moreover, it is more robust and beneficial to obtain higher precision point cloud. For instance, comparing the optimized point cloud in three datasets, it is evident that the best precision in dataset 3 is lower than in datasets 1 and 2. This is because the center office build limits the route to form more closed loops and obscures the view in different locations. Additionally, the statistical result shows that the average number of edges in each cycle is 19.6 in dataset 3, while it is 12.2 in dataset 1 and 14.5 in dataset 2. In addition, it can be seen that the constructed pose graph is highly correlated with the data acquisition trajectory, which means designing a suitable acquisition route is important for obtaining a high precision SLAM point cloud.

The residual consistency threshold α is the most important parameter in our algorithm and it determines the result of consistency validation. To analyze its influence on the performance of the algorithm, we chose different α values at interval 0.1. Figure 18 illustrates the edge validation accuracy with different values of parameter α. It can be seen that the edge validation accuracy increases first and then decreases as the α increases, as the correct edges will be detected as error edges with a small α and the error edges cannot be detected with a large α. Therefore, we recommend setting α to a small value at first and then increasing it gradually. The minimum appropriate value of α is 1.2 in dataset 3, while it is 1.1 in datasets 1 and 2, because the average number of edges in each cycle in dataset 3 is larger than datasets 1 and 2 which means more cumulative error in a cycle.

In order to test the versatility of our algorithm, we applied it on the public dataset of Cartographer [45]. This dataset is collected by a SLAM backpack with two Velodyne VLP-16 laser scanner at the Deutsches Museum. Figure 19 illustrates the overview and the backpack trajectory of the selected dataset. This dataset lasts 317.6 s and the trajectory length of it is 419.1 m.

We maintained the values of all parameters the same as in Table 2, except that α became 1.3. The total computing time was 3080.7 s. Figure 20 shows the qualitative comparison result of the point cloud before and after optimization. It is evident that the consistency of the point cloud was improved after optimization. Additionally, Figure 21 presents more details of comparison results.

For the lack of ground truth, we used the mean map entropy (MME) and mean plane variance (MPV) as the metrics of point cloud precision [46] and the evaluation results are shown in

Table 7. The precision evaluation result of the Cartographer dataset.

	Initial	Optimized
Mean map entropy	−2.86	−3.04
Mean plane variance (m)	0.067	0.039

. According to the table, both the MME and MPV are reduced after optimization, which indicates that our algorithm improved the point cloud precision successfully. In addition, comparing to the work of Nüchter et al. in [26], our algorithm achieved the similar performance with higher efficiency, for we used the feature similarity to guide the pose graph construction and did not need to construct edges in the pose graph iteratively.

The result in Table 3 and Table 4 show that the optimization algorithm proposed in this paper achieved good performance on original point clouds generated by both LOAM and Cartographer SLAM algorithms. As mentioned above, the LOAM algorithm uses only laser data as input, while the Cartographer algorithm uses IMU data as an auxiliary input. Additionally, the most important difference between the two algorithms is that the Cartographer algorithm has a loop closure detection module. It is the essential difference between the graph-based SLAM and the filter-based SLAM. Thus, LOAM and Cartographer can be seen as the typical representations of two different SLAM algorithm paradigms, which means that our optimization algorithm has the potential to be applied on results of different laser SLAM algorithms.

Several improvements can be considered for our algorithm. Firstly, ICP registration is the bottleneck of the computational efficiency of this algorithm. A more efficient algorithm, for example, the work in [47], can significantly improve the efficiency of the algorithm. Then, for the similarity matching problem in the repeated structure area, image data can be joined as auxiliary information. At the same time, considering the rapid development in deep learning of point cloud, the deep learning-based point cloud features like [48,49] are also worth experimenting with. Finally, we plan to integrate control points into the optimization algorithm in the next stage.

5. Conclusions

Using 3D laser SLAM technology to acquire the point cloud has the advantages of high efficiency and not relying on GNSS. This makes it one of the best choices for mapping the indoor or underground environment. However, the precision of 3D laser SLAM point cloud is not enough for some fields, which hinders its further application. In this paper, we presented a coarse-to-fine precision optimization algorithm to improve the precision of the 3D laser slam point cloud. The point clouds were first segmented and registered at the local level. Then, a pose graph of point cloud segments was constructed using feature similarity and global registration. At last, all segments were aligned and merged into the final optimization result. Moreover, in order to guarantee the consistency of pose graph, a cycle based error edge elimination method was performed in global optimization. The experimental results demonstrated that the proposed optimization algorithm obtained good performances both in our test datasets and the Cartographer public dataset. In our test datasets, it successfully improved the precision of the point clouds generated by the Cartographer and LOAM laser SLAM algorithm even when there is serious distortion in raw input point clouds. The RMSE of point clouds was reduced by approximately 40%–50% and up to 63% after optimization in different datasets. In the Cartographer public dataset, our algorithm was more efficient compared to a similar algorithm.