[Math] Finding a Rotation Transformation from two Coordinate Frames in 3-Space

geometrymatricesrigid transformationtransformation

The question I'm trying to figure out states that I have 3 points P1, P2 and P3 in space. In one frame (Frame A I called it) those points are: Pa1, Pa2 and Pa3, same story for Frame B (namely: Pb1, Pb2 and Pb3).

Whats the rotation matrix from one to the other? That's literally all the information I have. What was suggested was make an intermediate coordinate frame and align one of it's axis' with a point. However I don't see how this will help with the problem.

Can anyone offer some advice how to tackle this problem? I'm stumped.

Thanks,
Edit: Both frames have the same origin, so there is no translation component.

Best Answer

The problem is called Wahba's problem (Note 1) and seeks to minimise the following:

$J(\mathbf{R}) = \frac{1}{2} \sum_{k=1}^{N} a_k|| \mathbf{w}_k - \mathbf{R} \mathbf{v}_k ||^2$

where $a_k$ are weights, $\mathbf{w}_k$ are the vectors in one frame, $\mathbf{v}_k$ are the vectors in the other frame and $\mathbf{R}$ is the rotation matrix that you seek.

You can construct two vectors from the 3 points that you have.

The easiest way to solve it (in my opinion) is by using a Singular Value Decomposition as reported by Markley (1988):

  1. Obtain a matrix $\mathbf{B}$ as follows:

$\mathbf{B} = \sum_{i=1}^{n} a_i \mathbf{w}_i {\mathbf{v}_i}^T$

  1. Find the SVD of $\mathbf{B}$

$\mathbf{B} = \mathbf{U} \mathbf{S} \mathbf{V}^T$

  1. The rotation matrix is simply:

$\mathbf{R} = \mathbf{U} \mathbf{M} \mathbf{V}^T$

where $\mathbf{M} = diag(\begin{bmatrix} 1 & 1 & det(\mathbf{U}) det(\mathbf{V})\end{bmatrix})$

The answer will be "exact" for 2 vectors. With more than 2 vectors, it will choose the "best" rotation that fits the cost function.

For exactly 2 vectors, you can also use the Triad Method which is somewhat simpler to implement.

Note 1: This problem - and related problems - have different names in different fields. See also the following -

References: Markley, F. L. Attitude Determination using Vector Observations and the Singular Value Decomposition Journal of the Astronautical Sciences, 1988, 38, 245-258