Code covered by the BSD License  

Highlights from
Iterative Closest Point

4.8 | 30 ratings Rate this file 324 Downloads (last 30 days) File Size: 8.82 KB File ID: #27804 Version: 1.14
image thumbnail

Iterative Closest Point


Jakob Wilm (view profile)


30 May 2010 (Updated )

An implementation of various ICP (iterative closest point) features.

| Watch this File

File Information

The ICP algorithm takes two point clouds as an input and return the rigid transformation (rotation matrix R and translation vector T), that best aligns the point clouds.

[R,T] = icp(q,p,10);

Aligns the points of p to the points q with 10 iterations of the algorithm.
The transformation is then applied using
R*p + repmat(T,1,length(p));

The file has implemented both point to point and point to plane as well as a couple of other features such as extrapolation, weighting functions, edge point rejection, etc.

For an introductory text on the ICP algorithm and the implemented variants, see

Hans Martin Kjer & Jakob Wilm -- Evaluation of surface registration algorithms for PET motion correction

Required Products Statistics and Machine Learning Toolbox
MATLAB release MATLAB 7.14 (R2012a)
Other requirements For kDtree matching, the Statistics Toolbox v. 7.3 or higher is needed.
Tags for This File   Please login to tag files.
Please login to add a comment or rating.
Comments and Ratings (69)
05 Feb 2015 Amir

Amir (view profile)

@Jakob: it doesn't accept k=0, is ER(1) what I want? I really appreciate your support. Thanks!

Comment only
05 Feb 2015 Amir

Amir (view profile)

@Jakob: and k=0 (no iteration)? because I don't want any registration, I just want the RMS of two input cloud points!

Comment only
05 Feb 2015 Jakob Wilm

Jakob Wilm (view profile)

@Amir: RMS of closest point correspondences would probably be a suitable measure. You get it from the function in the third output.

Comment only
05 Feb 2015 Amir

Amir (view profile)

Hi Jakob, I have a target surface in the form of 3d point clouds and I have to continuously estimate this surface (point clouds) and then compare estimated surfaces with the target surface to see how good my new estimation is. I'm just looking for a metric to compare surfaces in point cloud form, does your mfile return such a metric? Or can I use it to get what I want?

Comment only
09 Jan 2015 ANTHONY

It works very well for my 3D data point fitting :) Thank you

19 Dec 2014 Walter

Walter (view profile)

Very nice code and thesis link! Works with my data, just as well as this implementation:

Comment only
30 Oct 2014 dhara

dhara (view profile)

excellent work and detail description and supported pdf make it soo good....

29 Oct 2014 Sudhanshu Nahata

Very nice and descriptive function. Error plot was icing on the cake!

12 Oct 2014 xiangz ?

Hi! Jakob. When I tried the parameter "weight" and gave a 1xn weight vector, it responsed : w is not a 'function_handle'. Did I get it right that I have to provide a weightting function here? or a weight vector is just enough.

btw, excellent code! thank you very much! It's really inspiring!


10 Oct 2014 peng

peng (view profile)

i don't know why i can not preview this content type.

10 Oct 2014 peng

peng (view profile)

12 Aug 2014 Jan

Jan (view profile)

works realy great on my application.
Can you may tell me the order of the rotations ? Is it: R(z)*R(y)*R(x)* P ?

25 Jul 2014 liang

liang (view profile)

alright, problem solved..

Comment only
20 Jun 2014 Francesco

Jakob, thank you for your answer in such short time.
I found that in my case of study kD-tree may be the best choice.
I'm actually trying to check how the algorithm is working on my clouds: is there a way to visulize the points that the algorithm choose as corresponding points on the two clouds?

thank you

Comment only
13 Jun 2014 Jakob Wilm

Jakob Wilm (view profile)

