Songming Chen
email: songming.chen@ec-nantes.fr

Vincent Fr
email: vincent.fremont@ec-nantes.fr

Uncertainty-Aware Adaptive Semi-Direct LiDAR Scan Matching for Ground Vehicle Positioning

Keywords: Signal Processing Feature Extraction Constant Velocity Motion Model Adaptive Initialization Chamfer Distance LiDAR-based sensing, Scan matching, Ego-motion, State estimation, Uncertainty quantification

Iterative Closest Point (ICP) algorithms are widely used in the literature for the estimation of relative transformations using 3D LiDAR point clouds. This class of algorithms proves to be efficient when the 3D data share sufficient overlapping parts and a good initial guess is provided. However, large relative motions and mutual occlusion of objects in real-road scenarios hinder traditional optimization-based ICP from achieving optimal estimation. This paper explores both direct and feature-based 3D LiDAR scan matching using the ICP framework in different contexts, such as parking, residential, urban, and highway scenarios. In order to guarantee the scan matching performances in scenarios with scarce geometric information and fast ego-vehicle motion, we propose an adaptive semi-direct scan matching method together with an alignment uncertainty quantification. The proposed semi-direct scan matching is tested on both the public KITTI and self-recorded LS2N datasets, which accomplishes the robust 6 Degrees of Freedom (DoF) pose estimation and consistent scene reconstruction. We demonstrate that the proposed approach outperforms the state-of-the-art and achieves the leading results with 68.3 % average relative fitness and 5.71 cm average RMSE, respectively.

I. INTRODUCTION

P ERCEPTION of the surrounding environment and self- positioning are important functions for intelligent vehicles. Global Navigation Satellite Systems (GNSS) allow for the estimation of the vehicle global position in outdoor environments. In GNSS-denied areas, the relative position is estimated using sensors such as the Light Detection and Ranging (LiDAR) that enable the construction of maps of the surrounding environment [START_REF] Li | P3-LOAM: PPP/LiDAR loosely coupled SLAM with accurate covariance estimation and robust RAIM in urban canyon environment[END_REF]. For safety-critical functions, LIDARs are used with video cameras to have better depth information and add redundancy [START_REF] Li | LiDAR for autonomous driving: The principles, challenges, and trends for automotive lidar and perception systems[END_REF]. This is mainly attributed to LiDAR's insensitivity to illumination conditions and omnidirectional field of view, which enables scale-aware 6 DoF pose estimation and precise 3D scene mapping.

LiDAR scan matching algorithms are categorized into direct and feature-based approaches based on matching raw data or local features in the LiDAR scans. The direct LiDAR scan matching method aligns the raw point clouds without distinguishing the feature points. The correspondences are built iteratively according to the closest neighbor criteria. And the optimal transformation is estimated that aligns the source and target point clouds. However, it needs to be noticed that the direct method always requires a good initial guess to start, and it does not work efficiently for partial overlapping and noisy scans. The feature-based LiDAR scan matching methods extract local features from raw point clouds and use high-dimension descriptors for matching these features. The established corresponding feature points will then be used to estimate the relative transformation, which increases the robustness against noise and erroneous matches. Nonetheless, in repetitive and feature-less environments, the feature-based method may perform poorly due to the absence of anchor features for scan alignment.

Semi-direct approaches were proposed in [3] [4] for visionbased state estimation to overcome the problem of visual illumination changes and loss of visual feature tracking. Basically, the feature-based method provides an initial estimate for the direct method, which then improves the accuracy and reliability of the estimation. Inspired by those performances in vision-based state estimation, we come up with a so-called semi-direct LiDAR scan matching approach that combines the conceptually complementary direct and feature-based scan matching. The proposed semi-direct LiDAR scan matching algorithm ensures that the registration is robust against the high-speed or large-rotation motion, or traveling in repetitive and feature-less environments. For vehicles driving on roads, the highly isotropic ground points are inevitably scanned and matched, which may bias the state estimation. In order to reduce the registration lag effects, ground points need to be removed beforehand. Thus, we first apply a simple but efficient method to identify and clear the ground points with the prior vehicle height and planar motion assumptions. Moreover, an uncertainty model of the relative transformation is also performed to evaluate the scan matching quality, which also lays the basis for pose-graph optimization [START_REF] Kümmerle | G2O: A general framework for graph optimization[END_REF]. The contributions of this paper are the following:

1) A robust semi-direct 3D LiDAR scan matching approach is proposed, whose adaptive initialization procedure facilitates accurate 3D point cloud registration.

2) The registration lag effect of 3D LiDAR scan matching is solved by implementing a consensus-based ground plane fitting and ground points removal. The consensus is based on the least summed distance of the potential ground points to the estimated plane. 3) An uncertainty model for the 3D LiDAR scan matching is proposed and evaluated. The main factors which degrade LiDAR scan alignment performances are also analyzed. 4) The uncertainty model is integrated into the global posegraph optimization to reduce the pairwise registration drift and to obtain consistent scene mapping. The rest of this paper is organized as follows: In Section II, the related work and recent advances in LiDAR-based scan matching problems is covered. Then, the proposed methodology is detailed in Section III. After that, the experimental results and corresponding analysis are shown in Section IV. Finally, the conclusion and future perspectives are given in Section V.

II. RELATED WORK

In the past few decades, several achievements in 6 DoF pose estimation using 3D LiDAR scan matching have been obtained, which reach centimeter-level precision. In order to tightly align two time-consecutive point clouds, ICP-based algorithms are well-recognized methods to estimate the relative transformation of the LiDAR scans.

A. Direct scan matching

