## 3 Point Cloud Alignment Methods
### ICP Fine Alignment Based on Kd-Tree  
After the 4 PCS coarse alignment stage, the contour between the source point cloud attitude transformation and the target point cloud can be basically overlapped. But there still exists a certain contour error, which requires fine alignment to improve the alignment accuracy. ICP is used as a classical point cloud alignment algorithm: through the matching of points to points and iteratively calculating the optimal solution of the transformation matrix of the source point cloud and the target point cloud, the alignment error is minimized. Because the ICP algorithm does not need any feature computation to achieve fast iteration to minimize the function error to get the required spatial transformation matrix, it is widely used in point cloud fine alignment. This paper adds the kd-tree on the basis of ICP to further optimize the computation time of the algorithm. kd-tree is a kind of tree data structure with a number of points in a k-dimensional space. It is widely used in range search and nearest neighbor search, as shown in Figure 7, and its basic principle is as follows:  
In kd-tree, the median value of each dimension is often used as the segmentation hyperplane. The root of the tree is the first dimension, and all child nodes are divided downward by this dimension. The basis for the division is as follows: if this node is smaller than the root node, it will be divided into the left branch of the next dimension; if it is larger than the root node, it will be divided into the right branch of the next dimension. Then, the second dimension after the division is the root node, which is divided according to the above division principle and then divided downward until the end of the leaf nodes.  
Figure 7: kd-tree schematic. (**a**) Tree diagram; (**b**) space division diagram.  
The traditional point-to-point ICP algorithm uses the distance between the corresponding points of two point clouds as the objective function to calculate the transformation matrix. The point-to-face ICP algorithm adopted in this paper uses the distance from each point in the source point cloud to the plane where the normal vector of the corresponding point in the target point cloud is located as the alignment criterion, and then iterates to find the optimal transformation matrix to find the best solution. The main steps of the kd-tree based point-to-plane ICP algorithm are as follows:  
Step 1: In order to solve the normal features of each point in the two point cloud sets, establish a kd-tree for near-neighborhood accelerated search on the source point cloud set \(P\) and the target point cloud set \(Q\) obtained after coarse alignment. Then, compute the weighting factor and feature descriptor based on the normal features, so as to find the corresponding point of each nearest neighbor of the source point cloud set \(P\) and the target point cloud set \(Q\).  
Step 2: The optimal transformation matrix \(T\) is solved based on the found nearest neighbors with the objective function:  
\[F\Big{(}R_{f},T_{f}\Big{)}=\min\frac{\sum\limits_{i=1}^{N}\left\|\left(F\Big{(} R_{f},T_{f}\Big{)}p_{i}-q_{i}\Big{)}n_{i}\right\|^{2}}{N} \tag{7}\]  
where \(R_{f}\) is the rotation matrix; \(T_{f}\) is the translation matrix; \(p_{i}\) and \(q_{i}\) are the corresponding points of the source point cloud set \(P\) and the target point cloud set \(Q\); and \(n_{i}\) denotes the normal vector of the point \(p_{i}\) corresponding to \(q_{i}\).  
Step 3: Set the maximum number of iterations \(N_{max}\) and the minimum threshold \(\epsilon\). Repeat the above steps; if the error of the two iterations is less than \(\epsilon\), i.e., \(F_{n}(R_{f},T_{f})\)-\(F_{n-1}(R_{f},T_{f})<\epsilon\), or the number of iterations is greater than \(N_{max}\), then stop iteration and find the final transformation matrix \(T\). Otherwise, continue to iterate until it meets the iteration conditions.