Towards a Fully Automated 3D Reconstruction System Based on LiDAR and GNSS in Challenging Scenarios

Abstract

High-precision 3D maps play an important role in autonomous driving. The current mapping system performs well in most circumstances. However, it still encounters difficulties in the case of the Global Navigation Satellite System (GNSS) signal blockage, when surrounded by too many moving objects, or when mapping a featureless environment. In these challenging scenarios, either the global navigation approach or the local navigation approach will degenerate. With the aim of developing a degeneracy-aware robust mapping system, this paper analyzes the possible degeneration states for different navigation sources and proposes a new degeneration indicator for the point cloud registration algorithm. The proposed degeneracy indicator could then be seamlessly integrated into the factor graph-based mapping framework. Extensive experiments on real-world datasets demonstrate that the proposed 3D reconstruction system based on GNSS and Light Detection and Ranging (LiDAR) sensors can map challenging scenarios with high precision.

1. Introduction

Rapid progress has been witnessed in the field of unmanned ground vehicles (UGVs) [1,2]. Autonomous driving is expected to address transportation problems such as road accidents and traffic congestion [3,4]. Among those enabling technologies, the creation and utilization of a high-precision map play an important role [5]. The high-precision map differs from the traditional map not only in its higher accuracy but also in that it contains much more environmental information, which will benefit other modules [6].

The high-precision map should be accurate both globally and locally. At the global scale, the map should be well aligned with the Global Navigation Satellite System (GNSS) trajectory [7]. Locally, the map should fully capture the local details of the surroundings, and the map’s accuracy should be at the centimeter level [8]. To meet these requirements, the mapping system should combine the global navigation approaches with local navigation approaches. Whilst the global navigation information could come from the GNSS system, the local navigation information could be obtained from the Inertial Measurement Unit (IMU), wheel odometer, or Light Detection and Ranging (LiDAR) odometry. However, both types of navigation information may become inaccurate in some circumstances [1,9].

For the global navigation information, the GNSS signal might become inaccurate due to the blockage of tall buildings or the well-known multi-path effect [10]. It may even become unavailable in certain scenarios, such as under bridges, in tunnels, in underground parking lots, etc. For the local navigation approaches, the wheel odometer may become inaccurate in the case of slippage [11]. The LiDAR odometry may also become inaccurate in a featureless environment or when surrounded by too many moving objects [9,12]. Therefore, an ideal mapping system should be capable of detecting the degeneracy state for both navigation approaches, and a robust fusion approach should be designed.

In this paper, we propose to use the factor graph as the fusion framework. The factor graph is a probabilistic framework that could capably handle the uncertainties of different navigation approaches and integrate the different sources of navigation information in a principled way [13].

We thoroughly analyze the different factors contained in the proposed factor graph and design approaches that could estimate the degeneracy state for each factor. The degeneracy state is then transformed to adjust the information matrix for each factor. Therefore, the proposed fusion framework automatically assigns high confidence to those highly confident factors and weakens the influence of those degenerate factors.

We test the proposed mapping system in several challenging scenarios with two types of vehicles equipped with two different kinds of LiDAR, as shown in Figure 1. The testing scenarios include over a high bridge with busy traffic, in an underground parking lot, and a featureless off-road environment. Experimental results show that the proposed mapping framework could overcome the influence of dynamic objects, the GNSS signal outage, and the scan matching degradation. All the testing scenarios are mapped with high precision. The maps built are also shown in Figure 1. To make the mapping system more complete, we also discuss problems regarding loop closure detection and automatic map update and extension.

In summary, we have made the following contributions in this paper:

• A factor graph-based fusion framework is proposed which could suitably integrate the global navigation information with local navigation information in a probabilistic way.
• A comprehensive degeneration analysis is performed for both the global and the local navigation approach. A new robust degeneration indicator is proposed for the local navigation approach which could reliably estimate the degeneration state of the scan matching algorithm. The degeneration state is then incorporated into the factor graph, thus enabling a more robust, degeneration-aware fusion approach.
• An improved submap-to-submap matching method is used to estimate loop closure constraints. The loop closure constraint can be reliably estimated even with a large initial position offset or a limited overlap field of view.
• The proposed mapping system has been extensively tested on real-world datasets in several challenging scenarios, including busy urban scenarios, featureless off-road scenarios, high bridges, highways, and large-scale settings. Experimental results confirmed the effectiveness of the mapping system.

