# Rotation and translation parameters from two 3D vectors

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 ?

• You mean Euler angles? The order of rotations matters, and you might have gimbal lock occur, depending on the order of rotation axes (you choose). Painful. I just use the rotation matrix and a versor (unit quaternion representing the rotation). It is simple to convert between the two. – Nominal Animal Jul 1 '16 at 15:18
• Also see Conversion formulae between different ways to achieve 3D rotation, at Wikipedia. – Nominal Animal Jul 1 '16 at 15:20
• Given a triplet of Euler angles, and their order (say, first angle is around the $z$ axis, and is applied first; second angle is around the $y$ axis, and is applied second; and third angle is around the $x$ axis, and is applied last), any single rotation or orientation can be described mathematically exactly. Unfortunately, there is no one order of the angles used, and there are dozens of possible orders, several in common use; and all the formulas depend on that order. "Euler angles" are not "one definition", but actually a family of related definitions. Which order you intend to use? – Nominal Animal Jul 4 '16 at 9:13
• If the two sets are ordered, so that $\vec{q}_i = \mathbf{R} \vec{p}_i + \vec{T}$, where $\vec{T}$ is the translation vector, and $\mathbf{R}$ is the rotation matrix, and there is not much noise in the data, then a simple linear least squares fitting will work. (Translation vector is often assumed to be $\vec{T} = \left ( 1/N \sum_i^N \vec{q}_i \right ) - \left ( 1/N \sum_i^N \vec{p}_i \right )$, i.e. additional displacements (noise) are assumed to cancel out, but you can trivially include the vector in the fitting also.) – Nominal Animal Jul 4 '16 at 14:45
• For unordered point sets, this becomes an optimization problem, and is much slower. If you are willing to dedicate enough computational resources, it is certainly doable; usually, it is easier to ensure the point sets are ordered. (In fact, the underlying problem reminds me of one that has vexed me quite a bit in molecular dynamics simulations: when using simple temperature controls, clusters start rotating. Fast, physically explainable cancellation of the rotation is similar to your problem, I think. Most computational physicists just use a different temperature control mechanism, though.) – Nominal Animal Jul 4 '16 at 14:52

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

• So that the $\theta x,y,z$ value correspond to Euler extrinsic rotations in x,y,z order, along the axes of the reference frame ? – Blue_Elephant Jul 20 '16 at 19:15
• @Blue_Elephant: Yes, exactly. Extrinsic, because the rotations are applied to the reference frame. That order, because the first rotation applied is around the $x$ axis of the reference frame, and last rotation is around the $z$ axis of the reference frame. Thus, $\mathbf{R} = \mathbf{R}_z \mathbf{R}_y \mathbf{R}_x$ ($= \mathbf{R}_z ( \mathbf{R}_y \mathbf{R}_x )$), because our vectors are column vectors, and we apply the rotation via pre-multiplication: $\vec{p}' = \mathbf{R}\vec{p}$. Have I mentioned I find Euler angles terribly confusing? :) – Nominal Animal Jul 20 '16 at 22:34
• Thank you so much for everything ! – Blue_Elephant Jul 21 '16 at 16:28