http://orcid.org/0000-0002-3355-9391
Figures
Abstract
The iterative closest point (ICP) algorithm is efficient and accurate for rigid registration but it needs the good initial parameters. It is easily failed when the rotation angle between two point sets is large. To deal with this problem, a new objective function is proposed by introducing a rotation invariant feature based on the Euclidean distance between each point and a global reference point, where the global reference point is a rotation invariant. After that, this optimization problem is solved by a variant of ICP algorithm, which is an iterative method. Firstly, the accurate correspondence is established by using the weighted rotation invariant feature distance and position distance together. Secondly, the rigid transformation is solved by the singular value decomposition method. Thirdly, the weight is adjusted to control the relative contribution of the positions and features. Finally this new algorithm accomplishes the registration by a coarse-to-fine way whatever the initial rotation angle is, which is demonstrated to converge monotonically. The experimental results validate that the proposed algorithm is more accurate and robust compared with the original ICP algorithm.
Citation: Du S, Xu Y, Wan T, Hu H, Zhang S, Xu G, et al. (2017) Robust iterative closest point algorithm based on global reference point for rotation invariant registration. PLoS ONE 12(11): e0188039. https://doi.org/10.1371/journal.pone.0188039
Editor: Huiguang He, Chinese Academy of Sciences, CHINA
Received: September 20, 2017; Accepted: October 31, 2017; Published: November 27, 2017
Copyright: © 2017 Du et al. This is an open access article distributed under the terms of the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original author and source are credited.
Data Availability: All relevant data are within the paper and its Supporting Information files.
Funding: This work was supported by the National Natural Science Foundation of China (61573274 to SD, 61379104), the Fundamental Research Funds for the Central Universities (xjj2017005 to SD, xjj2017067), and the Program of Introducing Talents of Discipline to University (B13043). The funders had no role in study design, data collection and analysis, decision to publish, or preparation of the manuscript.
Competing interests: The authors have declared that no competing interests exist.
Introduction
Point set registration has become an important topic of computer vision and pattern recognition due to its wide applications such as 3D reconstruction [1,2], medical image analysis [3,4], and image retrieval and classification [5,6]. Of many point set registration methods in existence, the iterative closest point (ICP) algorithm [7–9] has received significant attention because of its simplicity and efficiency.
In recent years, a number of researchers have devoted to the improvement of ICP, especially the speed and the robustness. To speed up the computation, a combination of ICP variants was proposed, but it needed a good initial guess [10]. Fitzgibbon proposed the Levenberg-Marquardt algorithm to speed up ICP [11]. In addition, many scholars made efforts to enhance the robustness. Lee et al. introduced a reliability matrix to present the rotation components of ICP [12]. Granger and Pennec proposed a multi-scale schedule using simulated annealing (SA) algorithm to strengthen the robustness [13].
Meanwhile, point set registration with the outliers and noises has drawn the attention of many researchers. Chetverikov et al. presented the trimmed ICP (TrICP) for partial registration by incorporating an overlapping percentage into a least square function [14]. Ridene and Goulette proposed a method using an adaptive dynamic threshold along with the RANSAC to remove outliers [15]. Du et al. proposed the probability ICP, aiming at solving the registration of point sets with noises [16].
However, the initial values problem remained to be solved in the above mentioned approaches. The importance of initial values lies in that an inappropriate initial value could lead ICP algorithm to trap in a local minimum. For instance, when the rotation angle between the two point sets is large and the initial guess is not accurate, the algorithm cannot register point sets correctly. To cope with this problem, we introduce a global reference point. Based on this, we can obtain the relatively accurate initial parameters by making use of the invariance of the distance between each point of sets and the global reference point. And then the point set registration problem can be solved by the new ICP algorithm.
This paper is organized as follows. In section 2, a brief review of the original ICP algorithm is stated. In section 3, aiming at solving the initial parameters of ICP algorithm, a global reference point is introduced and a robust ICP algorithm for large-angle rotation point set registration is proposed. In section 4, experimental results on part B of CE-Shape-1 are shown. In the last section, a conclusion is given finally.
The ICP algorithm
Given two n-dimension point sets in ℝn, the shape point set and the model point set
. The objective of original ICP algorithm is to find a rigid transformation, with which the shape point set is in the best alignment with the model point set. For the registration of these two point sets, the formulation is based on the least squares (LS) criterion as follows:
(1)
where R ∈ ℝn×n is a rotation matrix, and
is a translation vector.
The standard ICP algorithm solves this problem by iteratively registering the shape point set to the model point set with rotation matrix R and translation vector . The two basic steps of ICP can be summarized as follows:
Firstly, according to the (k - 1)th rigid transformation , we set up correspondence between two point sets:
(2)
Secondly, compute the new rigid transformation between and
:
(3)
The ICP algorithm begins with an initial rigid transformation, and then it repeats the above two steps until convergence. However, it has been proved that this method is locally convergent, which means that the algorithm is easily failed when the rotation angle between two point sets is large. For this reason, a good initial transformation is so important that it guarantees that the algorithm converges to the global minimum finally.
Our algorithm
In this section, we built a new objective function based on a global reference point, which is rotation invariant. After that, a new ICP algorithm is proposed to deal with this problem, which is demonstrated to be convergent.
3.1 Problem statement
The traditional ICP algorithm can register two point sets rapidly and precisely under the good initial values, but in practice, the initial rigid transformation is difficult to obtain. In this case, if the initial rotation angle is large and we still set up correspondence by finding the closest point, the original ICP algorithm will converge to a local minimum and the registration cannot be accomplished.
In Fig 1, the rotation angle between two point sets is large and correspondence is set up by searching the red points which are the closest to the blue point set. Obviously, the original ICP algorithm is totally wrong for this case. Although the error goes down, merely the two point sets become closer. Finally the ICP algorithm cannot register these two point sets well.
(a) Correspondence is established by searching closest points. (b) Registration results of ICP.
According to the above statement, the ICP algorithm depends heavily on the initial rotation. Aiming at the problem like this, we need to find a rotation invariant measurement. With the aid of this invariant, correspondence is established correctly even the initial rigid transformation is wrong. Along with this idea, we introduce a global reference point, such as the mean point of the point set, and then compute the distance between each point of point sets and the global reference point, which is rotation invariant. In Fig 2, we take the mean point of the point sets as a global reference point. And the distance between each point of point sets and the global reference point will not change, no matter how the point set rotate or translate. By means of this trait, we can introduce this measurement into the LS problem to set up the objective function, which is given as follows.
Given two point sets in ℝn, the shape point set and the model point set
. To set up the correspondence, we introduce the global reference points for two point sets respectively, which are
of P and
of M. For each point
of the shape point set P, we can calculate the Euclidean distance
between
and
. In the same way, the Euclidean distance
between
and
is calculated. Because the original positions and the new distance are different measurements, we use w to be the weight to control the relative contribution of the positions and features. In this way, the goal of the registration is to establish the correspondence between these two sets via the rigid transformation, which can be expressed as the following LS problem:
(4)
The objective function is composed of two terms. The former term is the traditional Euclidean distance between two point sets and the latter one is to measure the distance of the rotation invariant term which will not be affected by rotation transformation.
3.2 The robust ICP algorithm based on global reference point
Using the framework of the standard ICP algorithm, we give a variant of the ICP algorithm to solve the problem (4). As we introduce a global reference point, and the distance between the each point of sets and the global reference point is the rotation invariant feature, accordingly the new algorithm can accomplish the rigid registration whatever the rotation angle is. Similar to the ICP algorithm, the detailed steps of the algorithm proposed are present as follows.
Given the global reference points for two point sets respectively, such as the mean point of the sets, of P and
of M. For each point of the point sets, calculate the Euclidean distance between themselves and the global reference point:
(5)
Moreover, we give the weight w, and then the algorithm can be accomplished by repeating the following two steps using the distance mentioned above.
Firstly, we set up the correspondence by the (k - 1)th rigid transformation :
(6)
In the proposed algorithm, the process of setting up the correspondence between two point sets is similar to the standard ICP algorithm. In Eq (6), the former term is the Euclidean distance between two point sets and we add the latter term to extend the original vector. For the original points and
, the vector could be extended to
and
. Therefore, Eq (6) is rewritten as the following function.
In the above equation, we search the closest point in (n+1)-dimension space to establish a more accurate correspondence between two point sets. This step can be solved by many methods such as k-d tree [17] and Delaunay tessellation [18,19]. In this paper, we choose the latter to search the nearest point.
Secondly, we calculate the new rigid transformation via the known correspondence of the kth step. In Eq (4), the former term is the same as the traditional ICP and the latter one is a rotation invariant, which won’t change through the rigid transformation. The latter term has no impact on the transformation, but it can be helpful for establishing the correspondence. Therefore, the new rigid transformation is calculated by the following term:
(8)
The method of calculating the rigid transformation is the same as the original ICP algorithm. This problem can be solved by singular value decomposition (SVD) [20]. The detailed procedures can be summarized as follows:
where the singular value decomposition of H is:
Therefore, the rotation matrix is computed as follows:
(9)
Moreover, the translation vector is calculated as the following equation:
(10)
This new algorithm repeats the above two steps until convergence. In the iterative procedures, the weight w is non-increasing, which can make the algorithm much more precise. To control the convergence of algorithm via the change of the weight w, we give a threshold T. When the root mean square (RMS) error between two point sets is smaller than T, we set the weight to be RMS divides by a preset number a, where we set this number to be 20 in this paper.
Hence, we outline the proposed algorithm as follows.
Input: Two point sets and
.
Initialize: Rotation matrix R0, translation vector , weight w0, threshold T and a small enough number ε.
Repeat
Step 1: Compute (7) to set up correspondence by the (k - 1)th transformation .
Step 2: Compute (8) to obtain the new transformation via Eqs (10) and (11).
Step 3:
- If RMSk < T, then wk = RMSk/a.
- Else wk = wk−1
Until |RMSk − RMSk−1| < ε or k reaches a maximum number of iterations.
Output: Rotation matrix Rk and translation vector .
3.3 Theory analysis
In this section, the convergence property of our algorithm is analyzed. In addition, as the importance of the weight w in the whole algorithm, the selection principle and the adjustment strategy of w are discussed.
3.3.1 Convergence theorem.
As the procedure of the proposed ICP algorithm is similar to that of the original ICP algorithm, we can demonstrate that it is a local convergent algorithm.
Theorem 1. The proposed algorithm converges monotonically to a local minimum with respect to square distance.
Proof. Given shape point set and Fmodel point set
. Let
, where Rk−1 is a rotation matrix, and
is a translation vector. The correspondence can be established by (6), and the Euclidean distance with weighted rotation invariant feature shall be denoted as:
where
,
.
In the (k−1)th step, the square distance is:
After the rotation Rk and of Eq (9) are computed, the error becomes:
As the transformation Rk and minimizes the error between two point sets, we obtain:
Because the invariant term does not change with rigid transformation,
we have
.
Next, the weight is updated to be wk. We set the square error as:
As the weight w is non-increasing, we get:
In the kth step, the square error is:
According to the closest point principle,
Hence, repeat procedures above, we obtain:
As our algorithm converges locally, a good initial transformation is of great importance. A rotation invariant term is introduced to deal with the problem of the initial values, which will be discussed in the next section.
3.3.2 The rotation invariant weight.
As the proposed algorithm is a local convergent algorithm, the rotation invariant term is introduced to tackle this problem. In our algorithm, the weight w is of great importance. When the weight of the rotation invariant term is larger, the objective function (4) is much more robust to the rotation. Otherwise, the objective function is much more focus on the registration precision. Specially, our algorithm will degenerate to the traditional ICP when w equals to zero.
In the initialization of our algorithm, a quite large number is given for the weight w. During iterations, the error between the transformed point set and the model point set is gradually decreasing, which means the shape point set is transformed to the correct direction guided by the rotation term. In Fig 3(B), it is obviously that a large weight can guide the shape point set to a relatively correct position. In this time, the error is small and it means that the rotation matrix is approaching to the ground-truth. On this occasion, the former term of the objective function (4), which describes the precision of registration, should be much accounted of rather than the latter one which is a rotation invariant. Therefore, we make w go down as the error reduces to accomplish the accurate registration. In this way, a coarse-to-fine method is used to register two point sets with large rotation angle. The final result of the registration is shown in Fig 3(C).
(a) Point sets before registration. (b) The intermediate result of our algorithm when the weight is large. (c) Final registration result of our algorithm when the weight is quite small.
Experimental results
To prove the robustness, accuracy and convergence of our algorithm, we conduct the experiments on simulation and standard data of MPEG-7 CE-Shape-1 dataset [21], where our algorithm is compared with the original ICP.
4.1 2D simulations
In this section, the 2D Bird, Cup and Deer data from S1 Data are chosen as shape data to conduct the experiments. To demonstrate that our algorithm can deal with the registration for large-angle rotation, we carry out six experiments of six different rotation angles. To obtain the model point sets, we rotate the shape point sets with angle 30°, 60°, 90°, 120°, 150° and 180° respectively and translate the data randomly.
To compare the proposed algorithm with ICP, we define the εR = ‖Rt − R‖2, . We transform the original data by Rt and
, and compute the R and
by the registration algorithm. In addition, the root mean square (RMS) is used to measure these two algorithms, and all the results are presented in Table 1.
From Table 1, it can be seen that our algorithm is more robust than ICP. Specifically, when the rotation angle is 30° or 60°, εR, and RMS of our algorithm are similar to ICP, which means that these two algorithms can complete the registration while the rotation angle between two point sets is small. Moreover, our algorithm is more accurate. However, when the rotation angle between two point sets is large such as 90°, 120°, 150° and 180°, the original ICP could not accomplish all the point sets registration, while our algorithm does well in all the rotation angles between two point sets, which proves the robustness of our algorithm.
To demonstrate the accuracy of our algorithm intuitively, the local amplification results of the point sets with 30° rotation angle are shown in Fig 4. Red points represent the model point sets, and blue points represent the shape point sets after rigid transformation. It can be clearly seen that the result of our algorithm is more accurate than the original ICP.
(a) Registration result of ICP. (b) Local amplification result of ICP. (c) Registration result of our algorithm. (d) Local amplification result of our algorithm.
As for initial rotation angle between two point sets is large, Fig 5 displays the comparison of the original ICP and the proposed algorithm. The distance between two point sets goes to smaller slightly by ICP, and the registration fails ultimately. However, our algorithm can amend the large-angle rotation between two point sets by using a rotation invariant. As the intermediate results show, our algorithm rotate the shape point sets from a relatively large angle to a small angle. On this basis, the registration completes finally.
(a) Point sets before registration. (b) Registration results of ICP. (c) The intermediate results of our algorithm when the weight of the rotation invariant is large. (d) Registration results of our algorithm.
Moreover, the comparison of the convergence of our algorithm and ICP with respect to different rotation degrees is shown in Fig 6. On the one hand, when the rotation angle between two point sets is small, the RMS of ICP approaches to zero gradually via multiple iterative steps. By contrast, the proposed algorithm needs a few iterative steps to complete the registration. On the other hand, when the rotation angle between two point sets is large, apparently ICP would trap in a local minimum value and fail to register the two point sets. However, our algorithm can deal with this problem well by means of the rotation invariance. Meanwhile, the RMS of our algorithm decreases rapidly after a few steps and is close to zero finally. In a word, our algorithm converges with fewer steps whatever the initial rotation angle between two point sets.
(a) Deer with 30° rotation. (b) Deer with 120° rotation.
In the simulation experiments, the accuracy, convergence and robustness of our algorithm have been proved. Whatever the rotation angle between two point sets is, the algorithm we proposed can complete the registration efficiently and precisely.
4.2 2D standard database.
In this part, we compared these two algorithm on the part B of CE-Shape-1 database. In the experiments, three couples of Bat, Hammer and Horseshoe from S1 Data are used for registration. Moreover, the RMS errors of these two algorithms are given in Table 2, while εR and cannot be computed as the ground-truth of the rigid transformation could not be obtained. The registration results of these two algorithms are shown in Fig 7.
(a) Point sets before registration. (b) Registration results of ICP. (c) The intermediate results of our algorithm when the weight is large. (d) Registration results of our algorithm.
It can be seen in Table 2 that RMS of our algorithm is smaller than ICP. In Fig 7, it is obvious that ICP fails to register the two point sets but our algorithm does well in registering the point sets with large rotation angle. Meanwhile, Fig 8 gives the convergence comparison for these two algorithms. Our algorithm converges with fewer steps than ICP and the RMS of our algorithm is close to zero finally.
In conclusion, the proposed algorithm for 2D standard database is robust and convergent. Moreover, fewer steps of iterations are required to complete the registration.
Conclusion
This paper proposes a variant of ICP algorithm based on global reference point to deal with the problem of registering two point sets with large rotation angle. Aiming at solving this problem, a global reference point is introduced and the distance between each point and global reference point is rotation invariant. Hence a more accurate correspondence is established, which provides a good initial for registration. Then the registration can be accomplished efficiently and accurately.
Compared to the previous works, our contributions include two main aspects: 1) To avoid the traditional ICP algorithm being trapped into the local minimum, a global reference point is introduced and the more accurate correspondence is set up by the rotation invariance. 2) Fewer steps of iterations are required to complete the registration. Therefore, the proposed algorithm is faster and some experimental results demonstrate that our algorithm performs better than ICP.
Even though the proposed method can solve the rotation invariant registration, there are still many problems to be dealt with. In the future, our algorithm can be extended to registration for 3D data and some applications, such as medical image registration.
References
- 1.
Ahmed M, Yamany S, Hemayed E, Ahmed S, Roberts S, Farag A. 3D Reconstruction of the Human Jaw from A Sequence of Images. Conference on Computer Vision and Pattern Recognition. 1997: 646.
- 2. Gregor J, Whitaker RT. Indoor Scene Reconstruction from Sets of Noisy Range Images. Graphical Models. 2001; 63(5): 304–332.
- 3. Ying S, Wu G, Wang Q, Shen D. Hierarchical unbiased graph shrinkage (HUGS): a novel groupwise registration for large data set. Neuroimage. 2014; 84: 626–638. pmid:24055505
- 4. Du S, Guo Y, Sanroma G, Ni D, Wu G, Shen D. Building dynamic population graph for accurate correspondence detection. Medical Image Analysis. 2015; 26(1): 256–267. pmid:26519794
- 5. Gao Y, Wang M, Tao D, Ji R, Dai Q. 3-D object retrieval and recognition with hypergraph analysis. IEEE Transactions on Image Processing. 2012; 21(9): 4290–4303. pmid:22614650
- 6. Zhao X, Wang N, Zhang Y, Du S, Gao Y, Sun J. Beyond Pairwise Matching: Person Reidentification via High-Order Relevance Learning. IEEE Transactions on Neural Networks and Learning Systems. 2017.
- 7.
Besl PJ, Mckay ND. A method for registration of 3-D shapes. Robotics—DL tentative. 1992: 239–256.
- 8. Zhang Z. Iterative point matching for registration of free-form curves and surfaces. International Journal of Computer Vision. 1994; 13(2): 119–152.
- 9. Chen Y, Medioni G. Object modeling by registration of multiple range images. Image and Vision Computing. 1992; 10(3): 145–155.
- 10.
Rusinkiewicz S, Levoy M. Efficient Variants of the ICP Algorithm. Third International Conference on 3-D Digital Imaging and Modeling. 2001: 145–152.
- 11. Fitzgibbon AW. Robust registration of 2D and 3D point sets. Image and Vision Computing. 2003; 21(13): 1145–1153.
- 12. Lee BU, Kim CM, Park RH. An Orientation Reliability Matrix for the Iterative Closest Point Algorithm. IEEE Transactions on Pattern Analysis & Machine Intelligence. 2002; 22(10): 1205–1208.
- 13.
Granger S, Pennec X. Multi-scale EM-ICP: A Fast and Robust Approach for Surface Registration. European Conference on Computer Vision. 2002: 418–432.
- 14. Chetverikov D, Stepanov D, Krsek P. Robust Euclidean alignment of 3D point sets: the trimmed iterative closest point algorithm. Image and Vision Computing. 2015; 23(3): 299–309.
- 15.
Ridene T, Goulette F. Registration of fixed-and-mobile- based terrestrial Laser data sets with DSM. IEEE International Symposium on Computational Intelligence in Robotics and Automation. 2010: 375–380.
- 16. Du S, Liu J, Zhang C, Zhu J, Li K. Probability iterative closest point algorithm for m-D point set registration with noise. Neurocomputing. 2015; 157(CAC): 187–198.
- 17.
Bentley JL. K-d trees for semidynamic point sets. Symposium on Computational Geometry. 1990: 187–197.
- 18. Chen HM, Lin TH. An algorithm to build convex hulls for 3-D objects. Journal of the Chinese Institute of Engineers. 2006; 29(6): 945–952.
- 19. Barber CB, Dobkin DP, Huhdanpaa H. The quickhull algorithm for convex hulls. ACM Transactions on Mathematical Software. 1996; 22(4): 469–483.
- 20. Arun KS, Huang TS, Blostein SD. Least-Squares Fitting of Two 3-D Point Sets. IEEE Transactions on Pattern Analysis and Machine Intelligence. 1987; 9(5): 698. pmid:21869429
- 21. Latecki LJ, Lakämper R, Eckhardt U. Shape Descriptors for Non-Rigid Shapes with a Single Closed Contour. IEEE Conference on Computer Vision and Pattern Recognition. 2000: 424–429.