My goal is to align 3D point clouds with ICP. Somehow I have an error, I believe it is because of the cumulative matrix transformations.
For debugging I start with 2D point clouds, I created. For the creation of the point clouds, I create a random angle and sign them with cos() and sin() to a x and a y value, so I have random points on a circle. Than I use a translation and a rotation that rises iteratively for each new created image.
I am generating about 20 point clouds and store them in these 512*512 images. Than I want to load does images, create point clouds out of them and align them with ICP.
Now for the cumulative matrix transformation. Image at time 0 would have the Identity matrix. But ever other Image would get as transformation the matrix gathered from ICP (M) multiplied with the transformation matrix, from the last known position: Mi = M * Mi-1
I am not sure If this is the write way, or if I have to transform back to Identity before applying a full transformation.
My results are for 10 point clouds:
In the first we see the gathered point clouds without ICP and in the second with ICP. I tested it before only with translations, that worked really good. And than I tested it with only rotations, and there I had way to high errors. It could be, that the rotation is to high, so ICP aligns the points wrong and than finds wrong matches.
But if I test real data, images gathered from a Xbox Kinect camera, it seems to have the same error like in my example with 2D point clouds.
So am I calculating the cumulative matrix transformation wrong? or is there maybe a different problem which I don't see?
And How should I set up my ICP correctly? I only using the setting :
icp.setTransformationEpsilon (1e-9);
And is there any other way to test it correctly?
