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

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

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

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.

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

Comment only

13 Jun 2014

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

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

Comment only

13 Jun 2014

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

Comment only