Finding a Rotation Transformation from two Coordinate Frames in 3-Space
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.
Solution 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):
- Obtain a matrix $\mathbf{B}$ as follows:
$\mathbf{B} = \sum_{i=1}^{n} a_i \mathbf{w}_i {\mathbf{v}_i}^T$
- Find the SVD of $\mathbf{B}$
$\mathbf{B} = \mathbf{U} \mathbf{S} \mathbf{V}^T$
- 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
Solution 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$.
Solution 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
Solution 4:
Here is a nice link that explains it:
http://igl.ethz.ch/projects/ARAP/svd_rot.pdf