@ NS: directly this is not possible. Currently, the orthogonal Procrustes problem is solved, and hence a 6DOF transformation found in every iteration. If you have an analytical solution for the constrained case, you could plug that in instead, or use LM-optimization, which allows you to formulate the error function quite freely.
@ Francesco: it depends a bit on your definition of speed. kD-tree is good, and yields the same deterministic output as brute force search. Point to plane is a bit more costly per-iteration, but can yield faster convergence, and hence faster speed. It really depends, I recommend you do timing with your data.

Comment only
13 Jun 2014 Francesco

Which options are recommended to maximize speed of the algorithm?

Thank you.

Comment only
13 May 2014 N S

N S (view profile)

Hey jakob, someone asked similar question before but i would like to ask it again for my own purpose. Is it possible to have your icp algorithm constrained to only translation and also constrained to only translation in a specific direction? Regards,N.S

Comment only
12 May 2014 N S

N S (view profile)

07 May 2014 N S

N S (view profile)

Hi Jakob, I tried to improve my results by changing the matching method to "kd tree", "Delaunay", "brute force". but it did not change my results. could you please tell me what could be the reason? Regards, Nazila

Comment only
06 May 2014 Guangpeng Yan  
04 May 2014 Jinzheng Sha  
07 Feb 2014 Praful Managoli

Hey jakob ... I jus want to know the function call for your this ICP algorithm. Can you please take two 3D matrix data or images as example and show exactly we need to call this ICP function?? I'm badly in need of it. So please help me out!! ..

I tried to call this function using two CT scan images. But I'm facing alot of problem in function call.
Please help me out .

Thanx jakob!!

Comment only
28 Nov 2013 Haixing  
27 Oct 2013 Jakob Wilm

Jakob Wilm (view profile)

@arnold: this implementation was coded and tested for 3D data. That said, you might succeed by setting all z-coodinates to one.

Comment only
27 Oct 2013 arnold

arnold (view profile)

is there a way to use this with two dimensional data?

Comment only
06 Jun 2013 Geoffrey  
28 May 2013 dberm22

dberm22 (view profile)

Exactly what I was looking for! Perfect!

12 Feb 2013 Tim Zaman

I found that the implementation found here: performs better.

09 Feb 2013 Tim Zaman

Only works for simulated data. Real data needs a super high overlap already. Method works not as it should, it is more like iterative-most-overlap algorithm instead of closes point.

10 Dec 2012 ocelote  
27 Nov 2012 ocelote

It seems working but only if the model and data are similar. I have an object AB (3D) composed of A and B. I am using AB as a model and A as data, but in this case A becomes twice bigger in order to fit AB ???

23 Nov 2012 ocelote

It seems working but only if the model and data are similar. I have an object AB (3D) composed of A and B. I am using AB as a model and A as data, but in this case A becomes twice bigger in order to fit AB ???

Comment only
23 Nov 2012 ocelote

It seems working but only if the model and data are similar. I have an object AB (3D) composed of A and B. I am using AB as a model and A as data, but in this case A becomes twice bigger in order to fit AB ???

09 Nov 2012 Anurag Sai  
21 Sep 2012 Jakob Wilm

Jakob Wilm (view profile)

@Tomasz: Thanks for your comment! I have updated the function with your modification. I agree with you that this is a good way to ensure a plausible rotation matrix!

Comment only
19 Sep 2012 Tomasz Malisiewicz

Hi Jakob and Hans,

I've enjoyed using your code, but for a certain application I'm working on, I extracted your eq_point function inside icp.m and found what might be an issue.

There are some scenarios where the returned rotation matrix has det(R)==-1, and for my application this is an issue. I understand that in many real-world datasets this might never come up, but the fix seems easy.

It has already been noted that the closed form SVD-based solution you are using for [R,T] estimation from correspondences can sometimes be problematic.

You can checkout the following classic paper from Umeyama:

