How to calculate the Rotation Matrix from other known values?

Question

Dear Math Stackexchange community, how can I compute $R$ rotation matrix in this equation ($P=Rp+t$)? (Please see the figure. More detailed explanation is under the figure.)

The equation is for the transformation of $p=p_1,p_2,p_3$ points. $p$ is in Oxyz (local) reference frame. $p$ consists of 3 points of $p_1,p_2,p_3$. And $p_1,p_2,p_3$ each has 3 coordinates of $x,y,z$. Therefore p is 3x3 matrix. $p=\begin{bmatrix} p_{1x} & p_{2x} & p_{3x} \\ p_{1y} & p_{2y} & p_{3y} \\ p_{1z} & p_{2z} & p_{3z} \end{bmatrix}$

$t$ is the distance between Oxyz(local) and OXYZ(global) reference frame. $t=\begin{bmatrix} t_x \\ t_y \\ t_z \end{bmatrix}$. To be able to do matrix operations, we can take $t$ as $t=\begin{bmatrix} t_x&t_x&t_x \\ t_y&t_y&t_y \\ t_z&t_z&t_z \end{bmatrix}$

$P$ is the coordinates of p_1, p_2, and p_3 in OXYZ global reference frame. $P$ is a 3x3 matrix. $P=\begin{bmatrix} P_{1x} & P_{2x} & P_{3x} \\ P_{1y} & P_{2y} & P_{3y} \\ P_{1z} & P_{2z} & P_{3z} \end{bmatrix}$

$R$ is the (orthogonal) rotation matrix. $R=\begin{bmatrix} r_{11}&r_{12}&r_{13} \\ r_{21}&r_{22}&r_{23} \\ r_{31}&r_{32}&r_{33} \end{bmatrix}$

Therefore, all in all: $P=Rp+t$

$\begin{bmatrix} P_{1x} & P_{2x} & P_{3x} \\ P_{1y} & P_{2y} & P_{3y} \\ P_{1z} & P_{2z} & P_{3z} \end{bmatrix} = \begin{bmatrix} r_{11}&r_{12}&r_{13} \\ r_{21}&r_{22}&r_{23} \\ r_{31}&r_{32}&r_{33} \end{bmatrix}.\begin{bmatrix} p_{1x} & p_{2x} & p_{3x} \\ p_{1y} & p_{2y} & p_{3y} \\ p_{1z} & p_{2z} & p_{3z} \end{bmatrix}+\begin{bmatrix} t_x&t_x&t_x \\ t_y&t_y&t_y \\ t_z&t_z&t_z \end{bmatrix}$

We can derive the following form of the equation of $P=Rp+t$

$(P-t)=R.p$

$(P-t).p^{-1}=R$

$p$ is an arbitrary matrix, and not always be taken of its inverse. I can't take the inverse of $p$, if last raw of $p$ is zero.

Let's look at an example to make it clear: $P=Rp+t$

$p_1=(1,1,0)$ , $p_2=(2,4,0)$ , $p_3=(5,10,0)$ is the coordinates of three points. So in matrix form: $p=\begin{bmatrix} 1 & 2 & 5 \\ 1 & 4 & 10 \\ 0 & 0 & 0 \end{bmatrix}$

$t=\begin{bmatrix} 0 \\ 0 \\ 50 \end{bmatrix}$

$\alpha=10^\circ $ (rotation around the x axis)

$\beta=15^\circ $ (rotation around the y axis)

$\gamma=20^\circ $ (rotation around the z axis)

$R_x=\begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(\alpha) & -sin(\alpha) \\ 0 & sin(\alpha) & cos(\alpha) \end{bmatrix}$

$R_y=\begin{bmatrix} cos(\beta) & 0 & sin(\beta) \\ 0 & 1 & 0 \\ -sin(\beta) & 0 & cos(\beta) \end{bmatrix}$

$R_x=\begin{bmatrix} cos(\gamma) & -sin(\gamma) & 0 \\ sin(\gamma) & cos(\gamma) & 0 \\ 0 & 0 & 1 \end{bmatrix}$

$R=R_z.R_y.R_x$

Therefore, for $\alpha=10^\circ $, $\beta=15^\circ $, $\gamma=20^\circ $, the rotation matrix is :

$R=\begin{bmatrix} 0.9077 & -0.2946 & 0.2989 \\ 0.3304 & 0.9408 & -0.0760 \\ -0.2588 & 0.1677 & 0.9513 \end{bmatrix}$

$P=R.p+t$

