1 August 2011 Robust iterative closest point algorithm for registration of point sets with outliers
Author Affiliations +
Optical Engineering, 50(8), 087001 (2011). doi:10.1117/1.3607960
The problem of registering point sets with outliers including noises and missing data is discussed in this paper. To solve this problem, a novel objective function is proposed by introducing an overlapping percentage for partial registration. Moreover, a novel robust iterative closest point (ICP) algorithm is proposed which can compute rigid transformation, correspondence, and overlapping percentage automatically at each iterative step. This new algorithm uses as many point pairs as possible to yield a more reliable and accurate registration result between two m-D point sets with outliers. Experimental results demonstrate that our algorithm is more robust than the traditional ICP and the state-of-the-art algorithms.
Du, Zhu, Zheng, Liu, and Li: Robust iterative closest point algorithm for registration of point sets with outliers



Point set registration is important and complex in image processing. One robust and efficient approach to handle this problem is the iterative closest point (ICP) algorithm,1 but it cannot effectively register point sets with outliers including noise and missing data, which exist widely in applications.2 To overcome this problem, some scholars used thresholds or probability of distances to discard outliers,3 but these methods need an appropriate threshold which depends on the structure of point sets and thus is hard to choose. Meanwhile, some researchers attempted to reject outliers by adopting a coarse to fine process,4 but the methods often perform poorly in the case of a large amount of outliers in point sets.

Different from the above-mentioned work, Chetverikov 5 proposed the trimmed ICP (TrICP) algorithm for partially overlapping registration, which incorporates an overlapping percentage into a least square function to trim outliers. This algorithm, however, is time-consuming in that it needs to be repeated by traveling all possible overlapping percentage to search the best result even though Golden Section Search is used to accelerate the search speed. Based on this work, Phillips 6 presented the fractional ICP (FICP) algorithm for much faster speed, which computes the best correspondence and the overlapping percentage simultaneously, whereas the method depends greatly on a parameter which may lead to a false registration. To cope with the problem, this paper proposes a new objective function. Moreover, a novel robust and efficient ICP algorithm is presented to compute optimal overlapping percentage automatically and to complete the registration successfully.


Problem Statement

Given two point sets in [TeX:] \documentclass[12pt]{minimal}\begin{document}${\mathbb {R}^m}$\end{document} Rm , a model shape [TeX:] \documentclass[12pt]{minimal}\begin{document}$M \buildrel\Delta \over = \lbrace \vec{m}_i \rbrace _{i = 1}^{N_m }$\end{document} MΔ={mi}i=1Nm and a data shape [TeX:] \documentclass[12pt]{minimal}\begin{document}$P \buildrel\Delta \over = \lbrace \vec{p}_i \rbrace _{i = 1}^{N_p } \quad (N_m ,N_p \in {\mathbb N})$\end{document} PΔ={pi}i=1Np(Nm,NpN) . The rigid registration of m-D point sets is to build up correspondence and calculate rigid transformation (R, t) between P and M, hence


[TeX:] \documentclass[12pt]{minimal}\begin{document} \begin{equation} \begin{array}{*{20}c} \mathop {\min }\limits _{{\bf {R}},\vec{t},c(i)\in \lbrace 1,2,\cdots ,N_m \rbrace } & {\displaystyle\sum \limits _{i = 1}^{N_p } {\Vert {({\bf {R}}\vec{p}_i + \vec{t}) - \vec{m}_{c(i)} } \Vert _2^2 } } \\[17pt] {s.t.} & {{\bf {R}}^{\rm {T}} {\bf {R}} = {\bf {I}}_m ,{\rm { }}\det ({\bf {R}}) = 1} \end{array} \end{equation} \end{document} minR,t,c(i){1,2,,Nm}i=1Np(Rpi+t)mc(i)22s.t.RTR=Im,det(R)=1
where c(i) is the index of the i'th corresponding point in model shape M.

For partially overlapping point sets P and M, assume Pr is the overlapping part of P, which can match with M, and r denotes the overlapping percentage of P. The goal of partial registration is to build up the correspondence of Pr and M, and the distance is [TeX:] \documentclass[12pt]{minimal}\begin{document}$\sum _{\vec{p}_i \in P_r } {\vert \vert {\rm {\bf R}}\vec{p}_i + \vec{t} - \vec{m}_{c(i)} \vert \vert _2^2 }$\end{document} piPr||Rpi+tmc(i)||22 which increases with r. Hence, to include enough corresponding points as well as to exclude outliers, we divide the square distance by a penalty function g(r), an increasing function with respect to r. As too little overlapping part does not provide enough information, we assume the overlapping percentage r varies from 0.5 to 1. Therefore,


