How to calculate quaternion of a plane from the world position of $3$ points on a plane

208 Views Asked by At

enter image description here

As in the picture, within the parent plane there are $3$ points, red, green and blue. The purple and gold represents two objects between these $3$ points. Assume I’ve observed the world positions of the $3$ points before and after the rotation of the plane. How can I calculate the quaternion/euler angles based on the positions of the $3$ points for the plane? Furthermore, assume there are only the purple and gold objects and the $3$ points. How to calculate the translation and quaternions for the two objects based on the positions of the $3$ points?

1

There are 1 best solutions below

0
On

This is a common 3D problem: how to rotate three collinear points so that when the center one is at origin, one is at positive $x$ axis, and the last one as close to the positive $y$ axis as possible.


You have three points, $R$, $G$, and $B$, such that $\overline{GR} \perp \overline{GB}$, i.e. angle $\angle RGB = 90°$.

Let vector $\vec{u} = \overline{GR} = R - G$ and $\vec{v} = \overline{GB} = B - G$, and normalize them to unit length, $$\hat{u} = \frac{\vec{u}}{\lVert\vec{u}\rVert}, \quad \hat{v} = \frac{\vec{v}}{\lVert\vec{v}\rVert}$$ If the three really do form a right angle and you scaled them to length 1 correctly, then $$\left\lbrace ~ \begin{aligned} \hat{u} \cdot \hat{u} &= 1 \\ \hat{u} \cdot \hat{v} &= 0 \\ \hat{v} \cdot \hat{u} &= 0 \\ \hat{v} \cdot \hat{v} &= 1 \\ \end{aligned} \right . \tag{1} \label{1}$$ If the original points are not at a perfect right angle, then apply one step of the Gram–Schmidt process to orthogonalize $\hat{v}$ with respect to $\hat{u}$: $$\vec{u}^\prime = \vec{v} - \hat{u}\left(\vec{v} \cdot \hat{u}\right), \quad \hat{v} = \frac{\vec{u}^\prime}{\left\lVert\vec{u}^\prime\right\rVert}$$ so that as long as original $\vec{v}$ is not parallel to $\vec{u}$, you get a pair of orthonormal vectors that fulfill $\eqref{1}$.

Next, we can take their cross product to get the third basis unit vector $\hat{w}$, $$\hat{w} = \hat{u} \times \hat{v} \tag{2}\label{2}$$ At this point, we should have three orthogonal unit vectors, i.e. $$\begin{aligned} \hat{u} \cdot \hat{u} &= 1 \\ \hat{u} \cdot \hat{v} &= 0 \\ \hat{u} \cdot \hat{w} &= 0 \\ \end{aligned}, ~ ~ \begin{aligned} \hat{v} \cdot \hat{u} &= 0 \\ \hat{v} \cdot \hat{v} &= 1 \\ \hat{v} \cdot \hat{w} &= 0 \\ \end{aligned}, ~ ~ \begin{aligned} \hat{w} \cdot \hat{u} &= 0 \\ \hat{w} \cdot \hat{v} &= 0 \\ \hat{w} \cdot \hat{w} &= 1 \\ \end{aligned}, ~ ~ \begin{aligned} \hat{u} \times \hat{u} &= \vec{0} \\ \hat{u} \times \hat{v} &= \hat{w} \\ \hat{u} \times \hat{w} &= -\hat{v} \\ \end{aligned}, ~ ~ \begin{aligned} \hat{v} \times \hat{u} &= -\hat{w} \\ \hat{v} \times \hat{v} &= \vec{0} \\ \hat{v} \times \hat{w} &= \hat{u} \\ \end{aligned}, ~ ~ \begin{aligned} \hat{w} \times \hat{u} &= \hat{v} \\ \hat{w} \times \hat{v} &= -\hat{u} \\ \hat{w} \times \hat{w} &= \vec{0} \\ \end{aligned}$$ which therefore form a 3D coordinate system basis.

The rotation matrix $\mathbf{R}$ that rotates the coordinate system so that unit $x$ axis matches $\hat{u}$, unit $y$ axis matches $\hat{v}$, and unit $z$ axis matches $\hat{w}$, is $$\mathbf{R} = \left[ \begin{matrix} \hat{u} & \hat{v} & \hat{w} \end{matrix} \right ] = \left[ \begin{matrix} u_x & v_x & w_x \\ u_y & v_y & w_y \\ u_z & v_z & w_z \\ \end{matrix} \right] \tag{3}\label{3}$$ Because this matrix is a pure rotation, it is orthonormal, and its inverse is its transpose.

This means that the matrix that rotates $\overline{GR}$ to parallel to $x$ axis, and $\overline{GB}$ to parallel to $y$ axis (or as close as is possible), is $\mathbf{R}^{-1}$, $$\mathbf{R}^{-1} = \mathbf{R}^T = \left[ \begin{matrix} u_x & u_y & u_z \\ v_x & v_y & v_z \\ w_x & w_y & w_z \\ \end{matrix} \right] \tag{4}\label{4}$$

Wikipedia article on rotation formalisms in three dimensions describes some approaches for converting the above orthogonal matrix to different formalisms.

