A Coarse-to-Fine Generalized-ICP Algorithm With Trimmed Strategy

In this paper, we introduce a modified Generalized Iterative Closest Point (GICP) algorithm by presenting a coarse-to-fine strategy. Our contributions can be summarized as: Firstly, we use adaptively a plane-to-plane probabilistic matching model by gradually reducing the neighborhood range for given two point sets. It is an inner coarse-to-fine iteration process. Secondly, we use an outer coarse-to-fine strategy to bridge the point-to-point and plane-to-plane registration for refining the matching. Thirdly, we use the trimmed method to gradually eliminate the effects of incorrect correspondences, which improves the robustness of the methods especially for the low overlap cases. Moreover, we also extend our method to the scale registration case. Finally, we conduct extensive experiments to demonstrate that our method is more reliable and robust in various situations, including missing points, noise and different scale factors. Experimental results show that our approach outperforms several state-of-the-art registration methods.


I. INTRODUCTION
In recent years, with the rapid development of artificial intelligence and autonomous driving, the point set registration is becoming more and more popular. The point set registration problem aims to find the correspondence and transformation between two point sets, and transform one point set to its counterpart through accurate mapping [1]- [5].
Iterative Closest Point (ICP) [6], [7] is a classical point set registration algorithm via alternate iteration to search the correspondence and update the transformation. ICP algorithms of point-to-point and point-to-plane versions are both widely used for their simplicity and effectiveness. In practical applications, however, ICP still has many limitations. The prominent problem is the uncertain correspondence and the failure often induced by sampling, occlusion, outliers, missing or noisy data.
To overcome this shortcoming, many methods have been developed. Zhang proposed an outlier rejection strategy for the correspondence via the distance threshold [8]. It can tick out some unmatched point pairs and outside overlap regions induced by obstructing, missing and outliers. Meanwhile, it is hard to solve the uncertain corresponding problem caused by discretized sampling via the point-to-point based method.
The associate editor coordinating the review of this manuscript and approving it for publication was Guitao Cao .
So some point-to-plane and plane-to-plane based methods were introduced [1], [7], [9]. Among these, Generalized-ICP (GICP) [1] proposed a framework by minimizing the planeto-plane distance. In addition, the point-to-point based and point-to-plane ICP algorithm can also be viewed as a special case of the GICP framework. Intuitively, they assume that the data are locally planar; thus the searching regions to the closest correspondences are wider than that of the standard pointto-point ICP. So, plane-to-plane ICP obviously improves the robustness against measurement noise.
Then, many modified plane-to-plane ICP methods have been widely used for point cloud data alignment. Visual features and descriptors are introduced into the plane-to-plane error metric [10], [11]. Han et al. proposed a hierarchical searching scheme for multi-resolution data to improve the robustness with respect to the local minimum [12], [13].
However, GICP took the trade-off between the accuracy and the robustness for measurement noise. We observe that the final plane-to-plane distance cannot reach as low as the point-to-point ICP. Secondly, GICP is also sensitive to the initial position and thus only achieves local minimum. Thirdly, noise, occlusion and missing points will often make GICP fail to align.
Compared with GICP [1], the principal component analysis (PCA) pays more attention to global distribution. PCA uses the Singular Value Decomposition (SVD) method [14] FIGURE 1. Flowchart of the proposed coarse-to-fine framework. This is an inner and outer combined coarse-to-fine algorithm to balance the tradeoff between the accuracy and the robustness to noise.
to obtain several principal axes of two point sets, then aligns the centers and their principal axes [15]. This process can give a better initial rigid transformation, even scale mapping. It can be viewed as the roughest registration strategy.
To solve the problem of the local minimum caused by missing and noise degeneration, Trimmed ICP (TrICP) [16] was proposed, which ticks out outliers and then conducts ICP by minimizing the Trimmed Squared Distance (TSD). On this base, improved TrICP algorithms appeared by introducing an objective function to estimate the overlap rate [17], [18]. Then, Peng et al. [19], Dong et al. [20] extended this trimmed strategy to the nonrigid transformation. Du et al. [21], [22] proposed an ICP algorithm based on the probability or correntropy for precise registration with outliers and noise. Empirically, these methods can significantly improve the performance in a variety of noise degenerations. However, these methods only concern the point-to-point registration problem.
In this paper, we use a coarse-to-fine algorithm combining the inner and outer method to balance the trade-off between the accuracy and the robustness to noise. The inner coarseto-fine GICP algorithm starts with a wide range of planeto-plane matching, and the range decreases gradually during each iteration, which is less sensitive to the initial position and more robust to measurement noise, while the outer coarse-to-fine strategy bridges the point-to-point and planeto-plane registration for refining the matching, which can further improve the accuracy. Moreover, we also propose an adaptive pruning to reject incorrect correspondence in this process, which can avoid the local minimum at the low overlap case caused by missing points and outliers. Finally, we also consider extending the GICP from the rigid to the scale transformation. The basic framework is illustrated in Fig. 1.
This paper is organized as follows. In Section II we introduce the related work, which includes GICP algorithm and the trimmed strategy. Then, we describe our methodology and algorithm in Section III. In Section IV, we introduce the scale stretch version of our method. Experiments and analysis are shown in Section V. Section VI concludes the paper.