[TeX:] \documentclass[12pt]{minimal}\begin{document} \begin{equation} \begin{array}{*{20}{l}} \mathop {\min }\limits _{c,r,{P_r},R,\vec{t} } \hfill & {\displaystyle\frac{1}{{{e^\lambda }{r^\lambda }}}\displaystyle\sum \limits _{{{\vec{p}}_i} \in {P_r}} {||{\mathbf {R}}{{\vec{p}}_i} + \vec{t} - {{\vec{m}}_{c(i)}}||_2^2} } \\[15pt] \; \; \; s.t. & {{{\mathbf {R}}^T}{\mathbf {R}} = {\mathbf {I}}_m,{\text{ }}\det ({\mathbf {R}}) = 1} \hfill \\[5pt] {} & {\lambda \in [{\lambda _{\min }},{\lambda _{\max }}],{\text{ }}r \in [0.5,1]} \hfill ,\\[5pt] {} & {{P_r} \in P,{\text{ }}|{P_r}| = r|P|} \hfill \\ \end{array} \end{equation} \end{document} minc,r,Pr,R,t1eλrλpiPr||Rpi+tmc(i)||22s.t.RTR=Im,det(R)=1λ[λmin,λmax],r[0.5,1],PrP,|Pr|=r|P|
where e( · ) is the exponential function, λ is the control parameter, and | · | is the cardinality of a set, meaning the number of elements of the set.

In Eq. 2, it is easily proved that the objective function is a convex function with respect to r, which means the computation of r can be easily completed by iteration.


Point Set Registration with Outliers

ICP is good in minimizing Eq. 1 by repeating two steps: compute the closest point in model shape for each point in data shape and calculate the rigid transformation. As Eq. 2 is a convex function with respect to r, we compute r at each iterative step. Hence, similar to ICP, the objective function 2 can be optimized iteratively after the initialization:

Step 1. Set up the correspondence with the (k − 1)'th rigid transformation [TeX:] \documentclass[12pt]{minimal}\begin{document}$({\rm {\bf R}}_{k - 1} ,\vec{t}_{k - 1} )$\end{document} (Rk1,tk1) :


[TeX:] \documentclass[12pt]{minimal}\begin{document} \begin{equation} c_k (i) = \mathop {\arg \min }\limits _{j \in \lbrace 1,2, \ldots ,N_m \rbrace } \vert \vert {\rm {\bf R}}_{k - 1} \vec{p}_i + \vec{t}_{k - 1} - \vec{m}_j \vert \vert _2^2. \end{equation} \end{document} ck(i)=argminj{1,2,...,Nm}||Rk1pi+tk1mj||22.
To solve Eq. 3, many efficient methods can be used, such as the nearest point search algorithm based on Delaunay triangulations,7 whose computation complexity is O(Npln Nm).

Step 2. Compute the k'th overlapping percentage rk and the subset [TeX:] \documentclass[12pt]{minimal}\begin{document}$P_{r_k }$\end{document} Prk according to the known control parameter λ and the corresponding point sets [TeX:] \documentclass[12pt]{minimal}\begin{document}$\lbrace {\rm {\bf R}}_{k - 1} \vec{p}_i + \vec{t}_{k - 1} \rbrace _{i = 1}^{N_p }$\end{document} {Rk1pi+tk1}i=1Np and [TeX:] \documentclass[12pt]{minimal}\begin{document}$\lbrace \vec{m}_{c_k (i)} \rbrace _{i = 1}^{N_p }$\end{document} {mck(i)}i=1Np by minimizing the objective functions:


[TeX:] \documentclass[12pt]{minimal}\begin{document} \begin{equation} r_k = \mathop {\arg \min }\limits _{0.5 \le r \le 1} \sum \limits _{\vec{p}_i \in P_r } {\vert \vert {\rm {\bf R}}_{k - 1} \vec{p}_i + \vec{t}_{k - 1} - \vec{m}_{c_k (i)} \vert \vert _2^2 } / (e^\lambda r^\lambda ), \end{equation} \end{document} rk=argmin0.5r1piPr||Rk1pi+tk1mck(i)||22/(eλrλ),


