Algorithm 1. The process of back-end optimization

Input:

LiDAR odometry position xi, xj

RTK-GNSS position zi

Loop closure position zi,j′

Output:

Optimized vehicle position xopt

1: Trajectory alignment for xi, zi and zi,j′

2: for each position xi do

3:   if meet optimization cycle times h then

4:    execute optimization process:

5:    xopt = arg min F(xi, xj, zi, zi,j′)

6:   else

7:    add RTK-GNSS position zi constraint

8:    if loop closure position detected then

9:      add loop closure position zi,j′ constraint

10:    end if

11:   end if

12: end for

13: return optimized vehicle position xopt