The ICP (iterative closest point) algorithm finds a rigid body transformation such that a set of data points fits to a set of model points under the transformation. Default is to use least squares minimization but other criterion functions can be used as well. The implementation is based on the IRLS-ICP described in .
 Bergström, P. and Edlund, O. 2014, “Robust registration of point sets using iteratively reweighted least squares”, Computational Optimization and Applications, vol 58, no. 3, pp. 543-561, doi: 10.1007/s10589-014-9643-2
 Bergström, P. and Edlund, O. (2016) 2017, “Robust registration of surfaces using a refined iterative closest point algorithm with a trust region approach”, Numerical Algorithms, doi: 10.1007/s11075-016-0170-3
Springer Nature’s SharedIt links (full paper online access):
A demonstration of applications, where an ICP-algorithm  is implemented in Matlab and C, can be seen on YouTube
The function works well but I confirm that with large rotation 45° it fails.
The same clouds of points (with large rotation 45°) don't give any problem in two different coordinate measuring machine software that I have.
I am interested in documentation for least square distance method. Thanks.
fails with large rotation but i am very interested in your algorithm
thank you so much
please Is there any paper or documentation describing this algorithm? In this way I can understand how it works.
Thank you very much!
Note:fails with large rotation!In my test, it fails from 61 degree.
??? Undefined function or method 'nearestNeighbor' for input arguments of type
Error in ==> icp at 188
[vi,resid] = nearestNeighbor(DT,data');
Hi,I'm very interested in your algorithm，but i cannot understand it very well,can you give me more describing about this algorithm?
Hi, I'm very interested in your ICP code. Is there any paper or documentation describing this algorithm? In this way I can understand how it works.
Thank you very much!
My email is:firstname.lastname@example.org
I'm trying to use your program in Octave because I don't currently have access to Matlab. I've nearly got it sorted out, but I'm not 100% sure what the nearNeighbor command is being used for. I am guessing it is interpolation using nearest neighbor methods. If not, please let me know.
Data points containing Inf or NaN are not
how to solve this warning?
changing all NaNs to zeros?
This method works with 2D points, not 3D.
I am getting errors while using icp.m.
Error using qhullmx
The data is degenerate in at least one dimension - ND set of points lying in (N+1)D space.
Error in delaunayn (line 101)
t = qhullmx(x', 'd ', opt);
Error in icp>icp_struct (line 416)
Error in icp (line 318)
tes_flag=icp_struct(tes_flag); % construct a Delaunay tesselation and two vectors make it easy
to find in Tes
Error in test_2 (line 4)
[R, T] = icp(points1, points2);
0. 0. 0.
1. 0. 0.
2. 0. 0.
3. 0. 0.
0. 1. 0.
1. 1. 0.
2. 1. 0.
3. 1. 0.
0. 2. 0.
1. 2. 0.
2. 2. 0.
3. 2. 0.
0. 3. 0.
1. 3. 0.
2. 3. 0.
3. 3. 0.
1. 2. 3.
2. 2. 3.
3. 2. 3.
4. 2. 3.
1. 3. 3.
2. 3. 3.
3. 3. 3.
4. 3. 3.
1. 4. 3.
2. 4. 3.
3. 4. 3.
4. 4. 3.
1. 5. 3.
2. 5. 3.
3. 5. 3.
4. 5. 3.
points1 = dlmread('points1.txt', ' ');
points2 = dlmread('points2.txt', ' ');
[R, T] = icp(points1, points2);
Your ICP is EXCELLENT!Thank you very much !
Hi,Your ICP is very well. In this moment I'm working the icp algorithm.I'm very interested in it.Is there any paper discribing your algorithm? Thank you. My email is email@example.com
Hi,I'm studying the icp algorithm.I'm very interested in it.Is there any paper discribing your algorithm?Could you tell me,thank you very much.My email is :firstname.lastname@example.org
Hi, your ICP code works very well. Is there any paper or documentation describing this algorithm? In this way I can understand how it works.
Hi,I'm very interested in your algorithm. Is there any paper discribing your algorithm?Could you tell me,thank you very much .my email is :email@example.com
Hi,I'm very interested in your algorithm，but i cannot understand it very well,can you give me more describing about about this algorithm? firstname.lastname@example.org
your icp scrip is very good. i could use it very often. But is there any paper discribing your algorithm? I´m interested in its details to understand the operations it does. How does it work?
Hi Mr. Bergström,
your icp scrip is very good. i could use ist very often. But is there any paper discribing your algorithm? I´m interested in its details to understand the operations it does. How does it work?
hello, i would like to know what are the hypothesys for using the ICP and around which axes the rotation R is made ?
Can you please explain how the err function is working in this?
Is there any literature describing this particular algorithm? Is it based on Besl and McKay (1992), or is this a different implementation?
I have run your source codes in Visual C++ 2008 and use the the same data and model for testing in C++, but the results from C++ are different in comparison with from icp_demo.m. When I re-calculate the new data by applying the rotation matrix and translation vector which I have obtained from C++, the plotting of new-data and model points are not match. Could you tell me what reasons? (P/S: my email is email@example.com, if possible please directly send your answers via my email). Thank you very much!
Nguyen Van Thai
cp_struct(tes_flag); % construct a Delaunay tesselation and two vectors make it easy to find in Tes
Good code !! Thank you.
I run the program for 3D data. I used 'model' and 'data' with different sizes and it worked. However, there are about 1-1.5 m differences in z values after gridding the data sets. I also would like to know what is the reference for the program. Thank you.
Does the code works for full 3D objects or only in 2.5D ?
By 2.5D i mean surfaces that can only handle one Z value for every X,Y
Hi, Thanks for this code
One quick question:
Should the new fitted points be
newdata = R*data - T instead of +T as commented in the code?
Hi I can't download icp.rar from this page. can anyone send it for me ?
This programe is quite good. It's the implementation of the original ICP algorithm. the only deficiency is that it only supports input model and data point set with the same dimension.
ICP algo give good results only if we are near a good matching.
You may prefer other methods to approach the solution.
Before than use ICP algo.
There are stochastic ICP that solve this problem.
This code is good, use it without risk
Did not handle scaling. Fails with large rotations and/or translations. Otherwise ok.
Note: I tested only with 2D data.
There could be more comments, especially for some of the alternate input arguements. But on the whole it's a well written and useful m-file for optimization and image registration.
The error message is quite long but the last line is mentioend in my previous message and first is this:
??? qhull precision error: initial facet 1 is coplanar with the interior point
While executing: | qhull d Qt Qbb Qc
When i run the program, following error comes:
Error in ==> icp at 275
icp_struct(tes_flag); % construct a Delaunay tesselation and two vectors make it easy to find in Tes
A new implementation of the ICP algorithm and three examples are added
Have done some small changes!
Have done the last update complete.
Have added a new choice for tesselaton structure.
ICP can now handle point clouds whos principal components do not span the whole space.
Added weighted sum of squares minimization.
Have added a choice to use an other seach method, tes_flag=2 (see help icp).
Download apps, toolboxes, and other File Exchange content using Add-On Explorer in MATLAB.