[TeX:] \documentclass[12pt]{minimal}\begin{document} \begin{equation} P_{r_k} = \mathop {\arg \min }\limits _{P_r \subset P,\vert P_r \vert = r \vert P\vert } \sum \limits _{\vec{p}_i \in P_r } {\vert \vert {\rm {\bf R}}_{k - 1} \vec{p}_i + \vec{t}_{k - 1} - \vec{m}_{c_k (i)} \vert \vert _2^2 }. \end{equation} \end{document} Prk=argminPrP,|Pr|=r|P|piPr||Rk1pi+tk1mck(i)||22.
Sort the squared distances of [TeX:] \documentclass[12pt]{minimal}\begin{document}$\lbrace ({\rm {\bf R}}_{k - 1} \vec{p}_i + \vec{t}_{k - 1} ,\vec{m}_{c_k (i)} )\rbrace _{i = 1}^{N_p }$\end{document} {(Rk1pi+tk1,mck(i))}i=1Np in an incremental order. Let [TeX:] \documentclass[12pt]{minimal}\begin{document}$f(r) = \sum _{{{\vec{p}}_i} \in {P_r}} ||{{\bf {R}}_{k - 1}}{{\vec{p}}_i}\break + {{\vec{t}}_{k - 1}} - {{\vec{m}}_{{c_k}(i)}}||_2^2 /({e^\lambda }{r^\lambda })$\end{document} f(r)=piPr||Rk1pi+tk1mck(i)||22/(eλrλ) . Each time we add paired points to the subset Pr and compute the new corresponding f(r). Finally, we find the minimum f(rk) by traveling all f(r), and then select the first rkNp point pairs as the subset [TeX:] \documentclass[12pt]{minimal}\begin{document}$P_{r_k }$\end{document} Prk .

Step 3. Compute the rigid transformation by minimizing the following squared distance:


[TeX:] \documentclass[12pt]{minimal}\begin{document} \begin{eqnarray} ({{\bf {R}}^*},{\vec{t}^*}) {=}\!\! \mathop {\arg \min }\limits _{\scriptstyle {{\bf {R}}^{\rm {T}}}{\bf {R}} {=} {{\bf {I}}_m}, \scriptstyle \det ({\bf {R}}) {=} 1} \sum \limits _{{{\vec{p}}_i} \in {P_{{r_k}}}}\!\! {||{\bf {R}}({{\bf {R}}_{k {-} 1}}{{\vec{p}}_i} {+} {{\vec{t}}_{k - 1}}) {+} \vec{t} {-} {{\vec{m}}_{{c_k}(i)}}||_2^2},\!\!\!\!\! \nonumber\\ \end{eqnarray} \end{document} (R*,t*)=argminRTR=Im,det(R)=1piPrk||R(Rk1pi+tk1)+tmck(i)||22,


[TeX:] \documentclass[12pt]{minimal}\begin{document} \begin{equation} {\rm {\bf R}}_k = {\rm {\bf R}}^\ast {\rm {\bf R}}_{k - 1} ,\mbox{ }\vec{t}_k = {\rm {\bf R}}^\ast \vec{t}_{k - 1} + \vec{t}^\ast . \end{equation} \end{document} Rk=R*Rk1,tk=R*tk1+t*.
With the known correspondence, many closed-form methods can be used to compute the rigid transformation of Eq. 6 such as singular value decomposition (SVD).8

Step 4. Let [TeX:] \documentclass[12pt]{minimal}\begin{document}$\varepsilon _k = \sum _{\vec{p}_i \in P_{r_k } } {\vert \vert {\rm {\bf R}}_k \vec{p}_i + \vec{t}_k - \vec{m}_{c_k (i)} \vert \vert _2^2 } / (e^\lambda r_k^\lambda )$\end{document} ɛk=piPrk||Rkpi+tkmck(i)||22/(eλrkλ) . If |εk − εk − 1| is sufficiently small or k reaches a maximum number of iterations, the inside loop stops with ϕ(λ) = εk and λ is updated by λ = λ − λstep. Repeat steps 1 through 3 by decreasing the parameter λ from its maximal value λmax  to its minimal value λmin .

Finally, ϕ(λ) decreases at the initial stage when λ increases from λmin  to λmax , and the best registration result is located at the first increasing point of ϕ(λ).

Discussion on the parameter λ. For Eq. 2, r increases with λ, namely, a larger λ leads to more point pairs. Following this, we run the iteration by changing λ in a decreasing order, which keeps as many point pairs as possible at the beginning and filters outliers gradually. Otherwise, the algorithm may easily drop into a local minimum because of insufficient points.