$P=$$\begin{bmatrix} 0.9077 & -0.2946 & 0.2989 \\ 0.3304 & 0.9408 & -0.0760 \\ -0.2588 & 0.1677 & 0.9513 \end{bmatrix}$$.\begin{bmatrix} 1 & 2 & 5 \\ 1 & 4 & 10 \\ 0 & 0 & 0 \end{bmatrix}$ $+\begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 50 & 50 & 50 \end{bmatrix}$

$P=\begin{bmatrix} 0.6131 & 0.6370 & 1.5925 \\ 1.2712 & 4.4239 & 11.0597 \\ 49.9089 & 50.1533 & 50.3832 \end{bmatrix}$

$p_1=(0.6131,1.2712,49.90)$,$p_2=(0.6370,4.4239,50.1533)$,$p_3=(1.5925,11.0597,50.3832)$.

Now let's take opposite example with the same values to find $R$, with the values of $P,p,t$ as known.

$P=R.p+t$

$\begin{bmatrix} 0.6131 & 0.6370 & 1.5925 \\ 1.2712 & 4.4239 & 11.0597 \\ 49.9089 & 50.1533 & 50.3832 \end{bmatrix}=R.$ $\begin{bmatrix} 1 & 2 & 5 \\ 1 & 4 & 10 \\ 0 & 0 & 0 \end{bmatrix}+$ $\begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 50 & 50 & 50 \end{bmatrix}$

Could you find $R$ here in the above equation?

To repeat the formulation:

$P=Rp+t$

$(P-t)=Rp$

$(P-t).p^{-1}=R.p.p^{-1}$

$(P-t).p^{-1}=R$

For the inverse of $p$ (where $p$ is $\begin{bmatrix} 1 & 2 & 5 \\ 1 & 4 & 10 \\ 0 & 0 & 0 \end{bmatrix}$ ), I can't take the inverse of $p$.

Here I am stuck. It is interesting for me, because for the condition that $R,p,t$ is known, $P$ can be calculated(9 unknown elements in 3x3 matrix of $P$). However, for the condition that $P,p,t$ is known, I am having problem to compute $R$ (9 unknown elements in 3x3 matrix of $R$). I should be able to compute the equivalent $R$ matrix for that inverse problem.

Do you have any idea about how to find $R$ rotation matrix? Any suggestion would be appreciated.

Thanks in advance.

Answer 1

Firstly, I think the extra gymnastics with $t$ is a distraction, so I will state the problem as:

$\mathbf{Q}=\left[\mathbf{q}_1\,\dots,\mathbf{q}_k\right]=\mathbf{P}-\mathbf{t}=\mathbf{R}.\boldsymbol{\mathcal{P}}=\mathbf{R}.\left[\mathbf{p}_1\,\dots\,\mathbf{p}_k\right]$

Where $\mathbf{Q}$ is some $3\times k$ real-valued matrix, which we can also view as $k$ column vectors $\mathbf{q}_{1,\dots k}$. $\mathbf{R}$ is a real-valued rotation matrix, which means it is orthogonal ($\mathbf{R}^T\mathbf{R}=\mathbf{Id}=identity$) and has determinant of $det\left[\mathbf{R}\right]=+1$. Finally, matrix $\boldsymbol{\mathcal{P}}$ is made up from 3d real-valued column vectors $\mathbf{p}_{1,\dots k}$. The aim is to determine the rotation matrix.

Here, for simplicity, I will work with linearly independent sets of $\mathbf{p}$-vectors, so $k=\mbox{rank}\left(\mathbf{Q}\right)=\mbox{rank}\left(\boldsymbol{\mathcal{P}}\right)$

$k=\mbox{rank}\boldsymbol{\mathcal{P}}=3$

If you can pick three linearly independent vectors $\mathbf{p}_{1,2,3}$ you can have a unique solution for $\mathbf{R}$ given $\mathbf{Q}$ (by inverting $\boldsymbol{\mathcal{P}}$). If you cannot such unique solution does not exist.

Use rank function to check whether you have independent $\mathbf{p}$-vectors

$k=\mbox{rank}\boldsymbol{\mathcal{P}}=2$

If you only have two linearly independent vectors, I think the best you can in principle get two possible solutions, of which both fit the data. In some special cases you will still be able to get the unique solution.

Use Gram-Schmidt process to extract two orthonormal vectors $\mathbf{\hat{n}}_{1,2}$ which can be expressed as $\mathbf{\hat{n}}_{1,2}=\alpha_{1,2}\mathbf{p}_1+\beta_{1,2}\mathbf{p}_2$.

There will be two further orthonormal vectors $\mathbf{\hat{q}}_{1,2}=\alpha_{1,2}\mathbf{q}_1+\beta_{1,2}\mathbf{q}_2$.