The remainder of this paper is organized as follows: in Section 2, some related works are introduced. The proposed mapping system is presented in Section 3. Section 4 provides the experimental results, and the conclusions are discussed in Section 5.

2. Related Work

LiDAR-based mapping has been extensively studied [1,14]. A complete mapping system includes at least the following modules: the front-end scan matching, the loop closure detection, and the back-end optimization. This paper reviews related works from the following aspects.

2.1. Scan Matching

As the basis of the LiDAR-based mapping system, scan matching methods can be generally divided into two categories: local approaches and global approaches [12]. Iterative Closest Point (ICP) and its variants [15,16] are widely used local scan matching approaches. Point-to-point, point-to-plane, or plane-to-plane distances are usually applied to measure spatial distances of local structures extracted from two scans. The local structure could also be approximated using the Gaussian model [17]. The transformation is then estimated by minimizing the distances of corresponding local structures. As a widely used approach in recent years, Lidar Odometry and Mapping (LOAM) [18] firstly extracts corner points and surface points based on the local geometric structures. The point-to-line distance and the point-to-plane distance are then separately applied to these corner points and surface points. Compared with these local approaches, the Correlative Scan Matching (CSM) [19] is a popular global scan matching approach that treats the scan as a whole. It enumerates all the possible transformations and selects the best one based on the global distance measure. Due to its heavy computational load, CSM is not suitable for 3D matching. Fu et al. [12] proposed to combine the global scan matching approaches with local scan matching approaches and obtained competitive results in off-road environments.

2.2. Loop Closure Detection

Loop closure is helpful for mapping, especially in large-scale environments or scenarios with inaccurate GNSS/Inertial Navigation System (INS) measurements. In Cartographer [20], a branch and bound-based algorithm is proposed to detect the loop closures. Ren et al. [21] improved the boosting-based approach to detect loop closures in a 2D Simultaneous Localization and Mapping (SLAM) system. Dube et al. [22] proposed a SegMatch-based approach where the descriptors of segments are used for place recognition. Based on RangeNet++ [23], Kong et al. [24] proposed a semantic graph-based method to recognize previously visited scenes.

2.3. Robust Mapping

As the back-end of the SLAM system, the constructed pose graph or the factor graph is usually optimized using off-the-shelf toolboxes, such as g2o [25], GTSAM [26], Ceres [27], or iSAM [13]. However, the factors involved in the factor graph might be outlier factors due to noisy observations or erroneous scan registrations. To eliminate the influence of these outliers, robust optimization algorithms have been designed, such as the Switchable Constraints [28], the Max-Mixture Model [29], etc. Although these approaches could downweight the influence of possible outliers, a robust, degeneracy-aware, front-end algorithm is still desired.

To improve the robustness of the front-end, the covariance matrix of the scan matching could be calculated to model the matching precision [19,30]. Zhang and Singh proposed a degeneracy indicator based on the geometric structure of the problem constraints in [31]. Aldera et al. [32] exploited the principal eigenvector of the scan matching to predict failures for introspective analysis. An end-to-end approach was proposed in [33] to learn the error model of the 2D LiDAR scan matching. Contrary to these works, we analyze the convergence of the optimization error of the scan matching algorithm and propose a new degeneration indicator based on it.

3. The Proposed Approach

3.1. System Overview

The framework of the proposed system is illustrated in Figure 2. It mainly consists of the following modules:

(I) Data preprocessing. This module is to preprocess the LiDAR point cloud, as shown in Figure 3. It includes three parts: keyframe selection, intra-frame compensation, and noise removal. Keyframes are selected based on the minimum odometric displacement (we set 1.0 m translation or 10 degree rotation). Distortion caused by LiDAR ego-motion is compensated through IMU/wheel encoder outputs [34,35]. We consider the moving objects as noise to the mapping system. They are filtered out by a multi-object detection and tracking method [36,37,38], as shown in Figure 3c. In the off-road environment, there might be floating dust as the wind blows, as shown in Figure 3d. These floating dust particles are removed by a specifically designed algorithm [39].

