Calculate the average/best Rotation between two coordinate systems

242 Views Asked by At

Through a sensor I get the rotation between points in coordinate system A to points in coordinate system B. The measured rotations between the coordinate systems are not $100\%$ identical due to the noise of the sensor.

How can I determine the average or optimal rotation matrix between the coordinate systems? Similar to this problem: stackoverflow: Averaging Quatenion, but contrary to that I do not want to use Quaternions, but try some least square approach.

Given: Rba(n): Rotation matrix from a to b, measured at n different time points

Wanted: Rba optimal

My approach: Minimization of the squared distance.

First I define $n $random points in space and apply the rotations to these points.

$$\begin{pmatrix} bx(n)\\by(n)\\bz(n) \end{pmatrix}=Rba(n) * \begin{pmatrix} \text{random}_xn\\\text{random}_yn\\\text{random}_zn \end{pmatrix}$$

And now I can calculate the rotation by means of the Krabsch algorithm using singular value decomposition to minimize the square distance between the input points and the transformed points. However, what I don't understand is that the calculated rotation matrix seems to be dependent on the input points. That is, I get different rotation matrices as a result for different input points, although the applied rotation matrices $Rba(n)$ remain the same. Why is that? And what is the right way?