*3.1. Voxelized Downsampling Based on Curvature Segmentation*

This paper presents a voxelized downsampling method based on curvature segmentation. Given a set of raw point cloud sequences collected by LiDAR, all points in the raw point cloud sequences are traversed and coarse clustering is performed using a breadth-first algorithm. Furthermore, the geometric angle threshold based on Euclidean distance is used to finely segment the point cloud clusters with similar depth. Let the scanning center of LiDAR be *O* and the two adjacent edge points *pa* and *pb* in the point cloud cluster with depths *da* and *db*, respectively (*da* > *db*). Let the number of point clouds in the point cloud cluster where point *pi* is located be *M*. Then, the roughness of point cloud *pi* is:

$$c = \frac{1}{|\mathcal{M}| \cdot ||d\_i||} \left| \left| \sum\_{j \neq i} (d\_i - d\_j) \right| \right|, i, j \in \mathcal{M} \tag{1}$$

Set the roughness threshold as *c*, then traverse *M*. We classify the points of *c* < *c* as the set of edge feature points, classify the points of *c* > *c* as the set of plane feature points and perform downsampling operations on them, respectively.

This method is mainly used in the feature extraction step of LiDAR odometry [4]; we extend it to the downsampling step. This means that, for any application where downsampling of point clouds is required, such as artefact inspection, the method can better restore the spatial distribution properties of point clouds by downsampling in clusters.

