File Exchange

image thumbnail

Iterative Closest Point Method

version 1.6 (5.88 KB) by

Fits a set of data points to a set of model points under a rigid body transformation

4.07692
26 Ratings

65 Downloads

Updated

View License

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 [1].

References:
 
[1] 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

[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

Doi links:

http://dx.doi.org/10.1007/s10589-014-9643-2

http://dx.doi.org/10.1007/s11075-016-0170-3

http://dx.doi.org/10.1007/s10589-015-9771-3

http://dx.doi.org/10.1007/s00170-010-2950-6

Springer Nature’s SharedIt links (full paper online access):

http://rdcu.be/nJRM

http://rdcu.be/noHE

http://rdcu.be/nJRV

http://rdcu.be/nJUW

A demonstration of applications, where an ICP-algorithm [2] is implemented in Matlab and C, can be seen on YouTube

https://youtu.be/lm7_mwpOk0E

Comments and Ratings (43)

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.
email: matteo.verardo@lighting.com

medo ali

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!
mahmood_022004@yahoo.com

Peng Bao

Note:fails with large rotation!In my test, it fails from 61 degree.

Serge

Serge (view profile)

HELP

??? Undefined function or method 'nearestNeighbor' for input arguments of type
'double'.

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?
leonardopaiva@deg.ufla.br
Thanks.

jianqi shi

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:shijianqi1991@163.com

Amelia Taylor

Per,

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.

Thanks,
Amelia

Hesham Eraqi

Hesham Eraqi (view profile)

Zeng Zhen

Zeng Zhen

Data points containing Inf or NaN are not
supported.
how to solve this warning?
changing all NaNs to zeros?

Zhang,

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)
        Tes=delaunayn(MODEL');

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);

points1.txt

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.

points2.txt

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.

test_2.m

clear;
points1 = dlmread('points1.txt', ' ');
points2 = dlmread('points2.txt', ' ');
[R, T] = icp(points1, points2);

Please advise.

south

south (view profile)

Your ICP is EXCELLENT!Thank you very much !

chang lele

Vicamce

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 vicamce@yahoo.com

zhongling Ma

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 :452687156@qq.com

Marina Marina

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.
Thanks.
xxmariinsxx@hotmail.com

Cheng Zhang

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 :mjcheng@mail.ustc.edu.cn

jack chem

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? we0704csu@163.com
Thanks.

change

change (view profile)

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?
john1529w@hotmail.com
Thanks

Ranger

Ranger (view profile)

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?
robertkamolz@googlemail.com
Thanks

leila hell

hello, i would like to know what are the hypothesys for using the ICP and around which axes the rotation R is made ?

Mithun

Mithun (view profile)

Can you please explain how the err function is working in this?

Yuan Billy

Daniel

Daniel (view profile)

Is there any literature describing this particular algorithm? Is it based on Besl and McKay (1992), or is this a different implementation?
cook348@gmail.com
Thanks.

Thai Nguyen

Dear Per,
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 svcorp110@gmail.com, if possible please directly send your answers via my email). Thank you very much!
Best regards,
Nguyen Van Thai
AOI Lab

Lu Jin

Lu Jin (view profile)

cp_struct(tes_flag); % construct a Delaunay tesselation and two vectors make it easy to find in Tes

Petter

Petter (view profile)

Good code !! Thank you.

Yuksel

Yuksel (view profile)

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.

Hi.
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
Thanks

Brian Chuang

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?

asadi

asadi (view profile)

Hi I can't download icp.rar from this page. can anyone send it for me ?
Thanks alot
atefe.asadi@yahoo.com

Jurandir Junior

Louis Tang

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.

Syed Islam

ray dury

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

erol seke

Did not handle scaling. Fails with large rotations and/or translations. Otherwise ok.
Note: I tested only with 2D data.

J. Lincoln Carpenter

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.

Tahir Yaqub

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
ERRONEOUS FACET:

While executing: | qhull d Qt Qbb Qc

Tahir Yaqub

Hi Bergstorm
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

Updates

1.6

Improved documentation

1.5

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).

MATLAB Release
MATLAB 8.5 (R2015a)

Download apps, toolboxes, and other File Exchange content using Add-On Explorer in MATLAB.

» Watch video