The direct ICP scan matching algorithm is firstly introduced by [START_REF] Besl | A method for registration of 3-D shapes[END_REF]. The ICP algorithm iteratively searches for the nearest neighbor in the target point cloud and builds the correspondences, which refines the optimal relative transformation through iterations. In [START_REF] Besl | A method for registration of 3-D shapes[END_REF], the point-to-point distance is used for the closest neighbor association, which might be too greedy when the two scans are captured with strong viewpoint changes. In order to mitigate this problem, the point-to-plane distance is proposed in [START_REF] Chen | Object modelling by registration of multiple range images[END_REF] for robust data association. Compared with the point-to-point distance metric, the point-to-plane distance metric relaxes the strict point-topoint correspondence restrictions, which is more adaptable for partially overlapping scans in practice. With the purpose of unifying the point-to-point and point-to-plane distance metrics into a probabilistic framework, Generalized-ICP (G-ICP) has been proposed in [START_REF] Segal | Generalized-ICP[END_REF]. The G-ICP framework aims to align the LiDAR scan surfaces instead of distinctive individual points, which is more tolerant to incorrect correspondences and also reduces the risks of being stuck in local minima for the partially overlapped LiDAR points clouds. Moreover, since the G-ICP framework leverages the probabilistic model for LiDAR scan alignment, it maintains better robustness against the measurement noise due to manufacturing limitations or extreme working conditions.

B. Feature-based scan matching

Instead of aligning the whole raw 3D point cloud, the feature-based methods extract distinctive feature points and build the correspondences based on pre-computed descriptors. For example, a double-layer feature-based method is proposed in [START_REF] Wang | Intelligent vehicle self-localization based on double-layer features and multi-layer LIDAR[END_REF] to extract ground and vertical features that match the corresponding scan observations. Also in [START_REF] Oh | Accurate alignment inspection system for low-resolution automotive LiDAR[END_REF], four planar feature points are extracted in the region of interest for accurate automotive LiDAR alignment inspection. In order to efficiently compute the features, the 3D point clouds are projected into rasterized range images in [START_REF] Ali | A feature based laser SLAM using rasterized images of 3D point cloud[END_REF], where the visual ORB features can be trivially extracted for state estimation and loop closure. Recently, inspired by images-based approaches, viewpoint invariant 3D descriptors such as FPFH [START_REF] Rusu | Fast point feature histograms (FPFH) for 3D registration[END_REF] and SHOT [START_REF] Tombari | Unique signatures of histograms for local surface description[END_REF] have emerged to characterize the local patches in the LiDAR scans. Sample Consensus Initial Alignment (SAC-IA) has been proposed in [START_REF] Rusu | Fast point feature histograms (FPFH) for 3D registration[END_REF] for the rough scan alignment. This approach is robust to sensor noise but might cause overfitting in information-deprived environments such as indoor corridors, outdoor tunnels, or highways. Furthermore, the emergence of data-driven methods is reflected in the estimation of the 6 DoF pose using an end-to-end approach as in PointLoc [START_REF] Wang | PointLoc: Deep pose regressor for LiDAR point cloud localization[END_REF].

C. Hybrid scan matching

The accuracy of the standard ICP scan matching is highly dependent on the initialization process as reported in [START_REF] Pomerleau | Comparing ICP variants on real-world data sets[END_REF]. It means that a large deviation of the initial alignment may cause the ICP optimization to diverge or get stuck in local minima. Thus, hybrid methods that incorporate visual, IMU, GNSS, and range sensor information have been proposed for reliable state estimation and scene reconstruction. In [START_REF] Wang | Simultaneous localization of rail vehicles and mapping of surroundings with LiDARinertial-GNSS integration[END_REF], the multi-sensor data are tightly coupled for accurate rail vehicle localization and mapping even in degenerated environments. The visual and LiDAR sensor information are loosely coupled by covariance intersection in [START_REF] Chen | A loosely coupled vision-LiDAR odometry using covariance intersection filtering[END_REF] for the robust inter-frame transformation estimation. Besides, a multisensor joint optimization approach has been proposed in [START_REF] Wang | An optimization-based multisensor fusion approach towards global drift-free motion estimation[END_REF] and the dual-layer optimization design ensures both local and global estimation consistency. At the same time, the RTK/IMU measurements are employed to verify the LiDAR scan matching accuracy to prevent degenerated estimation in [START_REF] Wang | A tightly-coupled framework for large-scale map construction with multiple non-repetitive scanning LiDARs[END_REF]. Furthermore, the drift of the incremental LiDAR scan alignment can be eliminated by the vision-based bag-of-words place recognition technique, as demonstrated in [START_REF] Ali | A life-long SLAM approach using adaptable local maps based on rasterized LIDAR images[END_REF].

D. Scan matching uncertainty

For the LiDAR scan matching, it is crucial to evaluate the corresponding uncertainty for the estimated relative transformation. This is beneficial for sensor fusion or weight assignment in pose graphs to mitigate local errors. Another advantage of uncertainty modeling is to bound the estimation error within a known confidence interval, which is the prerequisite for road obstacle avoidance and interactive navigation tasks. There exist several methods for the LiDAR scan matching covariance estimation. A singular value decompositionbased method is proposed for the LiDAR-based 6 DoF pose error propagation and covariance estimation [START_REF] Li | P3-LOAM: PPP/LiDAR loosely coupled SLAM with accurate covariance estimation and robust RAIM in urban canyon environment[END_REF]. Based on the estimated covariance, the accumulated errors can then be corrected with the GNSS data. A closed-form covariance estimation method is presented in [START_REF] Censi | An accurate closed-form estimate of ICP's covariance[END_REF], which is based on the objective function linearization around the optimal estimation. In order to correctly propagate the uncertainty from the measurement space to the estimation domain, the secondorder derivatives of the objective function are calculated and applied to the initial measurement noise. Nevertheless, it needs to be noted that the closed-form covariance estimation only considers the uncertainty caused by the sensor noise; thus, it could not apply to the local minima situations. Monte Carlo simulation [START_REF] Buch | Prediction of ICP pose uncertainties using monte carlo simulation with synthetic depth images[END_REF] is another branch of the uncertainty modeling methods, which iteratively generates different input point clouds to test the estimated transformation sensitivity. Then several estimates could be computed with different input samples. Based on Monte Carlo simulation scan matching results, the distribution of the relative transformation estimation can be constructed. Nevertheless, brute force sampling is timeconsuming and limits the application scenarios. The pipeline of the proposed approach is shown in Fig. 1. For consecutive LiDAR scans perceived in the vehicle local frame, point cloud pre-processing is necessary before implementing the scan matching. The point cloud pre-processing includes scattered outliers and ground points removal, which will significantly ease the false correspondence matching problem. Afterwards, a robust semi-direct scan matching is applied to compute the 6 DoF motion and to estimate the relative transformation covariance. Since the scan matchingbased localization is bound to drift over time, a pose-graph optimization is then used to reduce the local drift and to render a globally consistent mapping. In order to make the proposed approach easy to follow, the notations for the semi-direct scan matching are explained in Table I.

