Code covered by the BSD License  

Highlights from
Iterative Closest Point

4.79167

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

Iterative Closest Point

by

 

30 May 2010 (Updated )

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

| Watch this File

File Information
Description

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.

Example:
[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 http://www2.imm.dtu.dk/~jakw/publications/bscthesis.pdf

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

Required Products Statistics 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 (63)
30 Oct 2014 dhara

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!

Regards!
Brant.

10 Oct 2014 peng

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

10 Oct 2014 peng  
12 Aug 2014 Jan

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

alright, problem solved..
Thx

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

13 Jun 2014 Jakob Wilm

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

13 Jun 2014 Francesco

Which options are recommended to maximize speed of the algorithm?

Thank you.

13 May 2014 N S

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

12 May 2014 N S  
07 May 2014 N S

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

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!!

28 Nov 2013 Haixing  
27 Oct 2013 Jakob Wilm

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

27 Oct 2013 arnold

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

06 Jun 2013 Geoffrey  
28 May 2013 dberm22

Exactly what I was looking for! Perfect!

12 Feb 2013 Tim Zaman

I found that the implementation found here: http://www.mathworks.com/matlabcentral/fileexchange/24301-finite-iterative-closest-point 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 ???

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

@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!

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:
http://www.stanford.edu/class/cs273/refs/umeyama.pdf

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

20 Jul 2012 Jaden

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

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

/Jakob

17 Jul 2012 Jakob Wilm

@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.
Try:
z(x,y) = 50*exp(-(x-25.0).^2 ./400.0 -(y-25.0).^2 ./400.0);

/Jakob

17 Jul 2012 Jakob Wilm

@Jaden:
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' ?

/Jakob

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
P

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

18 Jun 2012 Jaden

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!

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:

k=0;
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)];
end
end

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

04 Jun 2012 Jakob Wilm

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

04 Jun 2012 Goran

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??

25 May 2012 Jakob Wilm

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

25 May 2012 Walter

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

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.
Regards,
Jakob

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
Sebastian

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

28 Mar 2012 Jakob Wilm

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

26 Mar 2012 Mithun

Does this algorithm include the scaling ICP?

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,
joachim

18 Nov 2011 ucd puri

Many Thanks- this was very helpful. Kind Regards.

18 Nov 2011 Jakob Wilm

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

Regards,
Jakob

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

18 Nov 2011 Huang Chun-Hsiang  
18 Nov 2011 Axel

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

18 Nov 2011 Axel

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

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

29 Jul 2011 wb fan

Hi,Jakob,
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

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

15 Sep 2010 Dengwen Zhou

Hi, Jakob Wilm

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

Can you update it?

12 Aug 2010 Jakob Wilm

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

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?

Updates
05 Jun 2010

Added link to thesis.

11 Aug 2010

Added rms_error and added a screenshot.

24 Jun 2011

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

24 Jul 2011

Combined functions in one file and added a demo script.

23 May 2012

Corrected link to thesis.

24 May 2012

Updated icp.m with a missing sub function.

17 Jul 2012

* 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

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

21 Sep 2012

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

25 Jan 2013

Replaced the link to the thesis.

Contact us