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?

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.

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?

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

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

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

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 .

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

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

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

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.

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

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?

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

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

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

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.

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:

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.

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

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

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