III. PROPOSED APPROACH

Symbol

Meaning

(x i , x ′ i )
The matched 3D points in the source and target point clouds

V i
The voxel point set for the point cloud downsampling c i

The centroid of 3D points inside

V i |•|
The cardinality of a 3D point set

x G l
The potential 3D ground point Ĝt

The estimated potential 3D ground points set h prior

The LiDAR installation height prior

⃗ n G x l The unit normal vector around x G l x G lz The z-coordinate of x G l γ off
The angle offset between ⃗ n G x l and vertical unit vector PG

The estimated ground plane P *

G

The optimal ground plane h off

The distance offset between x G l and

P * G {S △ t , S △ t+1 }
The pre-processed consecutive LiDAR scans

C ii ′ The inlier correspondence set t+1 T * t
The optimal transformation that aligns the consecutive LiDAR scans

⃗ n ′ i
The unit normal vector around

x ′ i N(x i )
The spherical neighbor point set around x i x

The mean position of points in N(x i ) Σ(x i )

The scattered matrix of points in N(x i )

{λ 1 i , λ 2 i , λ 3 i }
The eigenvalues of the Σ(x i ) eigenvalue decomposition

{e 1 i , e 2 i , e 3 i }
The eigenvectors of the Σ(x i ) eigenvalue decomposition {γ 21 , γ 32 }

The salience thresholds for the ISS features detection

d ij
The edge distance between the point x i and x j

CD(•)

The Chamfer Distance between the aligned point clouds

T f
The feature-based coarse transformation that aligns the consecutive LiDAR scans

T init

The initial transformation that aligns the consecutive LiDAR scans L size

The downsampling voxel size t+1 Tt The estimated transformation that aligns the consecutive LiDAR scans

T -1 t T t+1
The ground truth transformation that aligns the consecutive LiDAR scans

∥(•)∥ 2 The Euclidean norm r = [α, β, γ] T
The XYZ Euler angles parameterization t = [x, y, z] T

The XYZ translation parameterization R(•)

The rotation angle matrix parameterization

(•) ∧ The skew symmetric matrix of cross product multi- plication D(•)
The inconsistency indicator for uncertainty modeling Λ

The quadratic form information matrix A. 3D Point cloud pre-processing 1) Scattered outlier points removal: Sparse and nonpermanent scattered scene components, like tree leaves or bushes, may pose challenges for accurate scan matching. In our approach, a Gaussian distribution-based outlier rejection method is performed to remove the sparse and isolated points. Subsequently, the voxel grid filter with the leaf size L size is applied to down-sample and to approximate the point clouds for efficient scan alignment. For the points inside each voxel point set {x i :

x i ∈ V i }, they are approximated with their centroid c i = xi∈Vi x i |V i |
, where |•| denotes the cardinality of a set. This operation will greatly reduce the number of points without losing the raw points distribution.

2) Ground points removal: When traveling on real roads, the point cloud acquired by the LiDAR sensor naturally contains many ground points. Ground points on flat roads tend to be isotropic and they encode little geometric information for the data association step. A slight deviation during iterative nearest neighbor tracking can cause ground point mismatches and distort the entire scan alignment.

To address these problems, a consensus-based method is used to segment ground points, assuming the vehicle follows a planar motion and the LiDAR installation height is known. On this basis, a set of potential ground points {x G l : x G l ∈ Ĝt } are selected, incorporating all the points that locate h prior along the z-axis under the vehicle roof. Hereby, h prior is highly linked to the LiDAR installation height. Meanwhile, another condition for potential points selection is that the unit normal vector ⃗ n G x l around the potential ground point x G l needs to stay within an offset of γ off from the z-axis of the LiDAR local frame. The offset γ off allows the slightly sloped terrain points to participate in the ground plane estimation, considering the fact that the driving road is not always fully flat.

x G lz < -h prior , [0, 0, 1] • ⃗ n G x l > cos(γ off ) (1)
After that, the consensus-based method iteratively picks three non-collinear points {x G i , x G j , x G k } within the potential ground point set Ĝt . The selected points are used to fit the ground plane equation PG : Ax +By +Cz +D = 0. The distances of the potential ground points {x G l : x G l ∈ Ĝt } to the estimated plane PG are then computed and summed up. The summed distances are used to vote for the plane candidates until the convergence criteria are met. At the end of the iterations, the plane with the least summed point-to-plane distances is chosen as the optimal ground plane P * G . Once the ground plane is identified, points in Ĝt within distance threshold h off to the optimal ground plane P * G are also considered as ground points. This strategy helps to increase robustness against measurement noise for ground point segmentation. Finally, after the scattered outliers and the ground points removal, the pre-processed LiDAR scans {S △ t , S △ t+1 } t=1,••• ,τ -1 can be obtained for the following 3D point cloud registration.

B. 3D Point cloud registration

