Setting Method of Downsampling Factor and Grid Factor for NDT Relocation Algorithm in Dynamic Environment

Normal Distribution Transform (NDT) algorithm plays the role of detecting the relative pose of the vehicle in high-precision map. In this paper, a method of setting downsampling factor and grid factor for NDT relocation algorithm in dynamic environment is proposed, which can solve the problems of excessive NDT relocation error and location loss caused by dynamic objects accounting for 1% to 35% of the volume of scanning point cloud in vehicle environment. To simulate a real dynamic point cloud environment, the single-frame LiDAR point cloud space is voxelized into a mesh. Each grid is assigned a random number evenly distributed between 0 and 1. The threshold value for whether to add a Gaussian noise point is also set. Seven representative dynamic objects on the highway are selected. The threshold value of probability distribution function of Gaussian noise object needs to be set. Then the volume content of the dynamic object in the single frame point cloud space is calculated according to the set threshold value by using the definite integral. By changing the content and volume of dynamic obstacles in a dynamic environment, the effects of the downsampling factor and the grid factor on the accuracy of the repositioning trajectory are obtained. The resampling and mesh coefficients are optimized based on the analysis of the repositioning trajectory accuracy. The results show that When the current sampling factor is fixed, the grid factor of the NDT algorithm is proportional to the RMSE factor. When the NDT grid factor is fixed and the down sampling factor is equal to the side length of the obstacle, the NDT relocation accuracy is the highest and reaches the local optimum.


I. INTRODUCTION
Relocation plays a critical role in autonomous driving related applications, such as localizing a vehicle in a known map, and maintaining the accuracy of simultaneous localization and mapping (SLAM). During the last two decades, a variety of relocation methods have achieved great success in tackling The associate editor coordinating the review of this manuscript and approving it for publication was Okyay Kaynak . such problems using Global Positioning System (GNSS) [1], [2], [3], Inertial Measurement Unit (IMU) [4], [5], [6], camera [7], [8], LiDAR [9], [10], [11], and other perceptual sensors.
A common solution to these problems is given by Inertial Navigation Systems (INS) employing GNSS and an IMU. However, GNSS suffer from disruptions or signal loss due to obstruction of the sky caused for example by city [1]. One solution to this problem is given by using odometry information to compensate for sudden jumps in the GNSS measurements [12], [13]. However, the performance often remains insufficient for autonomous driving related applications.
Cameras are one of the most attractive relocation sensors because of their inherent high information content, low cost, and small size. Visual relocation uses the large amount of information provided by the camera to estimate robot position [14], [15], [16], [17], [18]. Zheng [19] proposes a new visual measurement framework to achieve accurate, reliable and cost-effective vehicle positioning in semi-obscured and GPSobscured urban environments. In most practical work, the method of visual relocation using a camera is susceptible to changes in scene illumination. In order to improve the robustness of the visual relocation to the changes of ambient illumination in the actual work, Xu [20] proposed a method of constructing multiple maps of different illumination intensities. By means of constructing multiple maps with different light intensities, a map with consistent lighting is then selected based on the current image brightness. However, for outdoor large-scale relocation, ensuring its robust operation is still very challenging especially in changing environments.
An increasingly popular solution to the problem of moving vehicle positioning in city is given by dropping GNSS and camera measurements altogether, and by relying on a LiDAR that measures 3D scans of the environment. Compared with cameras and GNSS, LiDAR has better penetrability and anti-interference characteristics. Furthermore, the amount of data acquired by a LiDAR system is less than in the case of a camera. Hence the LiDAR sensor is used frequently for positioning and object detection [21]. However, most LiDAR-based localization solutions with prior point-cloud maps [22], [23], [24], [25] assume that the road scenes are relatively constant, while new constructions, road-side vegetation, partial occlusions by changing objects may severely compromise robustness. Therefore, an interesting but open question is whether LiDAR can be used for robust relocalization in large-scale changing environments.
In this article, the dataset is used to test the accuracy of the NDT relocation system in a dynamic environment. Our main contributions are summarized as follows: • A dynamic environment simulation and dynamic object content solution method based on Gaussian noise is presented. Single-frame point cloud voxels are gridded, and each grid is given a random number that is evenly distributed between 0 and 1. A threshold value for selecting a voxelized grid is set. The Gaussian noise point is added to the selected voxel grid. Finally, the volume content of dynamic objects in the single-frame point cloud space is calculated based on the threshold value selected by the definite integral.
• A method for calculating the number of LiDAR Points of a dynamic Gaussian noise object is presented. Seven dynamic objects with high frequency in highway environment were selected as the test target. When adults are 5m away from LiDAR, 100 LiDAR Points in disturbed point cloud space of 1 cubic meter are taken as the standard. The number of Gauss noise points added to a dynamic object is the dynamic object perturbation point cloud space volume multiplied by the number of LiDAR Points per unit point cloud space 100.
• A method for optimizing the settings of the downsampling coefficients and grid coefficients based on the NDT relocation algorithm in a dynamic environment is presented. By changing the content and volume of dynamic obstacles in a dynamic environment, the effects of the downsampling factor and the grid factor on the accuracy of the repositioning trajectory are obtained. Optimal solution of downsampling and grid coefficients using repositioning Trajectory Accuracy.
The remainder of this paper is organized as follows. Section II presents a literature review of existing studies on this topic. Section III then presents the proposed methodology. The practical experiments and the corresponding results are provided in Section IV. Finally, Section V draws some conclusions.