(II) Factor graph-based pose optimization. As the core of the mapping system, a factor graph is then constructed to fuse the different kinds of navigation information. The factor graph consists of nodes and edges. Each node represents the pose of the keyframe to be estimated. The edges represent different kinds of constraints posed on the nodes. These include the GNSS factor produced by the GNSS information; the local odometry factor produced by the wheel odometer and IMU, and the scan matching factor generated by the preprocessed LiDAR scans. For each kind of factor, we need to set an appropriate information matrix for it, which represents its uncertainty. Estimating the uncertainty of each navigation source is one of the main contributions of this paper and will be described in the following subsections.

(III) Map construction and extension. The factor graph is then optimized using any off-the-shelf optimizers, including g2o, GTSAM, Ceres, etc. In this paper, we choose to use iSAM [13] as the optimizer. The optimized poses are then utilized to merge the keyframe point clouds into a consistent map. To ensure the scalability and the possible extension of the constructed map, we also design specific algorithms in Section 3.5.

3.2. Pose Graph Optimization

The pose graph that we constructed is shown in Figure 4. It integrates multiple sensor input data and outputs the optimized poses $\left\{X^{*}\right\}$. Specifically, the following types of factors are involved: the motion prior factor $U_{i,j}^0$ from the IMU/wheel encoder [34,35], the scan-to-scan matching factor $U_{i,j}^1$, the scan-to-submap matching factor $U_{i,j}^2$, the loop closure factor $U_{i,l}^3$, and the integrated GNSS/INS factor $Z_i$. For brevity, $U_{i,j}^0$, $U_{i,j}^1$, and $U_{i,j}^2$ are all named as the local constraints (as shown with orange squares in Figure 4).

Based on Bayesian inference, the factor graph optimization is constructed as:

$$p(X,Z,U)=p\left(X_0\right)·\prod _{i,j}p\left(X_j\right|U_{i,j}^c,X_i)·\prod _{k}p\left(Z_k\right|X_k),$$

(1)

where X denotes the variable nodes, U represents the local constraints and loop closure constraints. Z denotes the GNSS/INS observations. As illustrated in [13], the motion model of $p\left(X_j\right|U_{i,j}^c,X_i)$ can be described as:

$$X_j=f_{i,j}^c(X_i,U_{i,j}^c)+w_{i,j}^c,$$

(2)

where $X_i$ and $X_j$ denote the corresponding nodes connected by constraint $U_{i,j}^c$, $f_{i,j}^c$ is the state transition process, $w_{i,j}^c$ is the process noise with covariance matrix ${\mathsf{\Lambda }}_{i,j}^c$, and $c\in \{0,1,2,3\}$ is the constraint type. The observation model of $p\left(Z_k\right|X_k)$ can be expressed as:

$$Z_k=h_k\left(X_k\right)+v_k,$$

(3)

where $Z_k$ denotes the GNSS/INS observation over the node $X_k$, $h_k$ is the observation function, and $v_k$ indicates the noise of the observation with covariance matrix ${\mathsf{\Gamma }}_k$.

Combining (1), (2), and (3), we obtain the following least squares formulation:

$$X^{*}=\arg \underset{X}{\min }\left(\sum _{i,j}\Vert f_{i,j}^c(X_i,X_j)-U_{i,j}^c{\Vert }_{{\mathsf{\Lambda }}_{i,j}^c}^2+\sum _k{\Vert h_k\left(X_k\right)-Z_k\Vert }_{{\mathsf{\Gamma }}_k}^2\right),$$

(4)

where ${\Vert e\Vert }_{\mathsf{\Lambda }}^2=e^T{\mathsf{\Lambda }}^{-1}e={\Vert {\mathsf{\Lambda }}^{-1/2}e\Vert }^2$.

We denote ${\mathsf{\Lambda }}^{-1/2}$ as $\mathsf{\Omega }$. Then, ${\mathsf{\Omega }}_{\mathsf{\Lambda }}$ and ${\mathsf{\Omega }}_{\mathsf{\Gamma }}$ are the confidence matrices (also named information matrices), which reflect the reliability of the factors and can be calculated by the inverse of the standard deviation. To facilitate calculation, $\mathsf{\Omega }$ is usually represented as a diagonal matrix, which suggests that the six degrees of freedom of X (x, y, z, $roll$, $pitch$, $azimuth$) are independent of each other.

Quantitatively estimating the noise level of each factor is the key to improving the pose estimation accuracy. For the noise model of the IMU/wheel encoder factor, we use the approach proposed in [34]. The scan matching factor and the GNSS/INS factor are then analyzed as follows.