The one I use, is unit quaternions (versors) or bivectors (which are the same in component-wise form). Essentially, let the versor be $\mathbf{q}$ and the rotation matrix $\mathbf{A}$, $$\mathbf{q} = (r ; i , j , k), \quad \mathbf{A} = \left[ \begin{matrix} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & a_{23} \\ a_{31} & a_{32} & a_{33} \\ \end{matrix} \right ]$$ such that if $(x, y, z)$ is an unit ($x^2+y^2+z^2=1$) axis vector, and $\varphi$ a rotation around that vector, then $$\mathbf{q} = \left\lbrace ~ \begin{aligned} r &= \cos\left(\frac{\varphi}{2}\right) \\ i &= x \sin\left(\frac{\varphi}{2}\right) \\ j &= y \sin\left(\frac{\varphi}{2}\right) \\ k &= z \sin\left(\frac{\varphi}{2}\right) \\ \end{aligned} \right .$$ describes that rotation, with $r^2 + i^2 + j^2 + k^2 = 1$ always (you can enforce this by dividing each component by $\sqrt{r^2 + i^2 + j^2 + k^2}$, without causing any directional or rotational bias). No rotation is $(1; 0, 0, 0)$, i.e. $r = 1$, and $i = j = k = 0$.

With $\mathbf{A} = \mathbf{R}$, you obtain the orientation of the three points with respect to coordinate axes. With $\mathbf{A} = \mathbf{R}^{-1}$, you obtain the rotation that brings the point $R$ to the positive $x$ axis and point $B$ to the positive $y$ axis or as close as is possible, when the center one ($G$) is moved to origin.

When you have two different orientations $\mathbf{R}_1$ and $\mathbf{R}_2$, then $\mathbf{A} = \mathbf{R}_2 \mathbf{R}_1^{-1}$ describes the rotation from $\mathbf{R}_1$ to $\mathbf{R}_2$, and $\mathbf{A} = \mathbf{R}_1 \mathbf{R}_2^{-1}$ the rotation from $\mathbf{R}_2$ to $\mathbf{R}_1$. (So, all of OP's questions are actually answered by this method, even though it might have originally looked that only a fraction of the question is considered.)

Note that versors $(r; i, j, k)$ and $(-r; -i, -j, -k)$ specify the exact same orientation. As rotations, they result in the same orientation, but the rotation angle is the other way, essentially $\varphi$ and $360°-\varphi$. If you want to ensure the rotation is through the smaller angle, just make sure $r$ is nonnegative: if it is negative, negate all four components.

Conversion from versor to rotation matrix is $$\mathbf{A} = \left[\begin{matrix} 1 - 2 ( j^2 + k^2 ) & 2 ( i j - k r ) & 2 ( i k + j r ) \\ 2 ( i j + k r ) & 1 - 2 ( i^2 + k^2 ) & 2 ( j k - i r ) \\ 2 ( i k - j r ) & 2 ( j k + i r ) & 1 - 2 ( i^2 + j^2 ) \\ \end{matrix}\right] \tag{5}\label{5}$$

For the inverse, we get best numerical stability and accuracy if we calculate $$\begin{aligned} s_i &= a_{11} - a_{22} - a_{33} \\ s_j &= a_{22} - a_{11} - a_{33} \\ s_k &= a_{33} - a_{11} - a_{22} \\ \end{aligned} \tag{6a}\label{6a}$$ and pick the solution based on which one of the three is largest:

  • If $s_i$ is the largest: $$\left\lbrace ~ \begin{aligned} i &= \frac{1}{2} \sqrt{1 + s_i} \\ r &= \frac{1}{4 i} \left( a_{32} - a_{23} \right) \\ j &= \frac{1}{4 i} \left( a_{21} + a_{12} \right) \\ k &= \frac{1}{4 i} \left( a_{31} + a_{13} \right) \\ \end{aligned} \right . \tag{6b}\label{6b}$$

  • If $s_j$ is the largest: $$\left\lbrace ~ \begin{aligned} j &= \frac{1}{2} \sqrt{1 + s_j} \\ r &= \frac{1}{4 j} \left( a_{13} - a_{31} \right) \\ i &= \frac{1}{4 j} \left( a_{12} + a_{21} \right) \\ k &= \frac{1}{4 j} \left( a_{23} + a_{32} \right) \\ \end{aligned} \right . \tag{6c}\label{6c}$$

  • If $s_k$ is the largest: $$\left\lbrace ~ \begin{aligned} k &= \frac{1}{2} \sqrt{1 + s_k} \\ r &= \frac{1}{4 k} \left( a_{21} - a_{12} \right) \\ i &= \frac{1}{4 k} \left( a_{13} + a_{31} \right) \\ j &= \frac{1}{4 k} \left( a_{23} + a_{32} \right) \\ \end{aligned} \right . \tag{6d}\label{6d}$$

You may wish to negate all four component afterwards, if $r$ is negative, to ensure the rotation uses the smaller angle of $\varphi$ and $360°-\varphi$. Again, this does not affect the orientation, only which way the rotation is done, through angle $\varphi$ or $360°-\varphi$. When $r$ is zero or positive, the rotation angle is minimized, the smaller of the two.