Algorithm 2. LiDAR localization in prior 3D point cloud map

Input:

RTK-GNSS position zi

Point cloud pi

Prior 3D point cloud global map M

Output:

LiDAR localization position xlidar

1: Load 3D point cloud map M

2: if get the initial position zi then

3:   load local submap Msub from global map M

4:   if need update submap Msub then

5:    update submap Msub

6:   else

7:    calculate position between pi and Msub:

8:     NDT registration xlidar = pi ∝ Msub

9:   end if

10: else

11:   wait for initial position zi

12: end if

13: return LiDAR localization position xlidar