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?


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

  • $\begingroup$ Awesome! Great idea. I'll try this and update with results. Thanks $\endgroup$ Feb 8, 2016 at 14:31
  • $\begingroup$ When updating the pose matrix, I would need to do an element by element multiplication, correct? $\endgroup$ Feb 8, 2016 at 16:52
  • $\begingroup$ No, all multiplications are matrix multiplications. $\endgroup$ Feb 8, 2016 at 17:14
  • $\begingroup$ So how does the multiplication when updating the pose work if its a 4x3 matrix (in 3D)? You can't perform that matrix multiplication right? Or am I missing something? $\endgroup$ Feb 8, 2016 at 17:21
  • $\begingroup$ No, you have to add a homogeneous component and store them as 4x4 matrices. I updated my reply. $\endgroup$ Feb 8, 2016 at 21:53

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.