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

[Geoffrey]

 

[dberm22]

Exactly what I was looking for! Perfect!

[Tim Zaman]

I found that the implementation found here: http://www.mathworks.com/matlabcentral/fileexchange/24301-finite-iterative-closest-point performs better.

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

[ocelote]

 

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

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

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

[Anurag Sai]

 

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

[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

[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

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

[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

[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

[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

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

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

[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

[Jakob Wilm]

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

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

[Jakob Wilm]

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

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

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

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

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

[Mithun]

Does this algorithm include the scaling ICP?

[Kevin Matzen]

 

[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

[ucd puri]

Many Thanks- this was very helpful. Kind Regards.

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

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

[Huang Chun-Hsiang]

 

[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

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

[Ben]

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

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

[Harivinod N]

 

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

[Jakob Wilm]

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

[Dengwen Zhou]

Hi, Jakob Wilm

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

Can you update it?

[Jakob Wilm]

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

[Harivinod N]

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

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