II. RELATED WORK
With the increasing interest in robotic technology, the construction of high-precision maps and the implementation of repositioning algorithms are being actively studied to enable automobile driving on roads.

A. SIMULTANEOUS LOCALIZATION AND MAPPING
New technologies such as machine vision and SLAM have tremendous potential in autonomous driving. SLAM is divided into two main methods: visual simultaneous localization and mapping (V-SLAM), which depends on a camera, and light detection and ranging simultaneous localization and mapping (LiDAR-SLAM), which depends on a LiDAR sensor. Visual SLAM can be divided into direct and indirect methods according to different ways of estimating camera motion. The direct method estimates camera motion by minimizing the photometric error of image pixels. SVO uses a classic sparse direct SLAM system [26]. The position estimation is performed through FAST features in the image. Because the number of estimated pixels is very small, its operation efficiency is very high.Because of the high sensitivity of the image to illumination changes, the visual SLAM system using direct methods has poor robustness. The indirect method, also known as the feature point method, estimates the camera posture by matching the feature points between images. The ORB-SLAM2 system is the most classic visual SLAM system based on the feature point method [27]. The system integrates three threads of tracking, local map, and loopback detection, which can effectively improve the running speed and pose accuracy of the system. Even though ORB-SLAM2 is very active in automatic driving. But many walls and windows in a city are made of glass or vinyl and are easily exposed to light during the day. VOLUME 10, 2022 The LiDAR sensor not only provides centimeter level distance information, but also has a wide detection range, independent of lighting conditions [28], [29]. LiDAR Odometry and Mapping (LOAM) [30], [31], [32] is presently the most representative real-time 3D laser SLAM algorithm based on feature matching. It has a small amount of calculation and motion compensation. LOAM performs point feature to edge/plane scan-matching to find correspondences between scans. Features are extracted by calculating the roughness of a point in its local region. Real-time performance is achieved by novelly dividing the estimation problem across two individual algorithms. One algorithm runs at high frequency and estimates sensor velocity at low accuracy. The other algorithm runs at low frequency but returns high accuracy motion estimation. The two estimates are fused together to produce a single motion estimate at both high frequency and high accuracy. The mapping accuracy of LOAM in dataset is always in the forefront, and the effect is also very good in the actual environment, so it can be used to build high-precision maps, as shown in Fig.1.

B. RELOCATION ALGORITHM
The NDT algorithm [33], [34], [35], [36], [37] is used to relocate maps with deviations [38], [39]. Instead of comparing the difference between point clouds and points in two cities, NDT first converts a high-precision map of a city into a normal distribution of multidimensional variables. If the transformation parameters make the two LiDAR data match well, the probability density of the transformation points in the reference system will be high. Therefore, when the transformation parameters are optimized to maximize the sum of probability density, the LiDAR point cloud data of the two cities will be best matched, as shown in Fig.2.
The use of a normal distribution to represent an otherwise discrete the high-precision map has many benefits. This chunked, smooth representation is continuously derivable and the probability density function of each lattice can be thought of as an approximation to a local surface, which not only describes the location of the surface in space, but also contains information about the orientation and smoothness of the surface.