Next, this paper proposes a point cloud downsampling strategy based on HashMap, instead of the random sample consensus (RANSAC), so that the downsampling result of the point cloud is closer to the approximate center of gravity of voxels. Let the coordinate of a feature point in a set of point cloud sequences in the voxel space be *p*(*x*, *y*, *z*). If the voxel grid size is *r*, the dimension of the voxel grid in the *x* direction is *Dx* = (*x*max − *x*min)/*r*, and the index of *p* in the *x* direction within the voxel grid is *hx* = (*x* − *x*min)/*r*. The same applies to the *y* and *z* directions.

After obtaining the 3D index of feature points in the voxel space, if the random sorting strategy of [11] is adopted, the sorting complexity will be *O*((*m* + *n*) ∗ log(*m* + *n*)), which has a negative impact on the down-sampling time. Therefore, this paper uses the hash function to sort the index of feature points quickly and map them to *N* containers (*N* = 80). The hash function is:

$$\text{hash}(h\_{\mathbf{x}}, h\_{\mathbf{y}}, h\_{\mathbf{z}}) = (h\_{\mathbf{x}} + h\_{\mathbf{y}} \cdot D\_{\mathbf{x}} + h\_{\mathbf{z}} \cdot D\_{\mathbf{x}} \cdot D\_{\mathbf{y}}) \text{ \textquotedbl{}}\mathbb{N} \text{ } \mathbb{R}^3 \to \mathbb{R} \tag{2}$$

To avoid hash conflicts, set the conflict detection conditions as follows:

$$\text{hash}(h\_{\mathbf{x}\_{\prime}}h\_{\mathbf{y}\_{\prime}}h\_{\mathbf{z}}) = \text{hash}(h\_{\mathbf{x}\_{\prime}}'h\_{\mathbf{y}\_{\prime}}'h\_{\mathbf{z}}') \text{ (}h\_{\mathbf{x}} \neq h\_{\mathbf{x}}' \Big| h\_{\mathbf{y}} \neq h\_{\mathbf{y}}' \Big| h\_{\mathbf{z}} \neq h\_{\mathbf{z}}' \Big) \tag{3}$$

Once the hash conflict is detected, the index value in the current container is output and the container is emptied, and the new index value is put into the container.

To sum up, the main improvement of this section lies in extending the curvature segmentation step originally used for feature extraction to the downsampling step, and using hash mapping instead of the random sampling method for point cloud sampling. For the LiDAR odometer, using the clustering line and surface features again after the feature extraction step can improve the accuracy of downsampling single-frame or discontinuous point clouds at a weak time cost, thus providing more accurate point cloud distribution results for the pose estimation step between consecutive frames. In addition, using HashMap to downsample can further improve the sampling efficiency, and the time consumption of quadratic curvature segmentation is almost negligible. For other applications that need to downsample point clouds, the point cloud clustering method based on curvature segmentation can restore the spatial distribution of point clouds more accurately, and the benefits of this method are extensive and obvious.

The results and time consumption of the improved point cloud downsampling process are shown in Figure 2 and Table 1. Cloud number *M* = 112624, the line feature extraction threshold is 1, the surface feature extraction threshold is 0.1 and *r* = 0.3. It can be seen from Figure 2c that the present method has a clearer reduction in the spatial distribution of diagonal lines within a rectangular point cloud. Therefore, it can be proved that our method can retain the texture feature information of the source point cloud to a greater extent, and the accuracy and real-time performance of the downsampling results can be improved.

**Figure 2.** Comparison of point cloud downsampling results. (**a**) Source point cloud; (**b**) downsampling results before improvement; (**c**) downsampling results after improvement.

**Table 1.** Comparison of the number of point clouds and time consumption after downsampling.


#### *3.2. Voxelized Point Cloud Registration*

The purpose of point cloud registration is to update the rigid frame transformation of a moving carrier by comparing two consecutive frames of point clouds or similar point clouds detected by a loopback to solve for the carrier's pose. Traditional LIO usually uses ICP to realize the precise registration of point clouds. The ICP can be briefly described as follows: given a set of source point cloud *A* = {*a*1, *a*2,..., *an*} and target point cloud *B* = {*b*1, *b*2,..., *bn*}, the nearest neighbor search of KDTree is used to obtain the inter-frame pose transformation relationship *bi* = *Tai*, and the optimal solution is achieved through multiple iterations. However, an unreasonable initial position selection will make ICP fall into the misunderstanding of the local optimal solution, and the calculation resource consumption of the single-point nearest neighbor search is large. In view of the defects of the ICP algorithm, this paper utilizes a method based on the distribution of feature points in voxels, as shown in Figure 3.

**Figure 3.** Comparison of point cloud registration strategies. (**a**) ICP/GICP; (**b**) NDT; (**c**) our algorithm.

As shown in Figure 3. the problem of constructing the nearest neighbor model of a point pair by using a tree diagram is transformed into constructing the nearest neighbor model of a point and a neighborhood point set. Firstly, the two sets of point cloud sequences are approximated as Gaussian distributions, i.e., *ai* ∼ *<sup>N</sup>*(*a*ˆ*i*, <sup>Σ</sup>*<sup>A</sup> <sup>i</sup>* ) and *bi* <sup>∼</sup> *<sup>N</sup>*(<sup>ˆ</sup> *bi*, Σ*<sup>B</sup> <sup>i</sup>* ), where *<sup>i</sup>* ∈ (1, *<sup>n</sup>*). <sup>Σ</sup>*<sup>A</sup> <sup>i</sup>* and <sup>Σ</sup>*<sup>B</sup> <sup>i</sup>* are covariance matrices of two sets of point cloud sequences, respectively. Let the distance between a pair of corresponding points between the target point cloud and the source point cloud be:

$$d\_i = b\_i - Ta\_i \tag{4}$$

Let the neighborhood point set of *ai* be *Bai* <sup>=</sup> *bj* # #*ai* − *bj* < *λ* , where *λ* is the neighborhood judgment threshold. Thus, the distance between the extended point and the neighborhood point set is:

$$\hat{d}\_{i} = \sum\_{j} \left(\hat{b}\_{j} - T\mathfrak{A}\_{i}\right) \tag{5}$$

As a result of *ai* ∼ *<sup>N</sup>*(*a*ˆ*i*, <sup>Σ</sup>*<sup>A</sup> <sup>i</sup>* ) and *bi* <sup>∼</sup> *<sup>N</sup>*(<sup>ˆ</sup> *bi*, Σ*<sup>B</sup> <sup>i</sup>* ), the rigid body transformation error *ei* is calculated as:

$$x\_i \sim \left(\sum\_j \left(\hat{b}\_j - T\mathfrak{A}\_i\right), \sum\_j \left(\Sigma\_j^B - T\Sigma\_i^A T^T\right)\right) \tag{6}$$

In this way, the smoothing of all neighboring point clouds in the neighborhood of *ai* is achieved. Let *μ* = *N*(∑*<sup>j</sup>* ˆ *bj* − *Ta*ˆ*<sup>i</sup>* and Σ = ∑*<sup>j</sup>* Σ*<sup>B</sup> <sup>j</sup>* − *<sup>T</sup>*Σ*<sup>A</sup> <sup>i</sup> <sup>T</sup><sup>T</sup>* ; because *ei* is a highdimensional Gaussian distribution, its probability density function expansion form is:

$$P(\boldsymbol{\varepsilon}\_{i}) = \frac{1}{\sqrt{(2\pi)^{N} \det(\boldsymbol{\Sigma})}} \exp\left\{-\frac{1}{2} (\boldsymbol{\varepsilon}\_{i} - \boldsymbol{\mu})^{T} \boldsymbol{\Sigma}^{-1} (\boldsymbol{\varepsilon}\_{i} - \boldsymbol{\mu})\right\} \tag{7}$$

The negative logarithmic form of Equation (7) is:

$$-\ln\left(P(e\_i)\right) = \frac{1}{2}\ln\left[\left(2\pi\right)^N \det(\Sigma)\right] + \frac{1}{2}\left(e\_i - \mu\right)^T \left(\Sigma\right)^{-1} \left(e\_i - \mu\right) \tag{8}$$

Solving the inter-frame pose transformation matrix *T* by maximum likelihood method:

$$T = \underset{T}{\text{argmax}} \underset{i}{\Pi} P(\mathbf{e}\_i) = \underset{T}{\text{argmin}} \sum\_{T} \mathbf{e}\_i^T (\Sigma\_j^B - T\Sigma\_i^A T^T) \mathbf{e}\_i^T \tag{9}$$

Furthermore, after introducing the number *Ni* of point clouds in the neighborhood *ai*, Equation (9) can be written as:

$$\begin{cases} \begin{aligned} T &= \operatorname\*{argmin}\_{\hat{i}} \sum\_{\hat{i}} (N\_{\hat{i}} \hat{\epsilon}\_{\hat{i}}^T \Sigma\_{\hat{i}}^{-1} \hat{\epsilon}\_{\hat{i}}) \\ \mathcal{E}\_{\hat{i}} &= \frac{\sum\_{\hat{i}} b\_{\hat{i}}}{N\_{\hat{i}}} - T a\_{\hat{i}} \\ \end{aligned} & \text{(10)} \\ \begin{aligned} \Sigma\_{\hat{i}} &= \frac{\sum\_{\hat{i}} \Sigma\_{\hat{i}}^B}{N\_{\hat{i}}} + T \Sigma\_{\hat{i}}^A T^T \end{aligned} \end{cases} \end{cases} \tag{10}$$

In addition to the smoothing of all neighboring point clouds in the neighborhood of *ai*, an iteration termination threshold *ε* was established to avoid falling into a blind region of local optima after multiple iterations as follows:

$$|RMSE\_{k+1} - RMSE\_k| > \varepsilon \tag{11}$$

where *RMSEk*<sup>+</sup><sup>1</sup> and *RMSEk* are the root mean square error of the previous *k* + 1 iterations and the previous *k* iterations, respectively. The iteration is completed when the absolute value of the change in the root mean square error |*RMSEk*<sup>+</sup><sup>1</sup> − *RMSEk*| ≤ *ε*, or the maximum number of iterations, is reached.