Now try to find $\psi$ that minimizes: $s=\left|\left(\cos\psi\,\mathbf{\hat{q}_1+\sin\psi\,\mathbf{\hat{q}}_2}\right)-\left(\cos\psi\,\mathbf{\hat{n}_1+\sin\psi\,\mathbf{\hat{n}}_2}\right)\right|$, this will give you the projection of the axis around which matrix $\mathbf{R}$ causes the rotation. Let the actual rotation axis be $\boldsymbol{{\hat{\omega}}}$, and its projection into plane spanned by $\mathbf{\hat{n}}_{1,2}$ be $\boldsymbol{{\omega}}_\mathcal{P}$. The logic here is simple if $\boldsymbol{\hat{\omega}}=\left(\cos\psi\,\mathbf{\hat{n}_1+\sin\psi\,\mathbf{\hat{n}}_2}\right)$ then rotating such vector will not change it at all, by definition.

Two interesting cases here are when $s=const$ for all $\psi$. Then your rotation matrix's axis is perpendicular to the plane spanned by $\mathbf{\hat{n}}_{1,2}$, and you can make the problem 2d (i.e. make $\mathbf{R}$ 2d) and find exact solution.

Another interesting case is when $s=0$ or very close to 0, say 1e-3, for some $\psi$. Then you found a way to express the rotation axis in terms of your orthonormal vectors: $\boldsymbol{\hat{\omega}}=\left(\cos\psi\,\mathbf{\hat{n}_1+\sin\psi\,\mathbf{\hat{n}}_2}\right)$. I think you will be able to work out the rotation angle of $\mathbf{R}$ in this case by looking at what happens with $-\sin\psi\mathbf{\hat{n}}_1+\cos\psi\mathbf{\hat{n}}_2$, i.e. vector that is perpendicular to rotation axis.

In general, if you found $\boldsymbol{\omega}_\mathcal{P}$ you can extend it to full rotation axis by adding to it a component of vector perpendicular to the plane spanned by $\mathbf{\hat{n}}_{1,2}$, $\mathbf{\hat{m}}=\mathbf{\hat{n}}_1\times \mathbf{\hat{n}}_2$. So

$$ \boldsymbol{\hat{\omega}}=\frac{\boldsymbol{\omega}_{\mathcal{P}}+f\mathbf{\hat{n}}_1\times \mathbf{\hat{n}}_2}{\left|\boldsymbol{\omega}_{\mathcal{P}}+f\mathbf{\hat{n}}_1\times \mathbf{\hat{n}}_2\right|} $$

There will be ambiguity in choosing the sign of $f$ - but that's the non-uniqueness for you. Once you have the rotation axis, you should be able to reduce the problem to 2d again, and solve it exactly (up to sign of $f$)

$k=\mbox{rank}\boldsymbol{\mathcal{P}}=1$

Not sure there is much you can do here.

Answer 2

For clarity reasons, let's use capital letters to denote Matrices, and lowercase for vectors so let's talk about $$ {\bf q}_{\,k} = {\bf R}\,{\bf p}_{\,k} + {\bf t} $$ and $$ {\bf Q} = {\bf R}\,{\bf P} + {\bf T}\quad \left| {\;{\bf T} = \left( {\matrix{ {t_{\,x} } & {t_{\,x} } & {t_{\,x} } \cr {t_{\,y} } & {t_{\,y} } & {t_{\,y} } \cr {t_{\,z} } & {t_{\,z} } & {t_{\,z} } \cr } } \right)} \right. = \left( {\matrix{ {{\bf t}\;,} & {{\bf t}\;,} & {{\bf t}\;} \cr } } \right) $$