III. NDT RELOCATION
First, it is necessary to load the high-precision map and process the LiDAR point cloud data obtained by LiDAR scanning. Then, the LiDAR point cloud data is divided into voxel grids. Gaussian noise is added to the obtained random voxel grids to simulate real obstacles. Finally, the simulated point cloud with added Gaussian noise is voxel down sampled as the input point cloud of NDT relocation algorithm. The specific steps are shown in Fig.3.

A. PROBLEM DESCRIPTION
Relocation loss or low accuracy due to uncertainty in the size and number of point cloud objects in a dynamic point cloud environment. Therefore, the low error NDT location in dynamic environment is a big challenge. Downsampling coefficient and grid coefficient have a great relationship with the relocation accuracy. Set the function relationship between the downsampling factor a and the NDT relocation error to be f D (a), The function relationship between the grid factor b and the NDT relocation error in the NDT relocation algorithm is f N (b), so the function relationship between the NDT relocation error and the grid factor B in the downsampling factor A and the NDT relocation algorithm is So the minimum error formula we need to get for NDT is:

B. DYNAMIC POINT CLOUD ENVIRONMENT SIMULATION
In order to obtain voxel grids at random locations, give each square a random number λ i is evenly distributed between [0,1], i is the ordinal number of the voxel grid in single-frame point cloud space, H (λ i ) is the function relationship of adding Gaussian noise to the single voxel grid. z is the threshold value that directly affects the probability value P of adding noise, ranging from 0 to 1, then the function formula of generating a Gaussian noise barrier is: f P (x) is the probability density function of adding Gaussian noise to each grid. Then the probability of each lattice adding noise to become a noise lattice is P, and the formula is: The location of each voxel grid obtained is random. In order to obtain the maximum capacity of the algorithm for dynamic obstacles of different volumes, the size of voxel grid probability n can be specified. After obtaining the random voxel grid, the Gaussian noise is added to the obtained voxel grid [40], [41], [42], [43], [44], [45]. The calculation method can be easily obtained by using the knowledge of normal distribution in probability theory. The probability density of Gaussian noise obeys the Gaussian distribution [46], [47], [48], and the formula of the Gaussian distribution is: Here µ is the mean value of the voxel grid taken, and the value of σ is a quarter of the voxel grid. The number of noise points for each grid is calculated according to the actual situation.
Based on the size of point cloud space disturbed by objects in the real dynamic environment, the spatial volume of point  cloud disturbed by common objects on the road is calculated. According to the safety requirements of highways, the safe distance between pedestrians and vehicles is larger than 3m. The average distance between the writer and the LiDAR is set to 5m. At this time, the number of LiDAR points constituting adults is about 200. The space volume of Adult Normal perturbation point cloud is about 2m 3 . Therefore, 1m 3 the number of additional LiDAR noise points in the volume point cloud space is set to 100. Data for other common objects on roads is shown in Table1.

C. VOXEL DOWNSAMPLING
The more dense the input point cloud points, the more computational complexity is required for NDT registration. Autopilot LiDAR positioning has higher real-time requirements. The less time it takes for point cloud registration, the better. Therefore, we can improve the speed of NDT registration by downsampling the input point cloud. To avoid the overlap between the added obstacle volume and the downsampling grid, only LiDAR point clouds within 60m are retained. In each voxel grid, the center of gravity of the voxel grid is used instead of all points in the voxel to display the other points in the voxel. So that all points in the voxel are represented by a single center of gravity. As shown in Fig. 5. As a result, the number of points is reduced, the matching speed is improved, the shape characteristics of the point cloud remain basically unchanged, and the spatial structure information is preserved. The larger the voxel raster selection, the fewer point clouds sampled, and the faster the processing speed. However, the original point cloud will be too fuzzy, and the smaller the voxel raster selection, the opposite effect will be achieved [49], [50], [51], [52].