1) Semi-direct scan matching: According to [START_REF] Pomerleau | Comparing ICP variants on real-world data sets[END_REF], direct scan matching with raw point clouds is less efficient for large baseline motions, while feature-based methods lose their advantages to bootstrap from local minima in feature-less environments. To complement the weakness of individual direct or feature-based methods, a hybrid semi-direct approach is proposed in this paper. The semi-direct LiDAR scan matching pipeline is detailed in Algorithm 1 at the end of Section III-B.1. Hereby, a feature-based method is leveraged to estimate a coarse but globally consistent inter-frame transformation that serves as a prior for the following multi-scale direct dense point cloud alignment. In the proposed semi-direct approach, the point-to-plane distance metric is adopted for the optimal relative transformation t+1 T * t estimation,

t+1 T * t = argmin t+1 Tt (xi,x ′ i )∈C ii ′ ∥(x ′ i -t+1 T t .x i ) T ⃗ n ′ i ∥ 2 (2)
where C ii ′ is the inlier correspondence set incorporating the matched point pairs {(x i , x ′ i )} in the source and target LiDAR scans. The plane unit normal vectors ⃗ n ′ i around x ′ i in the target LiDAR scan could effectively guide the scan matching process to distinguish points lying on different surfaces and discard unreliable correspondences {(x i , x ′ i )}. For the sake of reducing spurious correspondences, the selected features for the sparse point cloud alignment need to be distinctive and invariant to viewpoint changes. To this end, the salience-based intrinsic Shape Signatures (ISS) [START_REF] Zhong | Intrinsic shape signatures: A shape descriptor for 3D object recognition[END_REF] keypoints extraction method is adopted, in which the salience measure is derived from the scatter matrix Σ(x i ) eigenvalue decomposition.

x = 1 |N(x i )| xj ∈N(xi)
x j

(3)

Σ(x i ) = 1 |N(x i )| xj ∈N(xi) (x j -x) (x j -x) T (4)
where N(x i ) are the spherical neighbors of x i within a predefined radius and |•| denotes the number of points within the set. With the scatter matrix Σ(x i ) eigenvalue decomposition, the descending magnitude eigenvalues {λ 1 i , λ 2 i , λ 3 i } can be calculated, and their corresponding eigenvectors {e 1 i , e 2 i , e 3 i } are used to build the intrinsic reference frame. To ensure the distinctiveness of the local features, the ratios of the sequential eigenvalues of the scatter matrix Σ(x i ) are required to not exceed the salience thresholds γ 21 and γ 32 for the pruning purpose [START_REF] Zhong | Intrinsic shape signatures: A shape descriptor for 3D object recognition[END_REF].

λ 2 i /λ 1 i < γ 21 , λ 3 i /λ 2 i < γ 32 (5)
where γ 21 is the upper bound on the ratio between the second and the first eigenvalue and γ 32 is the upper bound on the ratio between the third and the second eigenvalue returned by the scatter matrix eigenvalue decomposition. The salience thresholds γ 21 and γ 32 guarantee that the established intrinsic reference frame exhibits maximum salience along the principal directions, which makes features more informative and recognizable from various viewpoints. As long as the ISS features are extracted from the 3D point cloud, the efficient Fast Point Feature Histogram (FPFH) descriptor [START_REF] Rusu | Fast point feature histograms (FPFH) for 3D registration[END_REF] is used for robust data association. FPFH is basically a 33-dimensional vector that characterizes the local geometry around a point. It efficiently enriches the keypoints description and helps to establish the keypoints correspondences. Through iteratively taking three pairs of matched feature points {(x i ,

x ′ i ), (x j , x ′ j ), (x k , x ′ k
)} and implementing correspondence consistency (edge similarity) check, the transformation matrix T f that optimally aligns the sparse feature points could be obtained by the Random Sample Consensus (RANSAC) method. The correspondence consistency check is to verify the edges distance

{(d ij , d ik , d jk ), (d ′ ij , d ′ ik , d ′ jk )}
formed by the features in each frame. This prevents mismatches in the environment with repeatable features. The correspondences are then considered valid if the features are not collinear

(x i -x j )/d ij ̸ = (x i -x k )/d ik (x ′ i -x ′ j )/d ′ ij ̸ = (x ′ i -x ′ k )/d ′ ik (6)
and their formed edges have similar length as

0.9 < d ij /d ′ ij , d ik /d ′ ik , d jk /d ′ jk < 1.1 (7)
Given a set of associated feature points, the RANSACbased scan matching does not require an initial guess and is more robust to outliers. This alleviates the problem of getting stuck at the local minima. However, in a featureless environment with few distinctive features to be extracted, feature tracking tends to be hard to manage. In this way, we need to evaluate the estimated transformation T f from the feature-based method and the transformation heuristics t Tt-1 . Hereby, the transformation heuristics come from the constant velocity motion model and the evaluation metric used is the Chamfer Distance. The Chamfer Distance CD(•) in ( 8) is a metric to measure the tightness of two aligned point clouds {S t , S t+1 }, with the expression as

CD (S t , S t+1 ) = 1 |S t | xi∈St min x ′ i ∈St+1 ∥x i -x ′ i ∥ 2 + 1 |S t+1 | x ′ j ∈St+1 min x ∈ j St ∥x j -x ′ j ∥ 2 (8)
and |•| stands for the cardinality of the point set. This step helps to choose a more reliable initialization point T init for the dense alignment.