II. RELATED WORKS
Given two point sets, the source set X = {x i |i = 1, . . . , m} and the target set Y = {y j |j = 1, . . . , l}, our aim is to find the best transformation T matching X to Y .

A. GICP ALGORITHM
The GICP algorithm proposed a plane-to-plane registration method based on the Mahalanobis distance, i.e., where Here d i is the corresponding Euclidean distance vector between T ·x i and y c(i) , c(i) represents the index of the nearest point in Y corresponding to x i , C X n,i and C Y n,c(i) are covariance matrices calculated by n closest neighborhood points around x i ∈ X and n closest neighborhood points around y c(i) ∈ Y , and R is the rotation.
To model the plane structure, [1] modified the covariance matrix as: where U x i and U y c(i) are obtained by the SVD of the original covariance matrix. Here we assume that the singular values are in descending order, the smallest singular value is replaced by a small constant , and the remaining two singular values are replaced by 1. When C X n,i = 0 and C Y n,c(i) = I, it is equivalent to the standard ICP.

B. TRIMMED STRATEGY
The trimmed strategy updates adaptively the overlap rate r to trim out unmatched points by optimizing the following objective function [17]- [20]: i is the Trimmed square distance (TSD), and λ is a parameter that decreases with iterations.

III. THE PROPOSED COARSE-TO-FINE ITERATIVE MATCHING ALGORITHM
PCA can roughly match point clouds and accelerate the convergence rate, but it may also result in worse initial positions. VOLUME 8, 2020 Compared with ICP, GICP considers a plane-to-plane registration model that covers more local information and is less sensitive to the noise. But the proper neighborhood range is hard to determine. Furthermore, we need to trim out the incorrect correspondences for the low overlap data or noising data. By synthesizing the advantages of these methods, we propose a coarse to fine iterative closest point algorithm to deal with these problems. The details of our algorithm are given as follows.
Here we list some notations used in our algorithm. Let k −1 be the last iteration, T k the current transformation consisting of a rotation matrix, a translation vector {R k , t k }, and n k the updated neighborhood range for the k-th iteration.

A. PREPROCESSING
In this part, we aim to obtain an initial transformation and set some initial parameters. In our algorithm, PCA is used to get the initial transformation T 0 and align two point sets roughly. However, it may cause the mirror symmetry or distribution center difference. In order to avoid the mirror symmetry problem as displayed in Fig. 2(a), we use axis reversal to detect and process as displayed in Fig. 2(b), where the target points are in blue and the source points are in red.

B. ESTIMATING CORRESPONDENCES
Having the transformation T k−1 fixed, for each point x i ∈ X , find its correspondence point y c(i) ∈ Y :

C. CALCULATING THE OVERLAP RATE
and sort them in the ascending order.
Then we calculate the overlap rate according as: where λ k = λ k−1 − ξ is a parameter of the trimmed strategy, and the positive constants λ 1 and ξ are defined in advance.