3.3. Scan Matching Factor

Mathematically, scan matching aims to estimate the relative transformation $\mathcal{T}$ between the target point cloud $\mathcal{P}$ and the reference point cloud $\mathcal{Q}$. Based on the different types of $\mathcal{Q}$, the scan matching factor involves three different forms: when $\mathcal{Q}$ is a neighboring keyframe, it forms a scan-to-scan matching factor; when $\mathcal{Q}$ represents a local sub-map, it generates a scan-to-submap matching factor. When $\mathcal{Q}$ is a scan of a revisited place, it generates a loop closure factor.

All three types of scan matching factors aim to minimize the following energy function:

$${\mathcal{T}}^{*}=\arg \underset{\mathcal{T}}{\min }\mathcal{E}\left(\mathcal{T}\right),$$

(5)

where $\mathcal{E}\left(\mathcal{T}\right)$ is a specifically designed energy function. This minimization function could be solved with the Levenberg–Marquardt algorithm through nonlinear iterations:

$$\begin{array}{cc}\hfill & \delta =-{\left(J^{T}J+\lambda \mathrm{diag}\left(J^{T}J\right)\right)}^{-1}{J}^{T}d\hfill \\ \hfill & \mathcal{T}\leftarrow \mathcal{T}+\delta \hfill \end{array},$$

(6)

where $J=\partial \mathcal{E}/\partial \mathcal{T}$ and d is the residual error, which is usually represented as the geometric distance between two matching primitives.

It is noted in [31] that the eigenvalues of $J^{T}J$ reflect the degeneracy state of the scan matching algorithm. Therefore, the authors of [31] defined the degeneration indicator as:

$$D={\lambda }_{min}+1,$$

(7)

where ${\lambda }_{min}$ is the smallest eigenvalue of $J^{T}J$. This method can detect degradation in some cases, but it might become unreliable, especially in the featureless off-road environment. Moreover, D has no probabilistic meanings, making it difficult to be integrated into the factor graph. To remedy this, we propose another degeneracy indicator in this paper. We firstly define ${\delta }_i$ as the update vector at each optimization iteration:

$${\delta }_i={[\Delta t_{x_i},\Delta t_{y_i},\Delta t_{z_i},\Delta r_{x_i},\Delta r_{y_i},\Delta r_{z_i}]}^T.$$

(8)

Then, by aggregating the ${\delta }_i$ from the last n iterations, we can calculate its covariance matrix:

$$\begin{array}{cc}\hfill & \mathsf{\Phi }=[{\delta }_1,{\delta }_2,\dots ,{\delta }_n]\hfill \\ \hfill & {\mathsf{\Sigma }}_{\delta }=\mathsf{\Phi }·{\mathsf{\Phi }}^T\hfill \end{array}.$$

(9)

Assuming that the six degrees of freedom are independent of each other, the diagonal matrix of ${\mathsf{\Sigma }}_{\delta }$ is denoted as:

$${\mathsf{\Sigma }}_{\delta }^{{}^{\prime }}=\mathrm{diag}({\mathsf{\Sigma }}_{\delta }^{t_x},{\mathsf{\Sigma }}_{\delta }^{t_y},{\mathsf{\Sigma }}_{\delta }^{t_z},{\mathsf{\Sigma }}_{\delta }^{r_x},{\mathsf{\Sigma }}_{\delta }^{r_y},{\mathsf{\Sigma }}_{\delta }^{r_z}).$$

(10)

Intuitively, when the degeneration occurs, the corresponding elements of ${\mathsf{\Sigma }}_{\delta }^{{}^{\prime }}$ will increase. Therefore, we combine D with ${\mathsf{\Sigma }}_{\delta }^{{}^{\prime }}$, and define its information matrix ${\mathsf{\Omega }}_{\mathsf{\Lambda }}^c$ as:

$${\mathsf{\Omega }}_{\mathsf{\Lambda }}^c=(k_1\mathrm{sign}(D-D_{th})+k_2)·{\xi }_{\mathsf{\Lambda }}\left({\left({\mathsf{\Sigma }}_{\delta }^{{}^{\prime }}\right)}^{-1/2}\right),$$

(11)

