Iterative Closest Point Algorithm

Question

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?

Answer 1

Using angles is a very bad idea of storing rotations. They are ambiguous and not always consistently defined.

Store your pose matrix as an augmented matrix of rotation and translation: $P=[R | t]$. In that convention, a point $\mathbf{x} \in \mathbb{R}^3$ is transformed via the operation:

$$ \mathbf{x}' = P\mathbf{x} $$

$P$ is a $4x3$ matrix, while $\mathbf{x}$ is a column vector of 3D coordinates.

Now let $\mathbf{P}_t$ denote the pose at iteration/time $t$, and $\mathbf{P}_\delta$ is the pose of the current update (Solution from the Kabash, Horn, Point-to-Plane etc). This time they are $4x4$ matrices assembled as follows:

$$ \mathbf{P}= \begin{bmatrix} R & t\\ \mathbf{0} & 1\end{bmatrix} $$

Then your new pose is:

$$ \mathbf{P}_{t+1}=\mathbf{P}_\delta \mathbf{P}_t $$

This way you could keep track of the matrix, along the minimization process, without having the need of tracking $R$ and $t$ separately. If your case is 2D, you could still do the same thing, only with reduced size matrices.

You could see an example of using this convention here.