D. UPDATING TRANSFORMATION
In this part, we update the transformation by minimizing the corresponding plane-to-plane distance. We use a large neighborhood range n 1 = n max as the initial range parameter.
In each iteration, the plane scale changes with the number of nearest neighbors.
Here we use D 2 n k ,i to represent the plane-to-plane squared distance: where n k = n k−1 − δ is the updated neighborhood range, and the constant δ is the parameter defined in advance. C X n k ,i is the modified covariance matrix corresponding to the current n k nearest points of point x i ∈ X , C Y n k ,c(i) is the modified covariance matrix corresponding to the current n k nearest points of point y c(i) ∈ Y . The covariance matrices are computed by (2) and (3).
We use d 2 i to represent the point-to-point squared distance: Having the correspondences and overlap rate fixed, we can get the Trimmed Mean Square Error (TMSE) for plane-to-plane matching (10) or point-to-point matching (11): where m r is the number of remaining points after trimming, For the optimization of transformation, we solve a 6-dimensional vector including rotation angles and translations. If n k ≥ n min , we use the TMSE function (10) and solve the nonlinear optimization problem by the Broyden-Fletcher-Goldfarb-Shanno (BFGS) algorithm [23]- [26], else we solve the TMSE function (11) by SVD [14], since it degenerates into a point-to-point situation when the scale is sufficiently small. Then we update the transformation T * k :

E. SUMMARY OF ALGORITHM
To summarize, our algorithm is listed as Algorithm 1.
Note that at the k-th iteration, before updating transformation, the error is Algorithm 1 An Adaptive Generalized-ICP Algorithm Require: Source set X , target set Y and their multi-scale covariance matrices; parameters: N iter , n max , n min , δ, λ max , ξ ; Ensure: Final transformation T * ; 1: Initial n 1 = n max , λ 1 = λ max ; 2: Obtain initial transformation T 0 by PCA. 3: for iteration k = 1 : N iter do 4: (5); 6: Fix the correspondence, calculate the overlap rate r k by (6); 7: Fix the correspondence and overlap rate, update transformation T * k using D 2 n k ,i or d 2 i ; 8: if n k ≥ n min then 9: optimize the TMSE function (10) via BFGS algorithm; 10: else 11: solve the TMSE function (11) by SVD; 12: end if 13: if the stopping condition is satisfied then 14: break; 15: else 16: n k+1 = n k − δ; 17: λ k+1 = λ k − ξ ; 18: continue; 19: end if 20: end for where m r = m × r. Then, after updating transformation, the new error becomes In the algorithm, when any of the following conditions are satisfied, the iteration stops.
1) The maximum iteration number N iter has reached; 2) The mean error E k is sufficiently small; 3) |ε k − E k | is sufficiently small.

IV. ISOTROPIC SCALE REGISTRATION
In practice, the scale factor is ubiquitous in registration. For example, scanning data from different perspectives and distances may have different scales. In order to match point sets better, besides the rotation and translation, we need to solve a scale parameter. Inspired by [15], [27], [28], to deal with the scale registration, we extend our algorithm to the isotropic scale case.
For the isotropic scale registration problem, the planeto-plane distance function can be written as: where s is the isotropic scale factor and M can be computed by (8). Similarly, the point-to-point distance function can be written as: When solving this optimization problem, we can add the partial derivative of the scale factor accordingly. The solution to other variables is similar to that mentioned above.

V. EXPERIMENTAL RESULTS AND ANALYSIS
In order to verify the performance of our algorithm, we compare it with other algorithms, i.e., ICP, TrICP and GICP. In addition, the results are also compared with our method using PCA and not using PCA, which are called Adaptive GICP (AGICP) and Modified GICP (MGICP), respectively.