where $\mathrm{sign}\left(x\right)=1$ when $x\ge 0$; otherwise, $\mathrm{sign}\left(x\right)=-1$. $D_{th}$ is set to 300, and $k_1=0.25,k_2=0.75$. ${\xi }_{\mathsf{\Lambda }}(·)$ is a normalization function. $c\in \{1,2,3\}$ corresponds to the three types of scan matching constraints.

It is observed from Figure 4 that $U^0$, $U^1$, and $U^2$ are all used to calculate the local constraints between two neighboring keyframes. Among them, $U^0$ is directly obtainable from the hardware. $U^2$ is the most computationally expensive approach but provides the most accurate results in most circumstances. Generally, we have ${\mathsf{\Omega }}_{\mathsf{\Lambda }}^0<{\mathsf{\Omega }}_{\mathsf{\Lambda }}^1<{\mathsf{\Omega }}_{\mathsf{\Lambda }}^2$. Moreover, the proposed degeneration indicator could be applied to both ${\mathsf{\Omega }}_{\mathsf{\Lambda }}^1$ and ${\mathsf{\Omega }}_{\mathsf{\Lambda }}^2$ based on (11). When $U^1$ or $U^2$ degenerate, ${\mathsf{\Omega }}_{\mathsf{\Lambda }}^1$ or ${\mathsf{\Omega }}_{\mathsf{\Lambda }}^2$ will be automatically set to a lower value. $U^0$ has the lowest accuracy in most circumstances. When both $U^1$ and $U^2$ degenerate, $U^0$ will play a more important role.

For the loop closure constraint $U^3$, it is usually more challenging than $U^1$ and $U^2$ due to the relatively large initial offset. Inspired by the work in [12], we adopt a combination of the global registration approach and the local registration approach, namely the CSM and LOAM. For LOAM, instead of performing the scan-to-scan registration or the scan-to-submap registration, we perform a submap-to-submap registration. Moreover, the degeneration analysis could also be applied to the local registration approach. Only those loop closure constraints with a low degeneration score are automatically added to the factor graph. For those possible degenerate loop closure candidates, either they are removed or a manual confirmation is performed.

3.4. GNSS/INS Factor

The integrated GNSS/INS device provides six-degrees-of-freedom global pose estimation over each node, which is expressed in (3). However, the GNSS signal might be blocked by tall buildings or trees, resulting in lower position accuracy. Besides the position output, the modern GNSS/INS devices also output affiliated information including the number of observed satellites $N_{vs}$, the position deviation $DE{V}_{pos}$, etc. Experimental results show that these values are strongly correlated with the GNSS/INS accuracy. Therefore, we use these values to adjust the information matrix in the factor graph. Let ${\mathsf{\Omega }}_{\mathsf{\Gamma }}$ represent the information matrix of the GNSS/INS factor. It is a $6\times 6$ diagonal matrix and defined as:

$${\mathsf{\Omega }}_{\mathsf{\Gamma }}=I·(k_1\mathrm{sign}(N_{vs}-N_{vs}^{th})+k_2)·{\xi }_{\mathsf{\Gamma }}\left({\left(DE{V}_{pos}\right)}^{-1/2}\right),$$

(12)

where $N_{vs}^{th}$ is set 10, and $k_1=0.25,k_2=0.75$. ${\xi }_{\mathsf{\Gamma }}(·)$ is a normalization function. I is a $6\times 6$ diagonal matrix denoted as:

$$I=\mathrm{diag}({\mu }_t,{\mu }_t,{\mu }_t,{\mu }_r,{\mu }_r,{\mu }_r),$$

(13)

where ${\mu }_t$ is the translational weight factor and ${\mu }_r$ is the rotational weight factor. According to the units of the translation ($meter$) and rotation ($rad$) variables, we set ${\mu }_r/{\mu }_t=5$.

3.5. Map Extension

The optimized poses $X^{*}$ are then used to stitch keyframes together to generate a global map. As shown in Figure 2, as the optimized poses are in 3D, we can generate both the 3D map and the 2D grayscale map. To facilitate the storage and the online usage of the map, the whole map is partitioned into small blocks by the global coordinate, and each small block is stored as a separate file. For the 3D map, each small block is stored using the Octree structure [40], whilst, for the 2D map, both the intensity and the height information are stored for each grid cell [6].