T init = argmin t Tt-1 ,T f {CD( t Tt-1 S △ t , S △ t+1 ), CD(T f S △ t , S △ t+1 )} (9)
Then the multi-scale pyramidal dense point cloud alignment is implemented to refine the coarse initial transformation T init . The multi-scale pyramid defines a two-layer maximum correspondence distance as 3×L size and L size , that are proportional to the downsampling voxel leaf size. The first layer point cloud alignment has the maximum correspondence distance of 3×L size , whose convergence criteria is simple to reach. As a result, the first layer alignment further eliminates the effect of false correspondence and reduces the risk of being stuck in the local minima. Then, based on the result from the first layer, the second layer pyramid searches in a finer scale for the final tight point-cloud alignment. This design adaptively determines the nearest neighbor searching radius without finetuning, which greatly facilitates optimization convergence for partially overlapping point clouds. Apply the ISS keypoints detection, FPFH description to

{S △ t , S △ t+1 } 5:
Implement the keypoints correspondence consistency check and only keep the valid ones using ( 6), [START_REF] Chen | Object modelling by registration of multiple range images[END_REF] 6:

Obtain the rough transformation T f with the consistent keypoints correspondences using RANSAC 7:

Evaluate the Chamfer Distance CD(•) of t Tt-1 and T f , then choose the initial guess T init using [START_REF] Wang | Intelligent vehicle self-localization based on double-layer features and multi-layer LIDAR[END_REF] 8:

Conduct the multi-scale dense alignment with T init and get the relative transformation t+1 Tt 9:

Update the LiDAR scan timestamp t ← t + 1 10: end while 2) Scan matching uncertainty: In the proposed pipeline, the uncertainty quantification is integrated into the pose-graph optimization to limit the frame-to-frame estimation drift. In order to maintain computation efficiency and estimation accuracy, the derivative-free covariance estimation method is adopted to assess the scan matching quality. Owing to the point cloud preprocessing and semi-direct coarse-to-fine point cloud alignment, a considerable number of outliers are filtered out. And we manage to predict the LiDAR scan matching uncertainty in large-scale outdoor scenarios with measurement noise and large viewpoint changes. Given the two consecutive LiDAR scans in the local sensor frame S t and S t+1 , the estimated relative transformation t+1

Tt tightly aligns the corresponding points 1 {x i : x i ∈ S t } and {x Tt , T t , T t+1 ) in ( 14), which reflects the overall discrepancies between the points in the inlier correspondence set C ii ′ . The inlier correspondence set C ii ′ incorporates all valid pairwise correspondences (x i ,

x ′ i ) within a predefined euclidean distance threshold t+1 Tt x i -x ′ i 2 < ϵ.
The error tolerance ϵ is linked to the downsampling voxel leaf size L size , which is set as 0.1 m in our case. And the information matrix can then be extracted via the local parameterization ξ = [r, t] T = [α, β, γ, x, y, z] T that expresses the estimated transformation t+1

Tt and true value transformation T -1 t T t+1 discrepancies in such manner:

T -1 t T t+1 t+1 Tt ≈ I 3 + r ∧ t 0 1 (10)
where r = [α, β, γ] T represents the rotation angles 2 along the x -y -z axes sequentially, and t stands for the corresponding 1 {x i } are points with the homogeneous coordinates and {x ∧ ivec } are skew symmetric matrices of points with the vectorized cartesian coordinates 2 For the local parameterization of transformation discrepancy, the rotation angles are far away from their singular positions of π 2 , thus the gimbal lock issue of Euler angle parametrization is avoided translation parts. And the approximation is made for the infinitesimal rotations angles:

R = R z (γ)R y (β)R x (α) ≈ I 3 +   0 -γ β γ 0 -α -β α 0   (11)
After that, with the cross product (•)× and skew symmetric matrix (•) ∧ substitution,

r × x ivec = -x ivec × r = r ∧ x ivec = -x ∧ ivec r (12)
the inconsistency indicator can be approximated as:

D( t+1 Tt , T t , T t+1 ) = (xi,x ′ i )∈C ii ′ t+1 Tt x i -x ′ i 2 = (xi,x ′ i )∈C ii ′ t+1 Tt x i -T -1 t+1 T t x i 2 = (xi,x ′ i )∈C ii ′ T -1 t T t+1 t+1 Tt x i -x i 2 ≈ (xi,x ′ i )∈C ii ′ ∥r × x ivec + t∥ 2 = (xi,x ′ i )∈C ii ′ -x ∧ ivec I 3 ξ 2 = ξ T Λξ ( 13
)
where Λ is the information matrix in the quadratic form with the expression as follows:

Λ = (xi,x ′ i )∈C ii ′ -x ∧ ivec I 3 T -x ∧ ivec I 3 (14)
The information matrix gives a direct reflection of the tightness of two LiDAR scans being aligned, and its inverse Λ -1 can be considered as the covariance matrix to model the scan matching uncertainty. Compared with the scan alignment itself, the covariance matrix is calculated only once at the final iteration, and its estimation time is negligible since it is derivative-free.

3) Scan matching back-end optimization: In order to reduce the LiDAR scan matching pairwise registration drift, the uncertainty quantification can be integrated into the global pose-graph optimization [START_REF] Kümmerle | G2O: A general framework for graph optimization[END_REF]. A pose graph is linked with edges which represent pairwise registration, and the deviated poses are adjusted and smoothed based on the edge constraints to optimize the overall discrepancies among the co-visible LiDAR scans. With such back-end optimization, the refined poses for point clouds registration can be obtained, which lays the basis for consistent scene mapping.

IV. EXPERIMENTAL RESULTS

To evaluate the performances of the proposed Semi-Direct Scan Matching (SD-SM) approach, extensive experiments have been carried out using the public KITTI [START_REF] Geiger | Vision meets robotics: The KITTI dataset[END_REF] and selfrecorded LS2N datasets at the Centrale Nantes Campus. Compared with more recent public datasets such as Nuscenes [START_REF] Caesar | nuScenes: A multimodal dataset for autonomous driving[END_REF]

