**4. Graph Optimization Framework**

#### *4.1. Local Pose Map Structure*

Authors should discuss the results and how they can be interpreted from the perspective of previous studies and of the working hypotheses. The findings and their implications should be discussed in the broadest context possible. Future research directions may also be highlighted.

The local state vectors in the local coordinate system in which the LiDAR and IMU are located are given as follows:

$$\begin{aligned} \mathcal{X}^L &= \left[ \mathbf{x}\_{b1}, \mathbf{x}\_{b2}, \dots, \mathbf{x}\_{bi}, d\_1^e, d\_2^e, \dots, d\_{k'}^e, d\_{1'}^p, d\_{2'}^p, \dots, d\_k^p \right] \\ \mathbf{x}\_{bi} &= \left[ p\_{bi'}^L q\_{bi'}^L v\_{bi'}^L, b\_{b'} b\_{\mathcal{S}} \right] \end{aligned} \tag{12}$$

where *xbi* denotes the state quantity after pre-integration of the *i*th IMU at *tk*, including position *p<sup>L</sup> bi*, rotation *<sup>q</sup><sup>L</sup> bi*, speed *<sup>v</sup><sup>L</sup> bi* and IMU bias *ba*, *bg*. *<sup>d</sup><sup>e</sup> <sup>k</sup>* is the distance from the LiDAR feature point at *tk*−<sup>1</sup> to the matching edge feature at *tk*, and *<sup>d</sup><sup>p</sup> <sup>k</sup>* is the distance from the feature point at *tk*−<sup>1</sup> to the matching planar feature at *tk*.

From this, the Gauss–Newton method can be used instead of the fastest gradient descent method used in [27] to minimize all cost functions so as to reduce the number of iterations for rapid convergence to a locally optimal estimate. The local optimization function is constructed as follows:

$$\min\_{\mathcal{X}} \left\{ \sum d\_k^e + \sum d\_k^p + \sum\_{k \in B} ||r\_B(\boldsymbol{z}\_{k+1'}^k, \boldsymbol{\mathcal{X}})||\_{\boldsymbol{\Sigma}\_b}^2 \right\} \tag{13}$$

where ∑ *d<sup>e</sup> <sup>k</sup>* <sup>+</sup> <sup>∑</sup> *<sup>d</sup><sup>p</sup> <sup>k</sup>* is used to solve the carrier pose *<sup>x</sup>LiDAR tk* in the local coordinate system of LiDAR at time *tk*. *rB z*ˆ *i*−1 *<sup>i</sup>* , X and Σ*<sup>b</sup>* are IMU measurement residuals and covariance matrices, respectively. The meanings of the terms are described below.

### 4.1.1. IMU Pre-integration Factor

Let *αi*+<sup>1</sup> *<sup>i</sup>* , *<sup>θ</sup>i*+<sup>1</sup> *<sup>i</sup>* , *<sup>β</sup>i*+<sup>1</sup> *i T* be the IMU pre-integration calculation value between the *i*th and *i* + 1th LiDAR key frames. Details of the derivation of the IMU pre-integration are presented in Appendix A. Δ*ti* is the time interval between the two LiDAR key frames, and the spatial transformation matrix from the IMU coordinate system to the LiDAR coordinate system in *i*th frame is represented by *Rbi <sup>L</sup>* . The IMU residual can be obtained as follows:

$$r\_B \left( \dot{z}\_i^{i+1}, \mathcal{X} \right) = \begin{bmatrix} \delta a\_i^{i+1} \\ \delta \theta\_i^{i+1} \\ \delta \theta\_i^{i+1} \\ \delta b\_i \\ \delta b\_a \\ \delta b\_{\mathcal{S}} \end{bmatrix} = \begin{bmatrix} \mathcal{R}\_L^{b\_i} \left( \boldsymbol{\mathcal{v}}\_{b\_{i+1}}^L - \boldsymbol{\mathcal{v}}\_{b\_i}^L \Delta t\_i - \boldsymbol{\mathcal{v}}\_{b\_i}^L + \frac{1}{2} \mathbf{g} \Delta t\_i^2 \right) - \dot{\mathcal{A}}\_i^{i+1} \\ 2 \left[ \boldsymbol{\mathcal{q}}\_{b\_i}^{L^{-1}} \otimes \boldsymbol{\mathcal{q}}\_{b\_{i+1}}^L \otimes \dot{\mathcal{B}}\_i^{i+1^{-1}} \right]\_{\mathbf{x} \mathbf{y} \mathbf{z}} \\ \mathcal{R}\_L^{b\_i} \left( \boldsymbol{\mathcal{v}}\_{i+1}^L - \boldsymbol{\mathcal{v}}\_i^L + \mathbf{g} \Delta t\_i \right) - \dot{\mathcal{B}}\_i^{i+1} \\ \boldsymbol{\mathcal{b}}\_{ai+1} - \boldsymbol{b}\_{ai} \\ \boldsymbol{b}\_{ai+1} - \boldsymbol{b}\_{ai} \end{bmatrix} \tag{14}$$

where the symbol [·]*xyz* represents extracting the real part of the quaternion used to calculate the rotation state error, and ⊗ represents the quaternion multiplication.

After the pose estimation of the previous key frame is completed, the IMU acceleration bias and gyroscope bigotry will be updated, the update amounts are set as *δ* - *b bi <sup>a</sup>* and *δ* - *b bi g* and the pre-integration calculation value at this time is updated as follows:

$$a\_{i}^{i+1} = \mathring{a}\_{i}^{i+1} + \frac{\delta \mathring{a}\_{i}^{i+1}}{\delta b\_{a}} \breve{\delta b\_{a}} + \frac{\delta \mathring{a}\_{i}^{i+1}}{\delta b\_{\omega}} \breve{\delta b\_{\omega}}$$

$$\theta\_{i}^{i+1} = \theta\_{i}^{i+1} \cdot \operatorname{Exp}(\frac{\delta \theta\_{i}^{i+1}}{\delta b\_{\omega}} \breve{\delta b\_{\omega}}) \tag{15}$$

$$\beta\_{i}^{i+1} = \beta\_{i}^{i+1} + \frac{\delta \mathring{\rho}\_{i}^{i+1}}{\delta b\_{x}} \breve{\delta b\_{a}} + \frac{\delta \mathring{\rho}\_{i}^{i+1}}{\delta b\_{\omega}} \breve{\delta b\_{\omega}}$$

#### 4.1.2. LiDAR Factor

The feature point cloud extracted by LiDAR can be divided into two types: line features and surface features. The LiDAR residuals of the two types need to be constructed separately and then summed to obtain the total LiDAR residuals. Details of the specific derivation of LiDAR residuals are presented in Appendix B. Figure 4 shows the schematic diagram of LiDAR residual construction.

**Figure 4.** Schematic diagram of LiDAR residual construction. (**a**) Line characteristic residual construction; (**b**) surface characteristic residual construction.

As shown in Figure 4, let a feature point obtained in the *k* + 1th scan have the coordinates of *X<sup>L</sup>* (*k*+1,*o*) in the LiDAR coordinate system, and the coordinates of two end points of the line features matched with it in the *k*th scan are *X<sup>L</sup>* (*k*,*a*) and *<sup>X</sup><sup>L</sup>* (*k*,*b*) . The residual error of the line features can be expressed by the point-to-line distance:

$$d\_{ck}^{L} = \frac{\left| \left( X\_{(k+1,\rho)}^{L} - X\_{(k,a)}^{L} \right) \times \left( X\_{(k+1,\rho)}^{L} - X\_{(k,b)}^{L} \right) \right|}{\left| X\_{(k,a)}^{L} - X\_{(k,b)}^{L} \right|} \tag{16}$$

Similarly, if the surface features that match it in the *k*th scan are represented as *X<sup>L</sup>* (*k*,*c*) , *X<sup>L</sup>* (*k*,*d*) and *<sup>X</sup><sup>L</sup>* (*k*, *f*) , then the surface feature residual can be represented by the point-to-surface distance:

$$d\_{pk}^{L} = \frac{\left| \left( \mathbf{X}\_{\left(k+1,\rho\right)}^{L} - \mathbf{X}\_{\left(k,d\right)}^{L} \right) \cdot \left( \left( \mathbf{X}\_{\left(k,c\right)}^{L} - \mathbf{X}\_{\left(k,d\right)}^{L} \right) \times \left( \mathbf{X}\_{\left(k,c\right)}^{L} - \mathbf{X}\_{\left(k,f\right)}^{L} \right) \right) \right|}{\left| \left( \mathbf{X}\_{\left(k,c\right)}^{L} - \mathbf{X}\_{\left(k,d\right)}^{L} \right) \times \left( \mathbf{X}\_{\left(k,c\right)}^{L} - \mathbf{X}\_{\left(k,f\right)}^{L} \right) \right|}\tag{17}$$