my solution was to change on line of eq_point to do the following:
R = V*diag([1 1 det(U*V')])*U';

I haven't thoroughly tested this, but it *seems* to fix the problem for me.

Cheers and happy hacking,
Dr. Tomasz Malisiewicz
MIT CSAIL Postdoctoral Research Scholar

28 Jul 2012 Bhavani

Ahhh, p and q are (3 x m) and (3 x n). My dataset was (m x 3). Think thats sorted. But now I get another error : I replaced all the tilde's with 'dummy' but there seems to be a division by zero error. I now get this response :
??? Error using ==> svd
Input to SVD must not contain NaN or Inf.
Error in ==> icp>eq_point at 345
[U,dummy,V] = svd(N); % singular value decomposition

Error in ==> icp at 231
[R,T] = eq_point(q(:,q_idx),pt(:,p_idx), weights(p_idx));

What do I do here?I have real 3D point clouds to register. Thanks again for your code and help

Comment only
20 Jul 2012 Jaden

Jaden (view profile)

Your code is working great for me! (It turns out that the random noise adding was causing my initial problem). However, it only seems to work well when the matching is close -- does it have a place to input an initial guess so that it works better?

17 Jul 2012 Jakob Wilm

Jakob Wilm (view profile)

@Mark Brophy:
Thank you very much for pointing this out. I discovered that the kNearestNeighbors was missing altogether, and that kNN searching is supported only in Stat. Toolbox v. 7.5 or higher.
I added the missing sub function and updated the code, which should be available in about 24 hours.


Comment only
17 Jul 2012 Jakob Wilm

Jakob Wilm (view profile)

@Pietro Pala:
I think you might be mistaking accuracy for robustness.
You are basically trying to register planes, and there is very little chance of converging with your data.
z(x,y) = 50*exp(-(x-25.0).^2 ./400.0 -(y-25.0).^2 ./400.0);


Comment only
17 Jul 2012 Jakob Wilm

Jakob Wilm (view profile)

I believe that everything in the algorithm is deterministic. However, I do not know the details of Mathwork's kD-tree implementation, and there might be some fuzziness.
You are not showing the code you run, so I have no chance to tell what exactly is going wrong.
Same results with 'Matching' == 'bruteForce' ?


Comment only
14 Jul 2012 Mark Brophy

Jakob, when you run icp without version 7.3 of the statistics package while using the 'plane' option, it bombs out on the calculation of the normal.

It appears that the stats package returns the knn in a different format than the kNearestNeighbors method.

Error using ==> minus
Matrix dimensions must agree.

Error in ==> icp>lsqnormest at 547
P = (x - repmat(p_bar,1,k)) *
transpose(x -
repmat(p_bar,1,k)); %spd matrix

Error in ==> icp at 160
arg.Normals = lsqnormest(q,4);

Comment only
18 Jun 2012 Jaden

Jaden (view profile)

Hello -- the ICP code seems to be working great, even with 2D inputs, except that I keep getting this error when I run the icp code in a loop (and it doesn't happen at the same iteration of the loop every time, sometimes at 86, 102, 106, 107, 109, and more) even with the same variables and settings and data.

Error code:

??? Error using ==> KDTreeSearcher.knnsearch at 165 Complex data is not allowed.

Error in ==> icp>match_kDtree at 335 [match mindist] = knnsearch(kdOBJ,transpose(p));

Error in ==> icp at 289 [~, mindist] = match_kDtree(q,pt,kdOBJ);

Error in ==> testing_icp_movie at 37 [Ricp Ticp ER t] = icp(M, D, 50,'Matching', 'kDtree', 'Extrapolation',true);

Error in ==> overallprogramming_Try2 at 39 testing_icp_movie;

Any ideas? Thank you!

Comment only
14 Jun 2012 Pietro Pala

I'm trying to test the accuracy of this routine on a known point distribution: model points are the [x y z] coordinates of a Gaussian function; data points are obtained by translating by T the (x,y) coordinates of the model points (data = model + repmat([1.5 1.5 0], size(model,1), 1)). Even for small values of T the routine cannot converge to the optimum. Here is the code used for the test:

for x=1:50
for y=1:50
z(x,y) = exp(-(single(x) -25.0).^2 ./400.0 -(single(y)-25.0).^2 ./400.0);
k = k+1;
model(k,:)=[x y z(x,y)];

surf(double([1:50]), double([1:50]), double(z));

data = model + repmat([15.5 15.5 0], size(model,1), 1);

[TR, TT, ER] = icp(double(model'), double(data'), 20)

Any suggestion? Thx

Comment only
04 Jun 2012 Jakob Wilm

Jakob Wilm (view profile)

Goran: make sure the file is in the Matlab path. Matlab 2007a/b should work.

Comment only
04 Jun 2012 Goran

Goran (view profile)

Hi Jakob, every time I try to run---

[Ricp Ticp ER t] = icp(M, D, 15);

I get an error: ??? Undefined function or method 'icp' for input arguments of type 'double'.

Can you tell me what is the problem, I have 2007 matlab, maybe the icp function don't exist in my case??

Comment only
25 May 2012 Jakob Wilm

Jakob Wilm (view profile)

Walter: inp, DT, kdOBJ, etc. are all scope managed, so there is no memory leak.

Comment only
25 May 2012 Walter

Walter (view profile)

Hi Jacob your code is very helpful.
BTW I noticed two memory leaks.
One is in the implementation of GlTree, I will post in that FileExchange. The other is in the usage of inputParser. You should clear the object inp.

23 May 2012 Jakob Wilm

Jakob Wilm (view profile)

Hi Sebastian,
thank you for letting me know. I have corrected the link, and will re-add the rmat2quat function ASAP. It will go through Mathworks review and should be available tomorrow.
To constrain the ICP to certain DOF, I do not believe there is a closed form solution for that kind of Procrustes problem, but you should check the literature.
My idea would be that you either modify eq_point() or eq_plane() and remove the 'unwanted' DOF from R and T after estimation (this is somewhat hackish). Or you could use eq_lmaPoint() which performs non-linear optimization on the parameters, so you can basically model anything there (this might be slow).
Alternatively you could pose the whole fitting procedure a non-linear optimization problem, which can actually be competitive with ICP. See: Andrew W. Fitzgibbon. Robust registration of 2d and 3d point sets. Image and Vision Computing, 21(13-14):1145–1153, 2003.

Comment only
23 May 2012 Sebastian

Hello Jakob,
Because your ICP implementation was featured in the recent MATLAB Digest - Mai 2012 newsletter, I revisited it to see if updates are available. I noticed in your new version that you forgot to include the 'rmat2quat' function in your icp.m file. You can reproduce the error if you enable Extrapolation in your demo.m file, e.g. line 57.

For my current work I would like to integrate some additional constraints like the ones mentioned in Joachims comment from 20 Feb 2012. Do you think this is possible with your framework?

Greetings from Germany

P.S. The link to the introductory text in the description is broken (page not found by DTU).

Comment only
28 Mar 2012 Jakob Wilm

Jakob Wilm (view profile)

Hello Mithun,
scaling is not solved for, only translation and rotation (6 d.o.f. rigid transform).

Comment only
26 Mar 2012 Mithun

Mithun (view profile)

Does this algorithm include the scaling ICP?

Comment only
25 Feb 2012 Kevin Matzen  
20 Feb 2012 Joachim

Hello Jakob,
very nice work. Thx a lot for it.
I have some special boundary conditions here which i am not sure how to implement and fit to your code.

I need to fix certain degrees of freedom during the ICP. As an example I would like to allow the result to be a translation only in X/Y direction and a rotation around Z-Axis.

Do you or anybody else know how to easily integrate such a feature into the code?

Thx in advance!
All the best,

Comment only
18 Nov 2011 ucd puri

Many Thanks- this was very helpful. Kind Regards.

Comment only
18 Nov 2011 Jakob Wilm

Jakob Wilm (view profile)

Hello ucd puri,

in your case, Matlab complaints about the following line (226 in icp.m):
[~, idx] = sort(mindist);
This is probably due to the fact that older versions of Matlab don't understand the tilde (~) as a way of discarding a certain output variable. You can either update to a newer version of Matlab or use a dummy output (you will have to do this at several places inside the code), e.g.:
[dummy, idx] = sort(mindist);


Comment only
18 Nov 2011 ucd puri

Hi Jakob
I am getting the following error when I run demo file. Any suggestions. many thanks.
??? Error: File: icp.m Line: 226 Column: 11
Expression or statement is incorrect--possibly unbalanced (, {, or [.

Error in ==> demo at 57
[Ricp Ticp ER t] = icp(M, D, 15);

Comment only
18 Nov 2011 Huang Chun-Hsiang  
18 Nov 2011 Axel

Axel (view profile)

Posted the comment to quickly. The argument checking seems to be fine. It is coherent with the rest of the code.

Sorry for the confusion

Comment only
18 Nov 2011 Axel

Axel (view profile)

There seems to exist a little bug in your function arguments parsing. The line:
inp.addParamValue('Normals', [], @(x)isreal(x) && size(x,1) == 3);
seems to check dimensions differently from what the help states.
Shouldn't instead read:
inp.addParamValue('Normals', [], @(x)isreal(x) && size(x,2) == 3;

19 Oct 2011 Ben

Ben (view profile)

Can it be used for 2D point sets registration? It raised error message asking for 3D data.

Comment only
29 Jul 2011 wb fan

wb fan (view profile)

Demo.m is very helpful, but it doesn't tell me how to use the other setting( Normals, Weight, Triangulation...).Could you please show me some more sample code.
Thanks for the great work.

11 Jul 2011 Harivinod N  
16 Mar 2011 Sebastian

Hello Jakob,
I've looked at different Matlab ICP-packages and yours is the fastest and most flexible so far. I just wanted to point out a little typo.

In the documentation [line 50] you write: Minimize {point} | plane | lmaPoint
But in the code [line 123] you check for: validMinimize = {'point','plane','lma_point'};

Thanks for the great package.

12 Oct 2010 Jakob Wilm

Jakob Wilm (view profile)

For kDtree matching, the Statistics Toolbox v. 7.3 is needed.

Comment only
15 Sep 2010 Dengwen Zhou

Hi, Jakob Wilm

The code package is very good! but misses KDTreeSearcher.m.

Can you update it?

Comment only
12 Aug 2010 Jakob Wilm

Jakob Wilm (view profile)

Thank you for your interest. The missing function (rms_error.m) has now been added.

Comment only
07 Aug 2010 Harivinod N

I am also getting the similar error as mention by Michel(30 Jul 2010). It will be helpful, if this is fixed.

30 Jul 2010 Michael H

Looks very good, but I can't get icp.m to run as I think it is missing the rms_error function. Am I doing something wrong?

05 Jun 2010 1.1

Added link to thesis.

11 Aug 2010 1.4

Added rms_error and added a screenshot.

24 Jun 2011 1.7

Corrected a typo in icp.m (as pointed out in the comments, thanks!)

24 Jul 2011 1.8

Combined functions in one file and added a demo script.

23 May 2012 1.9

Corrected link to thesis.

24 May 2012 1.10

Updated icp.m with a missing sub function.

17 Jul 2012 1.11

* Added a missing sub function: k_nearest_neighbors
* Fixed a bug in the lsqnorest sub function
* Removed GLTree reference because it is not available any longer
* Updated the examples to be clearer

10 Aug 2012 1.12

Clearing the input parser object due to memory leaks (in older Matlab versions?) -- thanks, Walter!

21 Sep 2012 1.13

Modified Orthogonal Procrustes to prevent non-plausible rotation matrix (det(R) should be +1).
-- Thank you, Tomasz!

25 Jan 2013 1.14

Replaced the link to the thesis.

Contact us