RF = |C ii ′ | |S t+1 | , RM SE = 1 |C ii ′ | (xi,x ′ i )∈C ii ′ ∥Tx i -x ′ i ∥ 2
(x i , x ′ i ) : (x i , x ′ i ) ∈ C ii ′ }
among the full target cloud S t+1 , and ideal relative fitness value is close to 1. RMSE measures the root mean square errors of all inlier correspondences, and lower RMSE value represents tighter alignment. In this section, we put the analysis emphasis on three principal parts that include the ground points removal, the semi-direct scan matching, and the uncertainty modeling.

A. Ground points segmentation

According to the KITTI sensor setup specifications 3 , the LiDAR installation height is 1.73 m from the ground. This verifies that our ground plane estimation P * G : -0.00x + 0.03y + 1.00z + 1.75 = 0 is precise and reliable. This can be mainly attributed to the fact that the potential ground points set has been strictly selected and only contains few outliers. As a result, the consensus-based ground plane estimation avoids local minima, which facilitates accurate ground points segmentation. The registration lag effect, which means that erroneous matches on the ground points will disrupt the seamless alignment of the point clouds, is shown in Fig. 2. And the qualitative registration lag effect result in Fig. 2 reveals the necessity to remove the ground points before conducting the LiDAR scan matching.

B. Registration results benchmarking

In order to benchmark the performances of the proposed semi-direct scan matching method, a thorough quantitative evaluation is conducted in various scenarios. The scenario types are listed as residential (KITTI 00, 02-03, 05-10), highway (KITTI 01), urban (KITTI 04), and outdoor parking (LS2N 00-01) areas. In these different driving scenarios, the vehicle ego-motion varies a lot. And it provides high-speed motion on the highway and mild motion in the parking area for us to investigate the performance of different LiDAR scan matching approaches. Table . IV lists the RF and RMSE metric values of the state-of-the-art scan matching methods as well as our approach, including Direct ICP [START_REF] Chen | Object modelling by registration of multiple range images[END_REF] (D-ICP), Featurebased ICP [START_REF] Rusu | Fast point feature histograms (FPFH) for 3D registration[END_REF] (F-ICP), Generalized ICP [START_REF] Segal | Generalized-ICP[END_REF] (G-ICP) and our Semi-Direct Scan Matching (SD-SM). The scan registration with the relative fitness below 30 % is considered as invalid and the RMSE value is not computed in that case.

From Table . IV, it is shown that the dense point cloud alignment using the direct ICP is generally superior to the sparse feature-based ICP. Particularly, for the sequence of LS2N 00, direct ICP outperforms other approaches in terms of the fitness metric. It is due to the fact that the ego-vehicle moves slowly in the parking area. Since more raw point cloud information is leveraged for scan matching, the direct ICP has better performance when the subsequent scans that share adequate overlapping areas. The sparsity of the point clouds obtained by 16-layer LiDAR is another reason for the poor performance of the feature-based method. However, when the ego-vehicle experiences dramatic viewpoint changes, the direct ICP loses its advantages, especially with inadequate initialization of the identity matrix transformation. Due to the unanticipated decrease of overlaps, the inherent iterative nearest neighbor association strategy of D-ICP is vulnerable and tends to be stuck in the local minima, see Fig. 3(a). On the contrary, high-dimensional feature descriptors are invariant to viewpoint changes. And the feature-based scan matching is more robust for the large baseline motion across consecutive frames. The putative descriptor-based keypoints correspondences are constructed beyond iterative nearest neighbor searching and association loop, which reduces the risks of being stuck in the local minima. With pre-established correspondences, the initialization-free sampling consensus-based method is applied to reject potential outliers and to obtain the optimal relative transformation. Nevertheless, it also needs to be mentioned that the feature-based scan matching is highly dependent on the feature detection procedure. This may lead to inaccurate or biased registration in the feature-less or feature-repetitive environments such as corridors or highways, see Fig. 4(a). The drawbacks of direct and feature-based scan matching reveal the necessity for their combination to provide more robust estimation in information-deprived environments. From the benchmarking results, it can be noticed that our semidirect approach outperforms the state-of-the-art methods by a considerable margin in various scenarios. It is shown that the proposed method achieves the leading results with 68.3 % average relative fitness and 5.71 cm average RMSE distance, respectively. It can be seen that our semi-direct method implements the scan matching in a coarse-to-fine manner. Thus, it is less sensitive to unmodelled artifacts such as moving objects, undergoing the view occlusion, viewpoint changes, and information-deprived environments, see Fig. 3(b). For instance, it can be seen from Fig. 5 that a van constantly appears in front of the ego-vehicle for the KITTI sequence 04. The existence of moving objects in the scene greatly degrades the performance of both direct and feature-based ICP methods, see the 7th row of the Table . IV. In this case, the D-ICP and F-ICP tend to be stuck in the local minima, which may partially align the point clouds and provide low fitness registration results.