In general, maps cannot be built all at once and the map should be extendable to cover previously unseen areas. As an illustrative example shown in Figure 5, $M_1$ is a pre-built map, and $M_2$ is a new area that will be added to $M_1$. To ensure the global consistency of the map, the following procedures are performed: (I) Generate the pose graph of $M_2$ and calculate the optimized poses of $M_2$. (II) Find the keyframes in $M_2$ that are closest to $M_1$, perform the submap-to-submap matching of these keyframes to $M_1$, and obtain the relative poses of these keyframes against $M_1$. (III) Assign high confidence to these optimized poses, and add them as extra constraints to the pose graph of $M_2$. $M_2$ is then re-optimized and added to $M_1$.

4. Experimental Results

The experimental platforms are two modified self-driving vehicles shown in Figure 1. Each platform is equipped with a multi-channel LiDAR, an IMU, a wheel encoder, and a Novatel Propack 7 GNSS/INS system. Platform 2 is an industrial robot and the task loads have to be mounted on top of it. Therefore, the LiDAR has to be mounted in front of the vehicle and only has a limited FoV of around 200 degrees. The offline mapping system is run on a laptop with an Intel Core i7 CPU at 2.20 GHz.

4.1. Map Quality Assessment

To measure map quality and to facilitate quantitative comparisons of the results, the mean map entropy (MME) [41] is used. Given a map $\mathcal{M}=\{P_1,P_2,\dots ,P_N\}$, the entropy of a point $P_k$ is calculated by:

$$mme\left(P_k\right)=\frac{1}{2}\ln \left(2\pi e\right|\mathsf{\Sigma }\left(P_k\right)\left|\right),$$

(14)

where $\mathsf{\Sigma }\left(P_k\right)$ is the covariance matrix of the point set ${\mathsf{\Omega }}_{P_k}$ in a certain neighborhood of $P_k$. The local radius is set to 0.5 m and $mme\left(P_k\right)$ is valid when the size of ${\mathsf{\Omega }}_{P_k}$ is larger than 5. The entropy is calculated for each point and the average is calculated as:

$$H\left(\mathcal{M}\right)=\frac{1}{N}\sum _{k=1}^{N}mme\left(P_k\right).$$

(15)

Generally, $H\left(\mathcal{M}\right)$ represents the sharpness of the map. Lower $H\left(\mathcal{M}\right)$ means higher map quality [41]. To verify the relationship between $H\left(\mathcal{M}\right)$ and the map quality, we add different levels of Gaussian noise (characterized by a standard deviation of $\sigma$) to maps sampled from two scenes. The original maps with $\sigma =0$ are shown in Figure 6. The resulting map entropy plotted against $\sigma$ is shown in Figure 7. For both samples, $H\left(\mathcal{M}\right)$ is positively correlated with $\sigma$, indicating that higher entropy characterizes poorer map quality.

4.2. Results of Noise Removal

Removal of dynamic vehicles: The comparison results with and without the removal of dynamic vehicles (RDV) are shown in Figure 8. As shown in Figure 8, the ghost shadows caused by moving vehicles (highlighted with blue dotted boxes in Figure 8a) are mostly filtered out by the proposed mapping system. Furthermore, the decrease in $H\left(\mathcal{M}\right)$ after the removal of dynamic vehicles suggests that the map quality has been improved.

Removal of floating dust: Two comparative mapping results with and without the removal of floating dust (RFD) in off-road scenarios are shown in Figure 9. It is obvious that the map quality has been improved after the removal of floating dust.

4.3. Analysis of Degeneracy-Aware Factors

The degeneration analysis of the GNSS/INS factor and the scan matching factor is performed in this section. The first dataset was collected by Platform 1 in city environments with a total driving length of 19.3 km, as shown in Figure 10. The evaluation of the GNSS/INS status is shown in Figure 11. Intuitively, frames with lower $N_{vs}$ or higher ${DEV}_{pos}$ have worse GNSS/INS status. A manual confirmation indicates that these frames are mainly captured under high bridges.

For the scan matching degeneration analysis, we evaluate the degeneracy indicators D and ${\mathsf{\Sigma }}_{\delta }^{{}^{\prime }}$. For easy comparison with D, we define E based on ${\mathsf{\Sigma }}_{\delta }^{{}^{\prime }}$:

$$E={\eta }_1({\mathsf{\Sigma }}_{\delta }^{t_x}+{\mathsf{\Sigma }}_{\delta }^{t_y}+{\mathsf{\Sigma }}_{\delta }^{t_z})+{\eta }_2({\mathsf{\Sigma }}_{\delta }^{r_x}+{\mathsf{\Sigma }}_{\delta }^{r_y}+{\mathsf{\Sigma }}_{\delta }^{r_z}),$$

(16)

where ${\eta }_1$ and ${\eta }_2$ are scale factors. The values of D and E are calculated for each scan-to-scan and scan-to-submap registration, and the results are shown in Figure 12a–d. Frames with lower D or higher E are more prone to degeneration and they are highlighted in pink. These frames are mostly captured on the highway, with less geometrical structures, or surrounded by many vehicles, as shown in sub-figures 2 and 3 in Figure 10. Furthermore, the scan-to-submap matching constraints have larger D and smaller E than the scan-to-scan matching constraints. Therefore, the scan-to-scan matching constraints are generally more prone to degeneration than the scan-to-submap constraints.

Moreover, the degeneration direction can also be inferred from the six components of ${\mathsf{\Sigma }}_{\delta }^{{}^{\prime }}$. Figure 12e shows the translational components of ${\mathsf{\Sigma }}_{\delta }^{{}^{\prime }}$ for the scan-to-submap matching constraints. It is observed that the longitudinal direction is more prone to degeneration, as shown by the blue dots. This is consistent with the feature distribution shown in close views 2 and 3 of Figure 10, where repeated features mostly appear along the longitudinal direction.

To further compare the performance of E and D, we conducted a test on the dataset collected by Platform 2 in the off-road scenario. We randomly selected five sets of data, each of which contained 200 keyframes. Zero-mean Gaussian noise was added to the last 150 scans for each set of data. Then, the scan matching algorithm was performed to compute the relative transformation between every two keyframes. We evaluated the translation errors and the degeneration indicators of the scan-to-submap matching factors. In Figure 13, the non-degenerate cases (without Gaussian noise) are shown in yellow and the degenerate cases (with Gaussian noise) are shown in pink. The translation error (shown in red) increases significantly for the last 150 keyframes of each group. Furthermore, the proposed degeneration indicator E (shown in green) shows a similar trend to the translation error, exhibiting better performance than the degeneration indicator D (shown in blue). Further comparison results between D and E will be described in Section 4.4.

4.4. Analysis of Loop Closures

In most scenarios, the GNSS/INS can provide a good initial value for loop closure detection. In the case of GNSS signal blockage, the loop closure has to be detected by the scan matching algorithm. In the featureless off-road environment, loop closure detection becomes more challenging.

We collected three sets of data by Platform 2 in the off-road scenario. A total of 846 pairs of loop closure constraints were generated, including 423 pairs with small translational offsets (around 2 m) and 423 pairs with large offsets (around 15 m). Similar to [12], the ground-truth position of each keyframe was obtained using the graph SLAM algorithm. For each group, we added five levels of translational and rotational noise, obtaining a total number of 4230 loop closure pairs. The noise parameters are shown in

Table 1. Five levels of noise for loop closure registration.

Setting	Translation Noise (m)			Rotation Noise (deg)
	$\mathit{Forward}$	$\mathit{Left}$	$\mathit{Up}$	$\mathit{Azimuth}$	$\mathit{Pitch}$	$\mathit{Roll}$
Level1	0.5	0.5	0.5	3.0	1.5	1.5
Level2	1.0	1.0	0.5	5.0	2.5	2.5
Level3	2.0	2.0	1.0	5.0	2.5	2.5
Level4	3.0	3.0	1.5	5.0	2.5	2.5
Level5	3.0	3.0	1.5	10.0	5.0	5.0

. The number shown in this table represents the mean ($e_m$) of the added Gaussian noise and the standard deviation ($e_{sd}$) is set to $25\%$ of $e_m$.

We choose NDT and LOAM as the baseline approaches. Figure 14 shows the error statistics of loop closure registration. Overall, the proposed method obtains the best performance at all levels of difficulty for both two groups. Furthermore, Figure 15 shows the proportion of correct loop closure registrations. We compare the registration result with the ground truth. It is considered as a correct registration if the translation error is less than 0.2 m and the rotation error is less than 0.5 degrees. Based on Figure 14 and Figure 15, the proposed approach performs best in both the overall accuracy and the success rate.

