ICP apparently doesn't work!

Hi.

I wrote this code to check icp:

*********

A=[0 0 0;0 0 1;0 1 0;0 1 1;1 0 0;1 0 1;1 1 0;1 1 1];

B=A+.5;

Apc=pointCloud(A);

Bpc=pointCloud(B);

[tf,Bpcicp]=pcregistericp(Bpc,Apc);

*****

I expected a unity Rotation and a 0.5 shift Translation in tf, but Bpcicp was found equal to Bpc. I cannot understand why Bpc is not shifted to A. Thanks.

Answer 1

[Ayush Anand]

Hi Mostafa,

I understand you want to test icp using the matrices A and B and how "pcregistericp" works with the corresponding point clouds; and are not getting the expected results. This is because "pcregistericp" expects both the point clouds to be of the same size. In your code, Apc and Bpc have different sizes as A and B have different sizes, hence the unexpected behavior.

You can resize the B matrix to match the dimensions of A before generating the point clouds, and keep the rest of the code as it is:

numPoints = size(A, 1);
B = B(1:numPoints, :);

This will result in Apc and Bpc having the same size and should give the expected results.

I hope this helps!