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

Question

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.

Answer 1

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$

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

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

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

• Orthogonal Procrustes problem
• Kabsch Algorithm

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

Answer 2

Consider the matrix $M_a$ that gives you the first frame representation of vectors $P_1, P_2, P_3$: $P_{a1}=M_a \cdot P_1, P_{a2}=M_a \cdot P_2, P_{a3}=M_a \cdot P_3$. Similarly, you've got some matrix $M_b$ that gives you the second frame representation: $P_{b1}=M_b \cdot P_1, P_{b2}=M_b \cdot P_2, P_{b3}=M_b \cdot P_3$. Since any $P_i$ is a column-vector, the equations above allow to combine the columns into matrices: $P=(P_1 P_2 P_3), P_a=(P_{a1} P_{a2} P_{a3}), P_b=(P_{b1} P_{b2} P_{b3})$, then $P_a=M_a \cdot P$ and $P_b=M_b \cdot P$. We want to find a matrix, $X$, that would transform $M_a$ into $M_b$: $M_b=M_a \cdot X$, then $X=M_a^{-1} \cdot M_b$.

We derive $M_a$ and $M_b$: $P_a \cdot P^{-1} = M_a$, $P_b \cdot P^{-1} = M_b$. Then $X = P \cdot P_a^{-1} \cdot P_b \cdot P^{-1}$. That obviously requires non-degenerate (non-planar) initial set of $P$ and non-degenerate frames $M_a$ and $M_b$.

Answer 3

Think the general problem is referred to as the absolute orientation problem and suggest to read below paper.

http://cis.jhu.edu/software/lddmm-similitude/umeyama.pdf

Answer 4

Here is a nice link that explains it:

http://igl.ethz.ch/projects/ARAP/svd_rot.pdf

Answer 5

I give an example using the matrix-calculator-language MatMate. We have a matrix A which represents three points in a 3D-coordinate system; the columns represent the x,y,z-axes. Then we have a rotation, its coordinates in matrix B. We try to find the roationmatrix t which provides A * t = B. To provide valid matrices I generate one random matrix A and by a random-rotation tx I generate a valid/meaningful matrix B The key is to find rotations to a "normalized" position, for instance a triangular layout of the coordinate-matrices. Then with t1 rotating A->triangular and t2 rotating B->triangular we get B = A * t1 * inverse(t2)
Example:

 A = randomu(3,3)   // provide a random matrix with 3 points in 3 axes
      A : 
   28.78       28.00       27.32
    9.36       87.72       54.37
   62.89       16.12       96.08

 tx = gettrans(randomu(3,3),"drei")   // provide a random rotation-matrix
 B = A*tx  // generate a valid matrix B which is a rotation of A
      B : 
   46.98        8.03       -9.36
   86.42       50.39       27.03
  108.94      -38.80       -8.57

===========================================================================

t1 = gettrans(A,"drei")  // find a "normalized" position of A, for example the     
                         // rotation to the "triangular" position/layout. 
                         // keep the required rotation-matrix in t1
A1 = A * t1              // rotate A to A1
      t1 : 
    0.59       -0.74       -0.32
    0.58        0.66       -0.47
    0.56        0.10        0.82

      A1 : 
   48.57        0.00       -0.00
   86.70       56.75        0.00
  100.61      -26.37       51.27


t2=gettrans(B,"drei") // now find the rotation which rotates B to the
                      // same "normalized" (=triangular) position/layout
B1 = B * t2
      t2 : 
    0.97        0.04        0.25
    0.17        0.64       -0.75
   -0.19        0.77        0.61

      B1 : // we see, that A1 and B1 are equal
   48.57        0.00        0.00
   86.70       56.75       -0.00
  100.61      -26.37       51.27


 chk = A * t1 * t2'  // check whether the combined rotation t1 and 
                     // inverse(t2)=transpose(t2) rotates A to B
      chk : 
   46.98        8.03       -9.36
   86.42       50.39       27.03
  108.94      -38.80       -8.57
 // We see, that this is equal to B and the combined rotation is tx=t1*t2' 

Answer 6

The way the question is worded, it sounds like you already have two frames A and B (which are orthonormal bases) readily available, which would make the points redundant.

[ If you only have the points, as long as they're not collinear, you can always construct an ONB from either of them: have vector u going from P₁ to P₂ and v from P₁ to P₃, then i = u/|u|; j = uxv, j = j/|j|; k = ixj; and your base is (i,j,k). ]

Since A is a transform from the common "neutral" coordinate system to the A coordinate system, multiplying with the inverse of A will be a transform back to the common coordinate system. In this case, A is an ONB, so "inverse" and "transpose" are synonyms, which makes your life a lot easier.
Similarly, B transforms from the neutral coordinate system to the B coordinate system.

So, to transform any point from A to B, you multiply it with A^T and then with B. You can multiply the matrices together to do the transform in one step if you have to transform many points.

Note that if you do this in a computer program, the transpose actually saves you computional effort rather than adding some, if you are wise while multiplying the matrices together (not the naive route of calling transpose() first and then multiply()).
Multiplying two matrices can be seen as taking dot products of rows and columns. Or, if you look at it this way, dot products of rows and transposed rows. Which makes multiplying a transposed matrix look a lot nicer, because now it maps perfectly to SIMD instructions and memory layout.