I have two sets of two points in 3D in the same reference frame. The two of them define two vectors.
I would like to know if it is possible to fully retrieve the three parameters that define the translation and the three others that define rotation according to the reference frame with only these 4 points ?
For example, I would like to be able to say: the first vector rotates that much around z-axis, that much around x-axis etc. With the same going for translation.
I've certainly heard it is possible to get the rotation matrix from the two vectors, but it does not give me access to the information I just mentioned (that much around such axis…), doesn't it ?
Best Answer
Let us assume you have points $\vec{a}_1$ and $\vec{b}_1$ in the first coordinate system, and $\vec{a}_2$ and $\vec{b}_2$ in the second coordinate system. Let us also assume that the coordinate systems are not scaled; only rotated and translated. If the points define a line segment, it is from $\vec{a}_1$ to $\vec{b}_1$ in the first coordinate system, and from $\vec{a}_2$ to $\vec{b}_2$ in the second coordinate system.
Let us choose the rotation to center on points $\vec{a}$ in their respective coordinate systems; that is, we translate by $-\vec{a}_1$ before rotation, and by $\vec{a}_1$ after the rotation, and we only rotate $\vec{b}$ around $\vec{a}$.
(You could choose different centers for the rotations, of course. A typical case for many points is to rotate the point cloud around its centroid. For two points, it is much simpler to just pick one point as the rotation origin.)
We can find the axis of the rotation and the sine of the rotation angle using vector cross product. In this case, $$\hat{r}\sin\varphi = \vec{c} = \frac{\left ( \vec{b}_1 - \vec{a}_1 \right ) \times \left ( \vec{b}_2 - \vec{a}_2 \right )}{\left\lVert \vec{b}_1 - \vec{a}_1 \right\rVert \; \left\lVert \vec{b}_2 - \vec{a}_2 \right\rVert}$$ where $\hat{r}$ is an unit vector, $r_x^2 + r_y^2 + r_z^2 = 1$. First, calculate $\vec{c}$, and then its norm: $$\lVert \vec{c} \rVert = \sqrt{c_x^2 + c_y^2 + c_z^2}$$ We'll also need the sign of $\cos\varphi$ to determine the correct quadrant for $\varphi$. $\cos\varphi$ has the same sign (and zeroes) as $$d = \left ( \vec{b}_1 - \vec{a}_1 \right ) \cdot \left ( \vec{b}_2 - \vec{a}_2 \right )$$ Then, $$\begin{cases} \vec{r} = \frac{\vec{c}}{\lVert \vec{c} \rVert}, & \varphi = \operatorname{asin}\left(\frac{1}{\lVert \vec{c} \rVert}\right), & d \ge 0 \\ \vec{r} = -\frac{\vec{c}}{\lVert \vec{c} \rVert}, & \varphi = -\operatorname{asin}\left(\frac{1}{\lVert \vec{c} \rVert}\right), & d \lt 0 \end{cases}$$
This axis-angle representation corresponds to a versor, or unit quaternion, $\mathbf{q}$, that represents the same rotation: $$\mathbf{q} = \cos\left(\frac{\varphi}{2}\right) + \sin\left(\frac{\varphi}{2}\right) r_x \mathbf{i} + \sin\left(\frac{\varphi}{2}\right) r_y \mathbf{j} + + \sin\left(\frac{\varphi}{2}\right) r_z \mathbf{k}$$ Typically, the components are referred to as $(w,x,y,z)$ or $(r,x,y,z)$ or $(a,b,c,d)$, in that order (scalar component of quaternion first); in other words, $$\begin{cases} q_w = q_a = q_0 = \cos\left(\frac{\varphi}{2}\right) \\ q_x = q_b = q_1 = \sin\left(\frac{\varphi}{2}\right) r_x \\ q_y = q_c = q_2 = \sin\left(\frac{\varphi}{2}\right) r_y \\ q_z = q_d = q_3 = \sin\left(\frac{\varphi}{2}\right) r_z \end{cases}$$
The Wikipedia page for Conversion between quaternions and Euler angles shows how to convert this to Euler angles, if we rotate around $x$ axis first, then around $y$ axis, and finally around $z$ axis:
$$\begin{cases} \theta_X = \operatorname{atan2}\left( 2 ( q_w q_x + q_y q_z ) , 1 − 2 ( q_x^2 + q_y^2 ) \right) \\ \theta_Y = \operatorname{asin}\left( 2 ( q_w q_y − q_z q_x ) \right) \\ \theta_Z = \operatorname{atan2}\left( 2 ( q_w q_z + q_x q_y ) , 1 − 2 ( q_y^2 + q_z^2 ) \right) \end{cases}$$
For completeness, the rotation matrix using this order is (also shown on the Wikipedia Conversion between quaternions and Euler angles page): $$\mathbf{R} = \left[\begin{matrix} C_Z C_Y & -S_Z C_X + C_Z S_Y S_X & S_Z S_X + C_Z S_Y C_X \\ S_Z C_Y & C_Z C_X + S_Z S_Y S_X & -C_Z S_X + S_Z S_Y C_X \\ -S_Y & C_Y S_X & C_Y C_X \end{matrix}\right]$$ where $$\begin{matrix} C_X = \cos \theta_X & S_X = \sin \theta_X \\ C_Y = \cos \theta_Y & S_Y = \sin \theta_Y \\ C_Z = \cos \theta_Z & S_Z = \sin \theta_Z \\ \end{matrix}$$ to keep the notation simple.
The transformation as defined above involves a translation before and after rotation. We can combine them to the usual post-rotation translation by applying the inverse rotation to the pre-rotation translation, and adding the post-rotation translation.
We translate by $-\vec{a}_1$ before the rotation, and $\vec{a}_1$ after the rotation, so we need to apply the inverse rotation to the pre-rotation translation to get the combined post-rotation translation: $$\vec{t} = \vec{a}_1 + \mathbf{R}^{-1}(-\vec{a}_1) = \vec{a}_1 - \mathbf{R}^T \vec{a}_1$$ Note that $\mathbf{R}^{-1} = \mathbf{R}^{T}$ because pure rotation matrices like $\mathbf{R}$ here are orthogonal.
If $\vec{a}_1 = (x, y, z)$ and $\vec{t} = (t_x, t_y, t_z)$, then $$\begin{cases} t_x = x (1 - C_Z C_Y) - y (S_Z C_Y) + z S_Y \\ t_y = x (S_Z C_X - C_Z S_Y S_X) + y (1 - C_Z C_X - S_Z S_Y S_X) - z (C_Y S_X) \\ t_z = -x (S_Z S_X + C_Z S_Y C_X) + y (C_Z S_X - S_Z S_Y C_X) + z (1 - C_Y C_X) \end{cases}$$
Separating the translation to pre- and post-rotation parts, and then combining the two, as above, is very often useful: it tends to make the problems simpler to solve, as you can basically freely choose the center of the rotation in the two coordinate systems, work out the rotation that way, and finally use the transpose of the rotation matrix to convert the pre-rotation translation to post-rotation translation. (Just remember to multiply the transpose of the rotation matrix $\mathbf{R}$ and the pre-rotation translation vector.)