Two typical loop closure matching results are shown in Figure 16. The large position offset and the limited FoV of each LiDAR frame pose significant challenges to the loop closure registration algorithm. It is observed in Figure 16 that both NDT and LOAM failed to obtain the correct registration result. In contrast, the proposed approach uses a combination of the global registration approach (CSM) and local registration approach (LOAM-based submap-to-submap matching) and correctly computed the registration results.

Moreover, the proposed degeneracy indicator E (simplified from ${\mathsf{\Sigma }}_{\delta }^{{}^{\prime }}$ (16)) is also evaluated for the loop closure registration algorithm. The performance of E and D is shown in Figure 17. The ROC curves are obtained by evaluating 4230 pairs of loop closures. The proposed indicator E obtains much better performance than the indicator D.

4.5. Mapping Results

In addition to the mapping results described in Section 4.3, the proposed mapping system is also tested under several other challenging scenarios.

Mapping results in challenging city scenarios: The mapping system is tested in a complex city scenario, including the city road under a bridge, highways, and an underground parking garage, as shown in Figure 18. Moreover, the data were collected during a high-traffic period, when the vehicle was always surrounded by many other moving objects. It is observed that the lane markings on the ground are clearly visible in the enlarged 2D grayscale maps (as shown in close views 1–3 of Figure 18). The high-precision 3D point cloud map of the underground parking garage suggests that the mapping system could accurately map the area without the GNSS signal, and the built map could be seamlessly aligned to the area with the GNSS signal.

The computational efficiency of the mapping system is also evaluated. In this test, the scan matching and the removal of dynamic objects can be processed in parallel at a frequency of 10 Hz. The route is around 34.5 km and consists of 31,189 keyframes. The detection and verification of loop closures take less than 15 min and the optimization can be performed in a few seconds. Finally, the rendering of maps takes around 20 min at a frequency of around 30 Hz. Overall, the construction of the map takes less than 90 min on a single CPU.

Mapping results in off-road scenarios:Figure 19 shows the mapping results tested on Platform 2 in the featureless off-road scenarios. Two sets of data with a total driving length of 10.859 km were collected. The driving route and the generated 3D map are overlaid on the satellite image. Three enlarged grayscale maps shown at the bottom of Figure 19 prove the effectiveness of the mapping system in the featureless off-road environment even with a LiDAR with limited FoV.

Mapping results at a high driving speed: In all the above experiments, the vehicle drives at a speed of around 30 km/h. In this test, we evaluate the mapping system at a higher driving speed. The mapping results are shown in Figure 20a. Some of the scenarios are relatively open with fewer feature points. The vehicle drives at an average speed of 38.2 km/h, and the maximum speed reaches 76.2 km/h (as shown in Figure 20b). Some of the mapping results are shown at the bottom of Figure 20a, where the lane markings on the ground are visible, demonstrating the effectiveness of the mapping system.

Large-scale mapping: Finally, the mapping system was also tested in a large-scale highly dynamic urban scenario. As shown in Figure 21, two sets of data were collected. The first dataset was collected from A to B on 3 September 2019 and the second dataset was collected from B to A on 28 July 2020. The total driving length is 56.3 km and the driving speed is shown in Figure 21b. A large number of dynamic vehicles make the scan matching tend to degenerate, especially over the high bridges. Moreover, GNSS signal blockage frequently occurs due to tall buildings. These problems are handled well by the degeneration analysis proposed in this paper.

5. Conclusions

In this paper, we proposed a framework for LiDAR-based 3D mapping in challenging scenarios. A degeneracy-aware factor graph is constructed by fusing different kinds of navigation information to improve the mapping accuracy and robustness. The reliability of each factor is analyzed, and the degeneration state can be effectively detected. A new scan matching degeneracy indicator ${\mathsf{\Sigma }}_{\delta }^{{}^{\prime }}$ is proposed. This new indicator can be seamlessly integrated into the pose graph optimization framework. The loop closure constraint can be reliably estimated by an improved submap-to-submap matching method even with a large initial position offset or a limited overlap field of view. Extensive experiments in several challenging scenarios are performed. Results indicate that the proposed mapping system works well both in the highly dynamic urban scenarios and the featureless off-road scenarios, even with GNSS signal blockage or at a higher driving speed. In the future, we plan to augment the mapping system with more informative visual features captured from cameras.