File Exchange

image thumbnail

Iterative Closest Point

version 1.14 (8.82 KB) by

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

4.63889
44 Ratings

179 Downloads

Updated

View License

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

Comments and Ratings (86)

please i want to ask if this code to correct the change of pose of 3d faces

liu yenfeng

thank you your effort,
but i have some problem:
if my data is a part of my model (ex:model=whole apple data=>half apple)
and the result is fail,the rms is 7.5 it's too high
anyone can tell me how to solve

Tamás Bécsi

liuzhiliang

you forgot to include the 'rmat2quat' function in your icp.m file.

Nice work, thank you for your effort. In my case it returns an error "Error using icp (line 128)
The value of 'q' is invalid. It must satisfy the function: @(x)isreal(x)&&size(x,1)==3.", which I can't figure out why it happens.

Please any help would be much appreciated.
Many thanks

WENNING

NICE YAYA LE

Hyung Kim

good. thanks

Lucas VIVIER

Ralph L

ThankYou...I was looking for this..

Hello Guys,
Please I need some help to enable me apply the ICP algorithm for 2D not 3D ok.
  
The code on the Matlab was very versed and complex. I just want to apply icp algorithm to match data of 2 sampling time ok.

Best regards,
Arthur

young may

Mohammad

Many thanks!!!

PatronBernard

Ayesha

Ayesha (view profile)

Ayesha

Ayesha (view profile)

@jakob: Is it possible to constrain your algorithm to estimate the rotation only in gravity direction, if possible then can you show me how?

Alex Gerber

Jakob, great work with this. I am trying to register hearts, which have both inner and outer surfaces, and I have the ability to determine which nodes correspond to which surface. Is it possible to use this information to explicitly set which surfaces should register closely? Or do you know of a better method for this?

Amir

Amir (view profile)

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

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!

Jakob Wilm

Jakob Wilm (view profile)

  • 1 file
  • 179 downloads
  • 4.63889

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

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?

ANTHONY

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

Walter

Walter (view profile)

Very nice code and thesis link! Works with my data, just as well as this implementation: http://www.mathworks.com/matlabcentral/fileexchange/24301-finite-iterative-closest-point

dhara

dhara (view profile)

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

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

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.

peng

peng (view profile)

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

peng

peng (view profile)

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 ?

liang

liang (view profile)

alright, problem solved..
Thx

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

Jakob Wilm

Jakob Wilm (view profile)

  • 1 file
  • 179 downloads
  • 4.63889

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

Francesco

Which options are recommended to maximize speed of the algorithm?

Thank you.

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

N S

N S (view profile)

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

 

Jinzheng Sha

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

Haixing

Jakob Wilm

Jakob Wilm (view profile)

  • 1 file
  • 179 downloads
  • 4.63889

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

arnold

arnold (view profile)

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

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

Jakob Wilm (view profile)

  • 1 file
  • 179 downloads
  • 4.63889

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

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

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?

Jakob Wilm

Jakob Wilm (view profile)

  • 1 file
  • 179 downloads
  • 4.63889

@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

Jakob Wilm (view profile)

  • 1 file
  • 179 downloads
  • 4.63889

@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

Jakob Wilm (view profile)

  • 1 file
  • 179 downloads
  • 4.63889

@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

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!

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

Jakob Wilm (view profile)

  • 1 file
  • 179 downloads
  • 4.63889

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

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

Jakob Wilm

Jakob Wilm (view profile)

  • 1 file
  • 179 downloads
  • 4.63889

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

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.

Jakob Wilm

Jakob Wilm (view profile)

  • 1 file
  • 179 downloads
  • 4.63889

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

Jakob Wilm (view profile)

  • 1 file
  • 179 downloads
  • 4.63889

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

Mithun

Mithun (view profile)

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

Jakob Wilm (view profile)

  • 1 file
  • 179 downloads
  • 4.63889

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

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

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;

Ben

Ben (view profile)

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

wb fan

wb fan (view profile)

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

Jakob Wilm (view profile)

  • 1 file
  • 179 downloads
  • 4.63889

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

Jakob Wilm (view profile)

  • 1 file
  • 179 downloads
  • 4.63889

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?

Updates

1.14

Replaced the link to the thesis.

1.13

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

1.12

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

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

1.10

Updated icp.m with a missing sub function.

1.9

Corrected link to thesis.

1.8

Combined functions in one file and added a demo script.

1.7

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

1.4

Added rms_error and added a screenshot.

1.1

Added link to thesis.

MATLAB Release
MATLAB 7.14 (R2012a)
Acknowledgements

Inspired: Optimal Step Nonrigid ICP

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

» Watch video