Indeed, the undergoing environments and realized egotrajectories have a deep impact on the performance of LiDAR scan matching. To be more specific, our approach obtains promising results in the highway scenarios (KITTI 01), which is very challenging for direct and feature-based methods because of the relatively low frame rate compared with the high-speed ego-motion (see the 4th row of Table. IV). The adaptive initialization strategy in [START_REF] Wang | Intelligent vehicle self-localization based on double-layer features and multi-layer LIDAR[END_REF] ensures that the starting point for the pose optimization is not biased. With a reliable initialization point considering the constant velocity motion The pose uncertainty estimation helps to bound the pose error within a known confidence interval. In this part, the proposed uncertainty modeling approach is benchmarked with the Monte-Carlo simulation-based method in [START_REF] Buch | Prediction of ICP pose uncertainties using monte carlo simulation with synthetic depth images[END_REF]. The detailed implementation of the Monte-Carlo simulation-based scan matching uncertainty modeling can be seen from Algorithm 2. It can be seen from Fig. 6 that our predicted confidence interval could accurately bound the estimation errors during most of the time. We attribute this primarily to our pre-conducted outdoor ground points removal, scattered outliers removal and multi-level semi-direct scan matching that incorporate more reliable correspondences for the pose estimation. As it can be inferred from [START_REF] Wang | PointLoc: Deep pose regressor for LiDAR point cloud localization[END_REF], more valid correspondences will result in a more confident state estimation, which is in line with the principle of maximum likelihood estimation. It can also be seen from ( 14) that the farther the inliers {x i } locate from the local sensor frame, the more confidence we gain from the pairwise correspondences. While it is observed that few overshoots occur at the road intersection or due to some moving objects in the scene, see Fig. 5 and Fig. 8. In the KITTI dataset, the LiDAR point clouds were captured at the frequency of 10 Hz, on which basis the relation between the frame number and timestamp could be established. The ego-motion at the road intersection in Fig. 8 will cause inadequate overlapping of subsequent LiDAR scans for state estimation, and the scan matching convergence can not be guaranteed in this situation. Besides, the existence of the moving object in Fig. 5 further complicates the point cloud registration process and may lead to inconsistency in uncertainty estimation. The LiDAR scans' inadequate overlapping and moving objects' contamination also pose challenges for the Monte-Carlo sampling methods, which can be seen from Fig. 7. The outlier frame numbers of two typical sequences KITTI 04 (moving objects contamination) and KITTI 05 (road intersection partial overlapping) are summarized in Table . V for better visualization. Apart from these few overshoots, the Monte-Carlo sampling performs very well in estimating the pose estimation uncertainty. The different voxel sizes' downsampling will approximate the raw point clouds at different scales, which helps the Monte-Carlo simulation to boost from the local minima while maintaining the estimation accuracy. Compared with the sampling-based Monte-Carlo simulation scan matching uncertainty estimation, Calculate the distribution of the estimated motion parameters that align {S △ t , S △ t+1 } 7: end while our proposed method achieves comparable performances. It can be seen from Algorithm 2 that the pre-processed LiDAR scans are downsampled with five different voxel sizes for Monte-Carlo simulation. And then the semi-direct LiDAR scan matching is conducted iteratively five times with different downsampled LiDAR point clouds to obtain the final distribution of the estimated motion parameters. However, for the proposed method, the semi-direct scan matching is conducted only once. And the covariance matrix is calculated at the final iteration based on the inconsistency indicator, which is derivative-free. Thus, the proposed method is much more computationally efficient than the Monte-Carlo simulation uncertainty estimation.

C. Uncertainty modeling

In the proposed approach pipeline, the uncertainty quantification is integrated into the global pose-graph optimization [START_REF] Kümmerle | G2O: A general framework for graph optimization[END_REF]. This helps to reduce the pairwise registration drift and to obtain consistent scene mapping, where the point clouds are registered with high fidelity. In our case, in order to accelerate the pose-graph optimization convergence, only the pose nodes in the graph are adjusted and the environment 3D points are not involved. By adjusting the pose nodes in the pose graph, the overall cost function will be minimized and the refined 6 DoF pose can be obtained. The scene mapping is fact the registered point clouds with the refined 6 DoF pose. It can be seen from Fig. 9 that, the scene mapping is consistent and the vehicle poses are smooth even with a sharp turning in the trajectory.

V. CONCLUSION AND FUTURE WORK

In this paper, an adaptive semi-direct LiDAR scan matching approach is proposed to overcome the weakness of individual direct or feature-based methods. The semi-direct LiDAR scan matching guarantees convergence in challenging environments such as undergoing high-speed motion and traveling in repetitive, feature-less environments. On this basis, extensive registration results on city, residential, highway, and parking scenarios with the relative fitness and RMSE metrics are presented and discussed. The superiority of the proposed semidirect LiDAR scan matching method is validated with both HDL-64E and VLP-16 Velodyne LiDARs. At the same time, scan matching uncertainty is modeled as well to evaluate the final convergence accuracy. Furthermore, we also analyze the possible sources that may lead to scan matching divergence in various scenarios. It is demonstrated that ground points and dynamic objects such as vehicles or pedestrians are the main causes of the estimation accuracy decrease. It is also noticed that significant errors frequently occur near road intersections and in highway scenarios, where it is more likely to come across dynamic vehicles and the geometric information is not adequate for reliable state estimation.

The main contribution of this paper lies in the front-end improvement of LiDAR-based state estimation and uncertainty modeling. For future work, we will focus on integrating the GNSS measurements [START_REF] Zhu | GNSS integrity monitoring schemes for terrestrial applications in harsh signal environments[END_REF] in the back-end to correct the odometry drifts for the large-scale scene [START_REF] Yan | EU long-term dataset with multiple sensors for autonomous driving[END_REF] application. Moreover, the semantic information can be leveraged for the moving objects segmentation [START_REF] Chen | A semantic-guided LiDAR-vision fusion approach for moving objects segmentation and state estimation[END_REF], which increases the automotive perception system. More efficient outliers rejection mechanisms for partially overlapping point clouds that incorporate the IMU pre-integration will also be studied to further ameliorate the scan matching accuracy and robustness with the presence of several moving objects in the scene.

