An ICP algorithm based on block path closest point search

At present, the traditional ICP algorithm has the problems of low efficiency and low precision. To solve these two problems, an ICP algorithm based on block path closest point search is proposed in this paper. The idea of the algorithm is as follows: firstly, the point cloud data is divided into blocks, and the nearest point block corresponding to the target point cloud is searched by the path method. Secondly, according to the global method, the nearest point can be determined only by finding the nearest point block, and complete all the closest match. The experimental results show that the improved ICP algorithm has faster speed and higher precision than the traditional ICP algorithm, for a large number of point cloud data advantage is more obvious.


Introduction
With the development of computer aided design technology, 3D reconstruction of real object has become the focus of computer vision and graphics and other related fields. Three dimensional reconstruction technology has a very wide range of applications in all fields of real life, including industrial automation, archeology, cultural heritage and computer-aided medicine.
At present, with the maturity of 3D scanning terminal, it is not a big problem to obtain accurate 3D model coordinates, but the acquisition of point cloud data on the surface of objects often needs to be completed by repeated measurements. In order to obtain the point cloud data of the whole surface of the object, it is necessary to integrate and match the local point cloud data. Data registration is one of the most important data processing objects in 3D object modelling. The precision of registration directly affects the accuracy of model building.
The iterative closest point algorithm keeps the point cloud closest to each other by iteration, at which point the cloud space is consistent [1][2].The traditional ICP algorithm is inefficient, so many researchers at home and abroad have made efforts to improve the ICP algorithm. In recent search, we propose to use point tangent planes to approximate point clouds, and finally to solve the least square distance from point to tangent plane, but this method is still relatively slow [3].There are two ways to improve the speed, which are reverse scaling and random search, but have some influence on the registration accuracy.
This paper focuses on the problem of long running time of the algorithm, the ICP algorithm was optimized ,through data block processing, reduce the number of calculations, improving the efficiency and accuracy of point cloud registration, it is of great significance for practical measurement and application.

The Principle of ICP Algorithm
ICP algorithm was first proposed by Besl, Mckay [4] and Chen in 1992. It is a set of point set (PSTPS) based on four variables. The ICP algorithm is mainly used for 3D point cloud registration, the main process is: given two three-dimensional cloud data sets from different coordinate systems, we can find the spatial transformation of the two point clouds so that they can make spatial matching. Assuming {Pi, i=1,2,... } represents the first point cloud in the space, and the alignment, matching, transformation of the second point cloud Qi makes the objective function minimal [5].
The essence of the ICP algorithm is the optimal least squares matching algorithm, it repeats the process to determine the relation between point clouds computing optimal rigid transformation, until a convergence criterion is satisfied. The purpose of the ICP algorithm is to find the rotation R and the translation T transform matrix between the target point cloud and the reference point cloud, so that the two cloud data can be optimally matched under some metric criteria. Specific steps of the ICP algorithm [6]: k represents the number of iterations.
1) Pick up clouds in the target cloud P, k i P P ∈ ; 2) Calculate the corresponding points in the reference point cloud, making 3) Singular value decomposition, we obtain the rotation matrix R K and the translation matrix T K ,  [6].
The traditional ICP algorithm usually selects all points in the point cloud as the reference point, finding the nearest corresponding point.However, in actual measurement, the generated cloud data usually has hundreds of thousands or even millions of points. At this point, if point cloud registration using standard ICP algorithm, although it can basically meet the requirements of registration accuracy, it takes too long and the actual efficiency is not high.

ICP Algorithm Based on Block Path Closest Point Search
The main idea of ICP algorithm based on block path nearest point search is as following: firstly, the reference points data is partitioned into blocks, then find the nearest closest block according to the distance between the block and the registration point cloud, finally,calculating the distance between the registration point and all points in the nearest closest block,the point in block with minimum distance is the corresponding point.Similarly, all the corresponding points will be found. The algorithm can reduce the running time and improve the efficiency but didn't change the amount of data. Assuming that H represents the reference point cloud, P represents the point cloud to be matched.
The concrete steps of algorithm are as follows: 1) Get the reference point cloud H and the registration point cloud P; 2) The reference point cloud is partitioned into blocks; 3) Find the nearest block to the registration point; 4) Search the nearest point in the nearest block finding in the step 3 and get the closest point cloud B; 5) By the Singular value decomposition of matrix P and B, we obtain the rotation matrix R K

Implementation of ICP Algorithm Based on Block Path Closest Point Search
According to the main idea of the ICP algorithm based on block path closest point search, the flow chart of its algorithm implementation is shown in Figure 1.  The steps can be summarized as follows: 1) The reference point cloud H is projected on the XY plane and the coordinate values x and y of each point can be obtained. Then we create the height matrix which store the Z value of the reference point,the coordinate of height matrix is the value of x and y obtained by project.

Reference point cloud
2) The matrix Height is processed and divided into blocks of 4*4, 8*8, or 16*16 sizes; 3) Calculate the average of each block,which is the data stored by the new matrix ZMean after segmentation. 4) According to the path shown in figure 2 to find the nearest point block: a) First, the initial block is determined according to the coordinates of the target points,making it a central point block; b) Second, calculate the distance between the target point and the four point blocks adjacent to the central point block based on the path,finding the block which distance is the smallest as the new center point block. c) Repeat the second step, until the nearest point block is found, which is the center point block itself; 9) Determine whether the distance d is less than 0.1mm, less than is convergence, or jump to step 4); 10) If convergent,we can obtain the registration point cloud after coordinate transformation.

Experimental Results and Analysis
The experiment was conducted on Intel (R), Core (TM), Experiments were performed using points of the same amount of data,with the same time,the accuracy of the traditional ICP algorithm is lower than that of ICP algorithm based on block path closest point search,the registration was incomplete,but the algorithm based on block path closest point search has good matching effect, such as figure 3 and figure 4, we can see that the two point clouds has a little distance in figure 3.To the contrary, the two clouds have been coincide in the figure 4.  Comparing and analyzing the experimental data, we can see that: 1) Doing experiments with the same amount of data points when the registration accuracy is the same, although the number of iterations of the ICP algorithm proposed in this paper is more than that of the traditional ICP algorithm, its running time is much lower than the traditional ICP algorithm, and the efficiency is very high, as shown in table 2; 2) When the amount of point cloud data is larger, the advantage of ICP the algorithm based on block path closest point search is more obvious in registration efficiency and accuracy. It is important for practical measurement and application, as shown in table 2.

Concluding Remarks
In this paper, an ICP algorithm based on block path nearest point lookup is proposed, the speed and accuracy of registration are optimized by looking for the appropriate block size. The experimental results show that it is more accurate and effective to use this algorithm for point cloud data registration when the block size is appropriate, especially when the amount of data is large. The algorithm has higher registration accuracy and convergence speed, and has high practical value in the actual three-dimensional reconstruction.