Iterative Closest Point

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

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 weighting functions, edge point rejection, winsorising, etc.

For an introductory text on the ICP algorithm and the implemented variants, see http://www.scribd.com/doc/58495347/Evaluation-of-Surface-Registration-Algorithms-for-PET-Motion-Correction

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

• Required Products: Statistics Toolbox
• MATLAB release: MATLAB 7.10 (2010a)
• Other requirements: For kDtree matching, the Statistics Toolbox v. 7.3 is needed. Alternatively one can use GLTree, submission #24607

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

[Harivinod N]

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

[Jakob Wilm]

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

[Dengwen Zhou]

Hi, Jakob Wilm
 
The code package is very good! but misses KDTreeSearcher.m.

Can you update it?

[Jakob Wilm]

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

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

[Harivinod N]

 

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

[Ben]

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

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

[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

[Huang Chun-Hsiang]

 

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

[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

[ucd puri]

Many Thanks- this was very helpful. Kind Regards.

[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

[Kevin Matzen]

 

[Mithun]

Does this algorithm include the scaling ICP?

[Jakob Wilm]

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

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

[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

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

[Jakob Wilm]

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