D. NDT RELOCATION ALGORITHMS
The first step is to grid 3D high-precision map point cloud. The cube is used to divide the entire space of the LiDAR points. Finally, the probability density function for each grid is calculated based on the points in the grid.
where µ is the mean of the normal distribution of the grids of the high-precision map, m indicates the number of points in the high-precision map grid, k means the kth point in the high-precision map grid, y k=1,...,m for all scanned points in a high-precision map grid, denotes the covariance matrix of the high-precision map grid, The probability density function of a grid can be described as The use of a normal distribution to represent an otherwise discrete the high-precision map has many benefits. This chunked, smooth representation is continuously derivable and the probability density function of each lattice can be thought of as an approximation to a local surface, which not only describes the location of the surface in space, but also contains information about the orientation and smoothness of the surface.
When using NDT registration, the goal is to find the pose of the current LiDAR scan in such a way as to maximize the likelihood that the currently scanned points lie on the surface of the high-precision map. The parameter we then need to optimize is the transformation (rotation, translation, etc.) of the currently scanned LiDAR point cloud, which we describe using a transformation parameter p. The current scan is a point cloud X = { x 1 , . . . , x n }, given the set of scan points X and the transformation parameter p, such that the spatial transformation function T ( p, x k )denotes the use of the pose transformation p to move the points x k ,combined with the previous set of density-of-state functions (one PDF for each grid), then the best transformation parameter p should be the pose transformation that maximizes the likelihood function: Then maximizing the likelihood is also equivalent to minimizing the negative log-likelihood − log : Then there is the optimization section. An optimization algorithm is used to tune the transformation parameter p to minimize this negative log likelihood. The NDT algorithm uses Newton's method for parameter optimization. Here the probability density function f ( x) does not have to be normally distributed; any probability density function that reflects the structural information of the scanned surface and is robust to anomalous scan points will do.