A. DATASETS AND EVALUATION
We use the data Turbine Blade, Dragon and Happy Buddha (Fig. 3) from the Large Geometric Models Archive. 1 Firstly, the original data are regarded as the source set X , and the target set Y is generated with various transformations, such as rotations from 10 degrees to 40 degrees. Then we add some outliers to the source set. After that, we add some random noise to the source set to generate data in noise condition, or drop some points from the target set to generate data in the missing data condition.
Here we suppose the estimated transformation is T , and the set X trans = T · X is obtained by transforming the original source set X . To evaluate our method, we calculate the Root Mean Squared Distance (RMSD) between X trans and the original target set Y .

B. POINT CLOUD REGISTRATION WITHOUT SCALE FACTOR
We perform experiments for Turbine Blade, Dragon and Happy Buddha in two cases: the noise case and the missing data case. In our experiments, we rotate the source data from 10 degrees to 40 degrees. For noise data, we add noise to 50% source points, and add 5% outliers. For missing data, we generate data by dropping 10% to 40% points from the target set and add 5% outliers.
Some visual results are shown in Fig. 4 to Fig. 10, which indicates the registration results for Turbine Blade, Dragon and Happy Buddha in the noise case and in the data          Figs. 4 and 6, we add noise to 40% source points, and add 5% outliers. All four algorithms perform well in the noise condition. However, when we enlarge the area marked by the black circle (Figs. 5 and 7), our methods are better than other three algorithms.
Figs. 8 and 10 illustrate the registration results for Turbine Blade and Dragon in the data missing case. For data in Fig. 8, we drop 30% points from the target set and add 5% outliers to source points. For data in Fig. 10, we drop 40% points from the target set and add 5% outliers to source points. Due to the low overlap rate, Turbine Blade data, GICP and TrICP all fail in matching. Contrary to these algorithms, our approach performs very well, which can be viewed in the enlarged figures (Figs. 9 and 11). Fig. 11 shows that our method with PCA is better than that of without PCA.
Statistical results are shown in Table 1 and Figs. 12 and 13. As shown in the table, our method almost gets the smallest RMSD errors of all the datasets no matter in the noise case or in the missing data case, which indicates that our algorithm performs consistently better than ICP, GICP, and TrICP. VOLUME 8, 2020   Table 1 shows that the results of our method with or without PCA nearly have no difference in the noise case, but the result with PCA will be better in the missing data case. The average number of iterations with PCA is about 16, and the average number of iterations is about 21 without PCA, which is lower than ICP, GICP and TrICP. It illustrates that our method converges faster.
In Figs. 12 and 13, we use boxplots to illustrate the RMSD error for all data in Table 1. The boxplot can be interpreted as: the x−axis represents four algorithms; y−axis represents their respective errors; on each box, the central red mark indicates the median, and the bottom and top edges of the box indicate the 25th and 75th percentiles, respectively; the whiskers extend to the most extreme data points not considered outliers, and the outliers are plotted individually using the red '+' symbol. Fig. 12(a) to Fig. 12(c) are the results for data in various noise situations. When the noise is small, due to the outliers, ICP gets a bad result, GICP and TrICP get results with some errors, and our results with or without PCA get a small RMSD error. With the increment of noise, the error of ICP becomes higher, but our results are still small. It indicates that our method is robust to noise.
Figs. 13(a) and 13(b) are the results for data in various overlap situations. When the overlap rate is 80%, the errors of TrICP and our method are all small, compared with the higher error in ICP and more abnormal results in GICP. As the overlap rate decreases, the abnormal results of ICP, GICP and TrICP increase, while our method obtains more stable results, which illustrates that our method is robust to missing data.
The boxplot figures reveal that our methods, no matter with or without PCA, nearly get the similar results in the noise case; but in the missing data case, AGICP gets higher error results.
We also compare the convergence rates of these algorithms. We measure the convergence by the Root-Mean-TSD (RMTSD) between Y and X trans = T k · X in every iteration. The RMTSD error is defined as Figs. 14 and 15 display the RMTSD error and the overlap rates of data illustrated in Figs. 4, 6, 8 and 10. In Fig. 14, GICP converges fast, but easily falls into local minima, while ICP and TrICP converges slowly. Our method accelerates the convergence procedure by PCA, uses the multi-scale plane matching in the early stage to avoid falling into the local minima, and refines the results by point-to-point matching in the later stage.    our method with or without PCA all get good overlap rate results. Fig. 15(c) shows that there is a little deviation in the overlap rate of TrICP; as seen from Fig. 14, the RMTSD error of TrICP achieves significantly small at last, but according to the registration result, the final matching result is biased, because TrICP gets a wrong overlap rate and retains the incorrect matching pairs only according to the distance from point to point. Our method can achieve a better result by plane matching, which reduces the risk of mismatching.