1. A rotation matrix is Orthogonal, which means that it preserves the dot product of two vectors and the modulus of their cross product.
   Consequently the two triangles $P,Q$ must be congruent, including reflection since we are dealing with a 2D shape embedded in 3D. If they are congruent then we shall have $$ \left\{ \matrix{ \left( {{\bf q}_{\,2} - {\bf q}_{\,1} } \right) \cdot \left( {{\bf q}_{\,2} - {\bf q}_{\,1} } \right) = \left( {{\bf p}_{\,2} - {\bf p}_{\,1} } \right) \cdot \left( {{\bf p}_{\,2} - {\bf p}_{\,1} } \right) \hfill \cr \left( {{\bf q}_{\,3} - {\bf q}_{\,1} } \right) \cdot \left( {{\bf q}_{\,3} - {\bf q}_{\,1} } \right) = \left( {{\bf p}_{\,3} - {\bf p}_{\,1} } \right) \cdot \left( {{\bf p}_{\,3} - {\bf p}_{\,1} } \right) \hfill \cr \left( {{\bf q}_{\,2} - {\bf q}_{\,1} } \right) \cdot \left( {{\bf q}_{\,3} - {\bf q}_{\,1} } \right) = \left( {{\bf p}_{\,2} - {\bf p}_{\,1} } \right) \cdot \left( {{\bf p}_{\,3} - {\bf p}_{\,1} } \right) \hfill \cr \left| {\left( {{\bf q}_{\,2} - {\bf q}_{\,1} } \right) \times \left( {{\bf q}_{\,3} - {\bf q}_{\,1} } \right)} \right| = \left| {\left( {{\bf p}_{\,2} - {\bf p}_{\,1} } \right) \times \left( {{\bf p}_{\,3} - {\bf p}_{\,1} } \right)} \right| \hfill \cr} \right. $$ where the labels shall be permuted if there is not a point to point correspondence between $\bf p$ and $\bf q$.

2. A triangle lies on a plane. If that plane passes through the origin then the position vectors of the vertices are co-planar, i.e. dependent and so the matrix ($\bf P$ and/or $\bf Q$) has determinant null and is not invertible.
   Also, a triangle which is far from the origin, when acted by a rotation matrix will experience a rotation about one of its points plus a translation of that point.

3. Then the most practical approach is to construct the matrices $$ \eqalign{ & {\bf \Delta P} = \left( {\matrix{ {{\bf p}_{\,2} - {\bf p}_{\,1} \;,} & {{\bf p}_{\,3} - {\bf p}_{\,1} \;,} & {\left( {{\bf p}_{\,2} - {\bf p}_{\,1} } \right) \times \left( {{\bf p}_{\,3} - {\bf p}_{\,1} } \right)} \cr } } \right) \cr & {\bf \Delta Q} = \left( {\matrix{ {{\bf q}_{\,2} - {\bf q}_{\,1} \;,} & {{\bf q}_{\,3} - {\bf q}_{\,1} \;,} & {\left( {{\bf q}_{\,2} - {\bf q}_{\,1} } \right) \times \left( {{\bf q}_{\,3} - {\bf q}_{\,1} } \right)} \cr } } \right) \cr} $$ which, for the congruence identities established above, will give $$ \overline {{\bf \Delta P}} \,{\bf \Delta P} = \overline {{\bf \Delta Q}} \,{\bf \Delta Q} $$ and $$ \left| {\,{\bf \Delta Q}\,} \right| = \left| {\,{\bf \Delta P}\,} \right| $$

4. Now we have $$ {\bf \Delta Q} = {\bf R}\,{\bf \Delta P} $$ and we can retrive $\bf R$ because $\bf \Delta P$ is invertible (if the triangle is not degenerated).
   To determine $\bf t$ we have then $$ \eqalign{ & \left( {{\bf q}_{\,2} - {\bf q}_{\,1} } \right) = {\bf R}\left( {{\bf p}_{\,2} - {\bf p}_{\,1} } \right)\quad \Rightarrow \cr & \Rightarrow \quad {\bf q}_{\,2} = {\bf R}\,{\bf p}_{\,2} + {\bf q}_{\,1} - {\bf R}\,{\bf p}_{\,1} \quad \Rightarrow \cr & \Rightarrow \quad {\bf t} = {\bf q}_{\,1} - {\bf R}\,{\bf p}_{\,1} = {\bf q}_{\,2} - {\bf R}\,{\bf p}_{\,2} = {\bf q}_{\,3} - {\bf R}\,{\bf p}_{\,3} \cr} $$

In conclusion and in plain words words

• translate both triangles to have one vertex at the origin;
• add to each triangle the cross-product of the edges from the origin, to build two tetrahedrons;
• rotate one tetrahedron to coincide with the other;
• translate back the triangles.

Example

Taking the data of your example $$ \eqalign{ & {\bf P} = \left( {\matrix{ 1 & 2 & 5 \cr 1 & 4 & {10} \cr 0 & 0 & 0 \cr } } \right)\quad \Rightarrow \quad {\bf \Delta P} = \left( {\matrix{ 1 & 4 & 0 \cr 3 & 9 & 0 \cr 0 & 0 & { - 3} \cr } } \right)\quad \left| {\,{\bf \Delta P}\,} \right| = 9 \cr & {\bf Q} = \left( {\matrix{ {0.6131} & {0.6370} & {1.5925} \cr {1.2712} & {4.4239} & {11.0597} \cr {49.9089} & {50.15330} & {50.3832} \cr } } \right)\quad \Rightarrow \cr & \Rightarrow \quad {\bf \Delta Q} = \left( {\matrix{ {0.0239} & {0.9794} & { - 0.8970} \cr {3.1527} & {9.7885} & {0.2280} \cr {0.2444} & {0.4743} & { - 2.8538} \cr } } \right)\quad \left| {\,{\bf \Delta Q}\,} \right| = 9.0008 \cr} $$

Then $$ {\bf R} = {\bf \Delta Q\Delta P}^{\, - \,{\bf 1}} = \left( {\matrix{ {0.9077} & { - 0.2946} & {0.2990} \cr {0.3304} & {0.9408} & { - 0.0760} \cr { - 0.2589} & {0.1678} & {0.9513} \cr } } \right)\quad \left| {\,{\bf R}\,} \right| = 1.0001 $$ and $$ {\bf t} = {\bf q}_{\,1} - {\bf R}\,{\bf p}_{\,1} = \left( {\matrix{ 0 \cr 0 \cr {50} \cr } } \right) $$ and these check with $$ {\bf Q} = {\bf R}\,{\bf P} + {\bf T}\quad \quad {\bf R} = {\bf R}_{\,{\bf z}} (\gamma ){\bf R}_{\,{\bf y}} (\beta ){\bf R}_{\,{\bf x}} (\alpha ) $$

Answer 3

Matrices can do a lot more than rotation. I think this may be part of what is making the problem difficult. Indeed to completely determine a general linear transformation in $\mathbb R^3,$ which is what a general real $3\times3$ matrix encodes, you need to know what happens to at least three linearly independent vectors. In this more general case, your $p$ matrix must be invertible in order for there to be a unique solution.

Limiting the scope of the transformation to a rotation changes the problem dramatically. You go from $9$ degrees of freedom in a general linear transformation in $\mathbb R^3$ (one degree of freedom for each entry in a $3\times3$ matrix) to just $3$ degrees of freedom.

Because of the limited degrees of freedom of a rotation, you do not need as much information. It is sufficient to know what happens to just two vectors that are not linearly dependent. (That is, they cannot be collinear.) Even that will give you a lot of redundant information, because we already know that the lengths of the vectors will not change and the angle between them will not change.

I'm not sure how to exploit this extra knowledge in an algorithm that relies purely on matrix arithmetic. But there are certainly ways to extract the necessary information from the two matrices $$ \begin{bmatrix} P_{1x} & P_{2x} & P_{3x} \\ P_{1y} & P_{2y} & P_{3y} \\ P_{1z} & P_{2z} & P_{3z} \end{bmatrix} - \begin{bmatrix} t_x&t_x&t_x \\ t_y&t_y&t_y \\ t_z&t_z&t_z \end{bmatrix} \quad\text{and}\quad \begin{bmatrix} p_{1x} & p_{2x} & p_{3x} \\ p_{1y} & p_{2y} & p_{3y} \\ p_{1z} & p_{2z} & p_{3z} \end{bmatrix}.$$ In fact you should only need the first two columns of each matrix. The answer to Calculating rotation for a pair of unit vectors given initial and final states uses the two "before rotation" vectors to generate an orthogonal basis, and likewise with the two "after rotation" vectors, and computes the rotation from one basis to the other (for which the matrix inversion method will work, since the three basis vectors are linearly independent). Or you could consider G Cab's answer, which starts with just a little three-dimensional vector arithmetic to set up a matrix-arithmetic solution.

Answer 4

One of the most robust ways of finding the rotation matrix would probably be by using Wahba's problem, which in your case has the following associated cost function

$$ J(R) = \frac{1}{2} \sum_{k=1}^3 a_k \| P_k - t_k - R\,p_k\|^2, \tag{1} $$

with $a_k > 0$ weights (but if you don't have any uncertainty in $P_k,t_k,p_k\in\mathbb{R}^3$ you could just set those to one). This cost function will have one global minima with $R\in SO(3)$ if the vectors $P_k-t_k$ or $p_k$ span at least a plane (which would be the case for your example).

The minimum of the cost function from $(1)$ can be found using

$$ B = \sum_{k=1}^3 a_k (P_k - t_k)\,p_k^\top. \tag{2} $$

Now find the singular value decomposition of $B$ such that

$$ B = U\,S\,V^\top, \tag{3} $$

where $U$ and $V$ are unitary matrices and $S$ a diagonal matrix. The rotation matrix $R$ that minimizes $(1)$ can be obtained using

$$ R = U \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & \det(U)\det(V) \end{bmatrix} V^\top. \tag{4} $$