A. DATA ANALYSIS
In order to test the robustness of the NDT relocation algorithm in a dynamic environment, we used dynamic environment dataset set with a total length of 486.750m and a duration of 310s.
To explore the effects of the mesh and downsampling coefficients of the NDT algorithm on the repositioning accuracy in a dynamic environment and under what conditions can the repositioning accuracy be highest. The transformation epsilon is 0.05, the step size is 0.1, and the maximum iterations is 30. The repositioned root mean square error data is used as a measure of accuracy. As shown in Fig. 6.
Fixed downsampling factor is needed to study the effect of the grid factor of NDT algorithm on the repositioning accuracy. Normally, 16-line LiDAR uses a down-sampling factor  of 1 m to 2 m, and 32-line LiDAR uses a down-sampling factor of 2 m to 3 m. The LiDAR used in this paper is 64-line LiDAR with a sampling factor of 3.0. As an example, the noise-added voxel grid is set to 2.47m, the content is set to 0.07, and the grid factor of NDT algorithm is 0.5m per interval.
As shown in Fig. 7, it can be seen from the figure that when the downsampling coefficient is fixed, the grid coefficient is proportional to the RMSE value. The larger the grid coefficient, the lower the accuracy. When the grid coefficient of the NDT algorithm is too small, the NDT algorithm cannot relocate, because the LiDAR point cloud data is played at 10hz speed, and the real-time performance cannot be met when the grid coefficient is too small. Therefore, in order to meet the real-time requirements, the grid coefficient needs to be increased, but when the grid coefficient is too high, the relocation accuracy will be affected. Therefore, in a dynamic environment, the grid coefficient should be as small as possible. The experimental results show that when the sampling factor is 3.0 and the unit is 0.5, the lowest grid factor of the NDT algorithm is 1 while the robustness of the algorithm is guaranteed.
In order to explore the relationship between the downsampling factor and the repositioning accuracy in the real environment, the repositioning accuracy can reach the highest when the downsampling factor is chosen. The transformation epsilon is 0.05, the step size is 0.1, the resolution is 1.0, and the maximum iterations is 30. 10 sets of limit content tests were performed on each volume dynamic target, with the average root square error value of successful repositioning as the final result to reduce the risk. As shown in Table2.
In the dynamic environment, the larger the unit dynamic object volume, the greater the impact on the NDT relocation algorithm, and the smaller the capacity in the dynamic environment, so the smaller the limit content of dynamic objects. In the process of testing the limit content of dynamic objects, the number of times that the limit content of a single large dynamic object loses its location is more than that of a single small dynamic object.
In the NDT relocation algorithm, the grid is set to 3m. When the volume is between 0 m 3 and 5 m 3 , the limit error increases gradually. Because the volume of a single dynamic object is small, but the number is large and dispersed, the influence on the NDT relocation algorithm is increasing. As the volume of the dynamic object increases, the limit precision of the NDT repositioning algorithm increases from 5 m 3 to 27 m 3 , and the effect on the positioning accuracy decreases because the number is too small. When the volume of a dynamic object is between 27 m 3 and 60 m 3 , the positioning accuracy decreases because the volume of a unit object is too large to have a greater impact on the environment.
As shown in Table3, it is found that when the edge length of the object in dynamic environment is 3 and equal to the downsampling factor, the root mean square error is the smallest and the accuracy is the highest. In order to explore the relationship between the downsampling factor and the repositioning accuracy in the real environment, and to determine the value of the downsampling factor, the repositioning accuracy can be the highest, as well as the influence of the downsampling factor and the NDT grid factor on different size obstacles. Because the downsampling factor  range is small when the NDT grid factor is 1.0. The NDT mesh factor is set to 1.5. Using the limit content of obstacles in the dynamic environment, the average RMSE is calculated when the downsampling factor is the barrier edge length value.
From the above experiments, it can be found that when the grid factor of NDT algorithm is fixed, there is no significant relationship between the downsampling factor and the repositioning accuracy. However, when the downsampling factor is constant and the edge length of the obstacle that appears most frequently in the dynamic environment is equal to the downsampling factor, the accuracy of relocation in this environment is the highest in many different environments. Thus, a local optimal result can be obtained.
In combination with the above experimental results, the downsampling factor is set to the edge length of the obstacle with the highest frequency in dynamic environment. The convergence law is used to minimize the NDT grid factor to obtain an appropriate factor. Finally, the average RMSE value is calculated. As shown in table 4.
When the downsampling factor is equal to the maximum obstacle edge length value in the dynamic environment and the NDT grid factor is the smallest, the redefinition accuracy is the highest, reaching the local optimum.

V. CONCLUSION
This paper solves the problem of excessive NDT relocation error and relocation loss caused by the random occurrence of dynamic objects accounting for 1% to 35% of the scanning point cloud volume in the vehicle environment. A calibration method of down sampling coefficient and grid coefficient based on NDT relocation algorithm in dynamic environment is proposed. To simulate a real dynamic point cloud environment, single-frame LiDAR point cloud spatial voxels are gridded. Each grid is given a random number that is evenly distributed between 0 and 1. The threshold value for adding Gaussian noise points to the selected voxelized grid is set. Seven representative dynamic objects on the highway are selected. The number of Gaussian noise points needed to be added for each dynamic object is obtained by multiplying the space volume of point cloud disturbed by the dynamic object in the real dynamic environment by the number of Gaussian noise points needed to be added per unit volume. Then the volume content of the dynamic object in the single frame point cloud space is calculated according to the set threshold value by using the definite integral. By changing the content and volume of dynamic obstacles in a dynamic environment, the effects of the downsampling factor and the grid factor on the accuracy of the repositioning trajectory are obtained. When the current sampling factor is fixed, the grid factor of the NDT algorithm is inversely proportional to the RMSE factor. The smaller the NDT grid factor, the higher the accuracy in the dynamic environment. When the NDT grid factor is fixed and the downsampling factor is equal to the side length of the obstacle, the NDT relocation accuracy is the highest and reaches the local optimum. In the dynamic environment, it is recommended that the sampling coefficient should be equal to the length of the obstacle side, and then the NDT grid coefficient should be as small as possible. It can effectively improve the positioning accuracy.