′

  i : x ′ i ∈ S t+1 } in the point clouds. Essentially, the uncertainty estimation is based on the inconsistency indicator D( t+1

( 15 )

 15 and |•| denotes the cardinality of the inlier correspondences set. The Relative Fitness (RF) measures the proportion of associated inliers {

Fig. 2 .

 2 Fig. 2. Qualitative results for the registration lag effect caused by the ground points erroneous matches for the KITTI sequence 05

Fig. 3 .Fig. 4 .

 34 Fig. 3. Point cloud registration results visualization at the road intersection for frame 133-134 in the KITTI sequence 05

  (a) Frame 126 in the KITTI sequence 04 (b) Frame 143 in the KITTI sequence 04

Fig. 5 .

 5 Fig. 5.Moving objects in the scene degrade the state estimation performance for the KITTI sequence 04

Fig. 6 .

 6 Fig. 6. Inconsistency indicator-based scan matching uncertainty along tx, ty and rz for the KITTI sequence 04

Fig

  Fig. Monte-Carlo simulation-based scan matching uncertainty estimation along tx, ty and rz for the KITTI sequence 04

Fig. 8 . 3 : 4 : 5 :

 8345 Fig. 8. Partially overlapping point clouds at the road intersection degrade the scan matching performance for the KITTI sequence 05

  (a) The main parking scene visualization with the front camera (b) The main parking scene reconstruction with the VLP-16 LiDAR

Fig. 9 .

 9 Fig. 9. The main parking area scene mapping and self-positioning with the VLP-16 LiDAR at the Centrale Nantes Campus (LS2N 00)

Songming

  Chen received the M.S. degree in Automatic Control and Robotics from the École Centrale de Nantes, France, in 2019. And then he obtained the Ph.D. degree in Automation and Robotics from the École Centrale de Nantes, within the ARMEN Team of LS2N Laboratory, UMR CNRS 6004, France, in 2022. His research interests belong to multi-sensor based perception and state estimation for autonomous vehicles. Vincent Fr émont received a Ph.D. degree in Automatic Control and Computer Science from the École Centrale de Nantes, France, in 2003. He is currently a Full Professor at École Centrale de Nantes, within the ARMEN Team of LS2N Laboratory, UMR CNRS 6004. His research interests belong to perception systems for autonomous mobile robotics with an emphasis on computer vision, machine learning and multisensor fusion.

3D LiDAR Scans 3D Point Cloud Registration Semi-direct Scan Matching Scan Matching Uncertainty Raw Point Clouds Valid Matches 3D Point Cloud Pre-processing Initial Poses Pose Uncertainties Scan Matching Back-end Optimization Refined Consistent Scene Mapping Scattered Outlier Points Removal Ground Points Removal Pre-processed Point Clouds Poses

	Fig. 1. Overview of the proposed semi-direct LiDAR scan matching and
	scene mapping framework

  Algorithm 1 Semi-Direct Scan Matching Algorithm Input: Consecutive LiDAR scans {S t , S t+1 } t=1,••• ,τ -1 , Relative transformation heuristics t

			Tt-1
	Output: Estimated transformation t+1	Tt that tightly aligns
		the consecutive scans
	1: Initialize 1	T0 ← I 4
	2: while New LiDAR scan arrives do
	3:	Remove the scattered outliers and ground points to get
		the processed LiDAR scans {S △ t , S △ t+1 }
	4:

TABLE II THE

 II PARAMETERS TABLE FOR THE PROPOSED SD-SM Our self-recorded dataset is collected with a light-weight Velodyne VLP-16 LiDAR, which only has 16 layers and renders sparser point clouds than the HDL-64E. The scene characteristics for the experimental datasets are described in TableIII, which include the scenario type, moving objects occurrence, and the number of road intersections. The parameters chosen for the proposed approach are summarized in the TableII. And all the LiDAR scans have been undistorted by compensating for the vehicle egomotion. The two evaluation metrics are the Relative Fitness (RF) and Relative Root Mean Square Error (RMSE) of the inlier correspondences {C ii ′ }, that can be expressed as

	L size	h prior	γ of f	h of f	γ 21	γ 32
	0.1m	1.4m	π 5 rad 0.2m 0.975 0.975
	and PandaSet [26], the KITTI dataset is more dedicated to the
	ego-motion estimation with available precise 6 DoF ground
	truth poses. The KITTI dataset point cloud is acquired with
	a Velodyne HDL-64E laser scanner, which has 64 channels
	with a maximum range of 120 m.

TABLE IV REGISTRATION

 IV RESULTS BENCHMARKING WITH METRICS OF RF(%) AND RMSE (cm)

	Dataset	Method	D-ICP	F-ICP	G-ICP	SD-SM
			RF	RMSE	RF	RMSE	RF	RMSE	RF	RMSE
	KITTI 00	47.7%	5.83	27.0%	-	59.7%	5.68	77.2%	5.53
	KITTI 01	26.8%	-	24.8%	-	27.2%	-	49.4%	6.23
	KITTI 02	38.1%	6.18	29.9%	-	57.7%	5.94	73.2%	5.75
	KITTI 03	47.2%	6.08	27.7%	-	62.8%	5.83	67.2%	5.75
	KITTI 04	31.7%	5.91	30.9%	6.03	31.6%	5.92	65.4%	6.05
	KITTI 05	43.8%	5.80	28.9%	-	61.4%	5.64	76.4%	5.51
	KITTI 06	27.6%	-	19.8%	-	40.9%	5.84	62.3%	5.75
	KITTI 07	56.8%	5.49	33.4%	5.98	62.4%	5.36	78.7%	5.26
	KITTI 08	39.1%	6.05	24.5%	-	50.4%	5.91	70.7%	5.68
	KITTI 09	28.3%	-	21.4%	-	52.6%	6.07	68.8%	5.85
	KITTI 10	49.5%	5.95	30.7%	6.38	69.8%	5.62	76.0%	5.53
	LS2N 00	74.7%	5.37	69.9%	5.56	74.3%	5.41	72.7%	5.34
	LS2N 01	44.8%	6.11	32.9%	6.40	44.9%	6.07	49.5%	6.02
	Average	42.8%	5.88	30.9%	6.07	53.5%	5.77	68.3%	5.71

http://www.cvlibs.net/datasets/kitti/setup.php

This work was carried out in the framework of the NExT Senior Talent Chair DeepCoSLAM (ANR-16-IDEX-0007), through the program Investments for the Future managed by the French National Agency for Research. The first author was also funded by China Scholarship Council.

The authors are with Nantes