I am currently working on an iterative closest point algorithm (in C++, see here).
I understand the basic premise of an ICP algorithm. You have two point clouds (a target and a reference) and you want to register the reference into the target. You do this by:
- Associate Pairs of Points (k-d tree or something similar)
- Find optimal rotation and translation such that the RMSE of the distance between the transformed points and the target is minimized (in my case I use the Kabsch algorithm with the singular value decomposition).
- Next, update the reference points with the previously calculated rotation and translation and iterate again using the new reference points and target points.
- Do this until stopping criteria is met.
My question is how to properly keep track of the rotation and translation. Currently I update the rotation by calculating the total rotation (in degrees) so far and adding to it the new rotation (in degrees). To keep track of the total tranlation I take the last translation (i.e. the total translation so far), multiply it by the newly calculated rotation and then add the newly calculated rotation. In other words:
(Assume current look is loop i, newRotation and newTranslation have just been calculated using the Kabsch method,
toMatrix() converts an angle in degrees to a rotation matrix,
toDegrees() converts a rotation matrix to an angle in degrees).
rotation(i) = toMatrix(toDegrees(rotation(i-1)) + toDegrees(newRotation)) translation(i) = (newRotation * translation(i-1)) + newTranslation.
This method seems fundamentally wrong though because my error (Root mean square error) increases with every iteration.
The data I am using to test this is very well matched, in fact I know the mapping beforehand, so the algorithm should converge rather quickly. I have verified my closest point matching works so I think the calculation of the 'total' rotation and translation is the problem. Is this the proper way to implement this or am I doing something wrong?