For partial registration, using as many point pairs as possible will yield a more reliable and accurate result. When two point sets are matched, it is easily proved ϕ(λ) is a decreasing function with respect to λ. As λ varies from λmin  to λmax , ϕ(λ) decreases at first and then increases when mismatching points are found. Therefore, the optimal result is located at the first increasing point of ϕ(λ) which guarantees enough information used in registration.

The proposed algorithm iterates in a similar way as the ICP algorithm. Despite more steps of our algorithm than the ICP algorithm at each iteration, both algorithms spend the same time on the correspondence computation which is the most time-consuming part. Compared with this, the calculation time of other steps can be relatively ignored. Therefore, our algorithm can get similar computation time as ICP, which is quite fast.


Experimental Results

We compare our algorithm with ICP,1 TrICP,5 and FICP,6 in which the latter two are given different values of λ, and the results are reported in root-mean-square (RMS) error.


2D Shapes Registration

Experiments are conducted here on part B of CE-Shape-1 (Ref. 9), where random noise exists on shapes. First, the totally overlapping bat and horse shapes are used to demonstrate our algorithm is robust for full registration. Second, to show the robustness of our algorithm for partial registration, we select one shape pair and trim down one part of each shape to make them partially overlap. The compared results are displayed in Table 1.

Table 1

Compared results on two-dimensional shapes.

bathorsetrimmed battrimmed horse

As is shown in Table 1, our method always yields a good registration with small RMS. ICP only deals with registration of totally overlapping shapes and performs poorly for trimmed shapes that have a large amount of missing data. TrICP can complete some partial registration though, it is quite unstable for different shapes. As TrICP searches the best overlapping percentage by traveling all possible values, the results are likely to be similar. FICP achieves on some shapes, but it substantially relies on the preset value of λ which varies for different shapes registration. Therefore, both TrICP and FICP cannot complete full and partial registration well. In addition, our method automatically obtains an appropriate value of parameter λ, which not only guarantees a proper overlapping percentage r but also uses as many point pairs as possible to obtain more accurate registration results. In bat and horse shapes registration, the largest overlapping percentage reaches while RMS is small. One intutive example of the registration results is shown in Fig. 1.

Fig. 1

Registration results on two-dimensional horse shapes. (a) The original shapes. (b) ICP's result. (c) TrICP's result with λ=4. (d) FICP's result with λ=1.3. (e) FICP's result with λ=5. (f) Our method's result.



3D Range Data Registration

We compare our algorithm with ICP and FICP on Stanford 3D Scanning Repository10 which is acquired from different viewpoints in practice. As the database has too many points and TrICP is too slow and unstable, we do not give the results of TrICP. In the database, Stanford Bunny (bun000 with 40,256 points and bun 045 with 40,097 points), Dragon (dragonStandRight_0 with 41,841 points and dragonStandRight_24 with 34,836 points), and Happy Budda (happyStandRight_0 with 78,056 points and happyStandRight_24 with 75,582 points) are adopted. The compared results are shown in Table 2.

Table 2

Compared results on Stanford Database.

BunnyDragonHappy Budda
AlgorithmsλrRMS (× 10−3)λrRMS (× 10−3)λrRMS (× 10−3)

Table 2 shows that our method gives the best registration results with least RMS, and computes the parameter λ automatically while ICP produces worse results and FICP performs well only when λ is equal to 3 or 5, which demonstrates FICP depends on the parameter λ and its results are unstable. The registration results are displayed in Fig. 2.

Fig. 2

Registration results on three-dimensional Stanford Bunny. (a) The original shapes. (b) ICP's result. (c) FICP's result with λ = 0.95. (d) FICP's result with λ = 3. (e) FICP's result with λ = 5. (f) Our method's result.




This paper proposes a novel approach for registration between two m-D point sets with outliers in the way of incorporating an overlapping percentage into the ICP algorithm. Compared with previous works, our algorithm can automatically compute rigid transformation, correspondence, and overlapping percentage without influence of a parameter. Furthermore, the best overlapping percentage is computed by including as much information as possible, which leads to a satisfying registration. A series of compared experiments demonstrate that our algorithm is stable and precise.


This work was supported by the National Natural Science Foundation of China under Grant Nos. 61005014, 90920008, and 90920301.


1.  P. J. Besl and N. D. McKay, “A method for registration of 3-d shapes,” IEEE Trans. Pattern Anal. Mach. Intell. 14(2), 239–256 (1992). 10.1109/34.121791 Google Scholar