C. POINT CLOUD REGISTRATION WITH ISOTROPIC SCALE FACTOR
In this part, we conduct experiments for data at various scales, from 0.9 to 0.6. At the same time, we also generate datasets with noise and missing data.
We add the isotropic scale factor into the original ICP, TrICP and GICP to generate SICP, TrSICP and GSICP, and solve the registration problems by using SVD method for ICP and TrICP, using BFGS method for GICP.   [19], [20], MGICP and AGICP.
Figs. 16 and 17 illustrate the registration results for Dragon in the scale registration with missing data and Turbine Blade in scale registration with noise. For data in Fig. 16, we drop 30% points from the target set, and expand the source data so that target data is 0.9 times the source data. For data in Fig. 17, we add noise to 40% source points, add 5% outliers and expand source data so that target data is 0.6 times source data.
SICP and TrSICP all trap in the local minima. On the contrary, our method matches the two point sets with a correct scale, and performs very good, which can be further observed by enlarging the area marked by the black circle of our results in Figs. 18(a) and 18(b).
Statistical results are shown in Table 2 and Fig. 19. In the table, from the RMSD error result, ICP, GICP and TrICP cannot deal with scale registration well without considering the scale factor. Our method almost obtains the best result, no matter with or without PCA method. SICP and TrSICP need to iterate many steps to achieve the convergence, and the results are not satisfactory. On the contrary, our results show that the convergence can be achieved in a small number of iterations, and with the PCA method, we obtain more accurate results.   Fig. 19(a) to Fig. 19(d) are the results for data in various scale situations with noise or missing data. Original ICP, TrICP and GICP obviously fail to match because they do not take the scale into account. SICP and TrSICP may fail in the scale registration, and get higher error with the scale decreasing. GSICP obtains the results with a low average error but not robust, and our method gets stable results at different scale factors no matter with PCA method or not.
In Figs. 20 and 21 we display the RMTSD error and the overlap rate. For the scale registration, SICP and TrSICP get high RMTSD errors, TrSICP also has a wrong overlap rate, which almost takes all points into account. GSICP gets a very small RMTSD error as the scale parameter decreases, but it is a degenerate case. Because GSICP and our method initially calculate the plane-to-plane distance, and our method calculates the point-to-point distance in the later stage, which causes some fluctuations in our RMTSD results. After PCA preprocessing, our method has greatly improved the calculation of scale and the overlap rate, so the correct convergence results can be obtained quickly.
Our method gets an initial position after PCA, uses multi-scaled plane-to-plane matching and trimmed method to reject the influence of incorrect correspondences, and refines the result by point-to-point matching finally. For the experimental data, no matter what random noise, isotropic scale factor, or the overlap rate in various rotation, our algorithm  Fig. 16 and Fig. 17.  can achieve satisfactory results. Experimental results demonstrate that our method is robust to noise and missing data.
In addition, we add the computation time comparison results in Table 3. The experimental results show that the run time of AGICP is much less than that of point-to-point methods, and comparable to GICP.

VI. CONCLUSION
In this paper, we have proposed a coarse to fine iterative closest point algorithm by introducing a modified multi-scale GICP algorithm to refine the matching accuracy, especially for low overlap cases. We have adopted a multi-scale planeto-plane matching by using a gradually reduced neighborhoods range, and trimmed method to reject the influence of incorrect correspondences. The extensive experiments demonstrate that our algorithm is more accurate and robust in a variety of situations, including missing points and noise.