2.  J. S. Park, “Incremental approach to shape recovery and registration from a sequence of images,” Opt. Eng. 46(11), 117202 (2007). 10.1117/1.2802152 Google Scholar

3.  T. Zinßer, J. Schmidt, and H. Niemann, “A refined ICP algorithm for robust 3-D correspondence estimation”, in Proc. IEEE Int. Conf. on Image Processing (ICIP), Barcelona, Spain, Vol. 2, pp. 695–698 (2003). Google Scholar

4.  D. Rodriguez-Losada and J. Minguez, “Improved data association for ICP-based scan matching in noisy and dynamic environments,” in Proc. IEEE Int. Conf. on Robotics and Automation (ICRA), Rome, Italy, pp. 3161–3166 (2007). Google Scholar

5.  D. Chetverikov, D. Stepanov, and P. Krsek, “Robust Euclidean alignment of 3D point sets: the trimmed iterative closest point algorithm,” Image Vis. Comput. 23(3), 299–309 (2005). 10.1016/j.imavis.2004.05.007 Google Scholar

6.  J. M. Phillips, L. Ran, and C. Tomasi, “Outlier robust ICP for minimizing fractional RMSD,” in Proc. Sixth Int. Conf. on 3-D Digital Imaging and Modeling (3DIM), Montreal, Canada, pp. 427–434 (2007). Google Scholar

7.  C. B. Barber, D. P. Dobkin, and H. Huhdanpaa, “The quickhull algorithm for convex hulls,” ACM Trans. Math. Softw. 22(4), 469–483 (1996). 10.1145/235815.235821 Google Scholar

8.  K. S. Arun, T. S. Huang, and S. D. Blostein, “Least-squares fitting of two 3-D point sets,” IEEE Trans. Pattern Anal. Mach. Intell. 9(5), 698–700 (1987). 10.1109/TPAMI.1987.4767965 Google Scholar

9.  L. J. Latecki, R. Lakamper, and T. Eckhardt, “Shape descriptors for non-rigid shapes with a single closed contour,” in Proc. IEEE Conf. on Computer Vision and Pattern Recognition (CVPR), Hilton Head, SC, pp. 424–429 (2000). Google Scholar

10. The Stanford 3D Scanning Repository:  http://graphics.stanford.edu/data/3Dscanrep/Google Scholar


087001_1_d1.jpg Shaoyi Du received double Bachelor degrees in computational mathematics and in computer science in 2002 and received his MS degree in applied mathematics in 2005 and PhD degree in pattern recognition and intelligence system from Xi'an Jiaotong University, China in 2009. His research interests include computer vision, pattern recognition and machine learning.

087001_1_d2.jpg Jihua Zhu received his BE degree from the Central South University China in 2004 and PhD degree in control science and engineering from Xi'an Jiaotong University, China, in 2011. He is a lecturer of Xi'an Jiaotong University. His research interests include mobile robot and image registration.

087001_1_d3.jpg Nanning Zheng graduated in 1975 from the Department of Electrical Engineering, Xi'an Jiaotong University, China, and received his ME degree in information and control engineering from Xi'an Jiaotong University, China in 1981, and PhD degree in electrical engineering from Keio University, Japan, in 1985. He is currently a professor and the director of the Institute of Artificial Intelligence and Robotics in Xi'an Jiaotong University. His research interests include computer vision, pattern recognition, computational intelligence, image processing, and hardware implementation of intelligent systems.

087001_1_d4.jpg Yuehu Liu received his BE and ME degrees from Xi'an Jiaotong University, China in 1984 and 1989 respectively, and PhD degree in electrical engineering from Keio University, Japan, in 2000. He is a professor of Xi'an Jiaotong University. His research interests include computer vision, computer graphics and digital content analysis.

087001_1_d5.jpg Ce Li graduated in 1995 from School of Electrical Engineering and Automation of Hefei University of Technology, China, and received his MS degree in information and control engineering from Xi'an Jiaotong University, China, in 2003. He is currently a PhD student in the institute of Artificial Intelligence and Robotics in Xi'an Jiaotong University. His research interests include computer vision, pattern recognition.

Shaoyi Du, Jihua Zhu, Nanning Zheng, Yuehu Liu, Ce Li, "Robust iterative closest point algorithm for registration of point sets with outliers," Optical Engineering 50(8), 087001 (1 August 2011). https://doi.org/10.1117/1.3607960

Evolutionary algorithms

Image registration

Data modeling


Optical engineering


Artificial intelligence

Back to Top