Euler Angles

Euler angles describe orientation of some rigid body by rotation around axes x, y, and z. Those angles are marked as

• φ: roll angle (around x-axis),

• θ: pitch angle (around y-axis), and

• ψ: yaw or heading angle (around z-axis).

There are 12 possible rotation combinations around axes x, y, and z [1]. The most often used is the combination 3–2–1, where the orientation of somebody coordinate frame according to the reference coordinate frame is obtained from the initial pose where both coordinate frames are aligned and then the body frame is rotated in the following order:

1. First is rotation around z-axis for yaw angle ψ,

2. then rotation around newly obtained y for a pitch angle θ, and

3. finally rotation around newly obtained x for a roll angle φ.

The DCM of this rotation transformation is

$\begin{array}{ll}\hfill & \mathit{R}={\mathit{R}}_x(\phi ){\mathit{R}}_y(\theta ){\mathit{R}}_z(\psi )\hfill \\ \hfill & =\left[\begin{array}{lll}\hfill cos\theta cos\psi \hfill & \hfill cos\theta sin\psi \hfill & \hfill -sin\theta \hfill \\ \hfill sin\phi sin\theta cos\psi -cos\phi sin\psi \hfill & \hfill sin\phi sin\theta sin\psi +cos\phi cos\psi \hfill & \hfill sin\phi cos\theta \hfill \\ \hfill cos\phi sin\theta cos\psi +sin\phi sin\psi \hfill & \hfill cos\phi sin\theta sin\psi -sin\phi cos\psi \hfill & \hfill cos\phi cos\theta \hfill \end{array}\right]\hfill \end{array}$

and the Euler presentation is as follows:

$\begin{array}{l}\hfill \begin{array}{cc}\phi & =arctan\left(\frac{R_{23}}{R_{33}}\right)\\ \theta & =-arcsin(R_{13})\\ \psi & =arctan\left(\frac{R_{12}}{R_{11}}\right)\end{array}\end{array}$

Parameterization using Euler angles is not redundant (three parameters for three degree of freedom). However, its main drawback is that it becomes singular at rotation θ = π/2 where rotation around z- and x-axes have the same effect (they coincide). This effect is known as Gimbal lock in classic airplane gyroscopes. In rotation simulation using Euler angles notation, this singularity appears due to division by $cos\theta$ (see Eq. 5.47 in Section 5.2.3).

Quaternions

Quaternions present orientation in 3D space using four parameters and one constraint equation without singularity problems such as in Euler angles presentation. Mathematically the quaternions are a noncommutative extension of the complex numbers defined by

$\begin{array}{l}\hfill \mathit{q}=q_0+q_1\mathit{i}+q_2\mathit{j}+q_3\mathit{k}\end{array}$

where complex elements i, j in k are related by expression i² =j² =k² = i j k = −1. q₀ is the scalar part of the quaternion and q₁i + q₂j + q₃k is termed the vector part. The quaternion norm is defined by

$\begin{array}{l}\hfill \parallel \mathit{q}\parallel =\sqrt{\mathit{q}{\mathit{q}}^{*}}=\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}\end{array}$

where q* is the conjugated quaternion obtained by q* = q₀ − q₁i − q₂j − q₃k. The inverse quaternion is calculated using its conjugate and norm value as follows:

$\begin{array}{l}\hfill {\mathit{q}}^{-1}=\frac{{\mathit{q}}^{*}}{\parallel \mathit{q}{\parallel }^2}\end{array}$

Rotation in space is parameterized using unit quaternions by

$\begin{array}{l}\hfill \begin{array}{cc}{q}_0& =cos\mathrm{\Delta }\phi /2\\ q_1& =e_{1}sin\mathrm{\Delta }\phi /2\\ q_2& =e_{2}sin\mathrm{\Delta }\phi /2\\ q_3& =e_{3}sin\mathrm{\Delta }\phi /2\end{array}\end{array}$

where vector e^T = [e₁, e₂, e₃] is the unit vector of the rotation axis and Δφ rotation angle around that axis. For unit quaternion it holds $q_0^2+q_1^2+q_2^2+q_3^2=1$.

Transformation

$\begin{array}{l}\hfill {\mathit{q}}_{v^{\prime }}={\mathit{q}}^{-1}\circ {\mathit{q}}_v\circ \mathit{q}\end{array}$

rotates vector v = [x, y, z]^T expressed by quaternion

$\begin{array}{l}\hfill {\mathit{q}}_v=x\mathit{i}+y\mathit{j}+z\mathit{k}\end{array}$

(or equivalently q_v = [0, x, y, z]^T) around axis e for angle Δφ to the vector v′ = [x′, y′, z′]^T expressed by quaternion

$\begin{array}{l}\hfill {\mathit{q}}_{v^{\prime }}=x^{\prime }\mathit{i}+y^{\prime }\mathit{j}+z^{\prime }\mathit{k}\end{array}$

where ∘ denotes the product of quaternions defined in Eqs. (5.16), (5.17).

An additional advantage of quaternions is the relatively easy combination of successive rotations. Product of two DCM can be equivalently written by the product of two quaternions [1]. Having two quaternions,

$\begin{array}{l}\hfill \mathit{q}=q_0+q_1\mathit{i}+q_2\mathit{j}+q_3\mathit{k}\end{array}$

and

$\begin{array}{l}\hfill {\mathit{q}}^{\prime }=q_0^{\prime }+q_1^{\prime }\mathit{i}+q_2^{\prime }\mathit{j}+q_3^{\prime }\mathit{k}\end{array}$

and rotating some vector from its initial orientation first by rotation defined by q′ and then for rotation defined by q, we obtain the following:

$\begin{array}{l}\hfill \begin{array}{cc}{\mathit{q}}^{″}={\mathit{q}}^{\prime }\circ \mathit{q}& =(q_0^{\prime }{q}_0-q_1^{\prime }{q}_1-q_2^{\prime }{q}_2-q_3^{\prime }{q}_3)\\ & +\mathit{i}(q_1^{\prime }{q}_0+q_2^{\prime }{q}_3-q_3^{\prime }{q}_2+q_0^{\prime }{q}_1)\\ & +\mathit{j}(-q_1^{\prime }{q}_3+q_2^{\prime }{q}_0+q_3^{\prime }{q}_1+q_0^{\prime }{q}_2)\\ & +\mathit{k}(q_1^{\prime }{q}_2-q_2^{\prime }{q}_1+q_3^{\prime }{q}_0+q_0^{\prime }{q}_3)\end{array}\end{array}$

or in vector-matrix form

$\begin{array}{l}\hfill \left[\begin{array}{l}\hfill q_0^{″}\hfill \\ \hfill q_1^{″}\hfill \\ \hfill q_2^{″}\hfill \\ \hfill q_3^{″}\hfill \end{array}\right]=\left[\begin{array}{llll}\hfill q_0\hfill & \hfill -q_1\hfill & \hfill -q_2\hfill & \hfill -q_3\hfill \\ \hfill q_1\hfill & \hfill q_0\hfill & \hfill q_3\hfill & \hfill -q_2\hfill \\ \hfill q_2\hfill & \hfill -q_3\hfill & \hfill q_0\hfill & \hfill q_1\hfill \\ \hfill q_3\hfill & \hfill q_2\hfill & \hfill -q_1\hfill & \hfill q_0\hfill \end{array}\right]\left[\begin{array}{l}\hfill q_0^{\prime }\hfill \\ \hfill q_1^{\prime }\hfill \\ \hfill q_2^{\prime }\hfill \\ \hfill q_3^{\prime }\hfill \end{array}\right]\end{array}$

The relation between the quaternion and DCM presentation is given by

$\begin{array}{l}\hfill \mathit{R}(\mathit{q})=\left[\begin{array}{lll}\hfill q_0^2+q_1^2-q_2^2-q_3^2\hfill & \hfill 2(q_0{q}_3+q_1{q}_2)\hfill & \hfill 2(q_1{q}_3-q_0{q}_2)\hfill \\ \hfill 2(q_1{q}_2-q_3{q}_0)\hfill & \hfill q_0^2-q_1^2+q_2^2-q_3^2\hfill & \hfill 2(q_0{q}_1+q_2{q}_3)\hfill \\ \hfill 2(q_0{q}_2+q_1{q}_3)\hfill & \hfill 2(q_2{q}_3-q_0{q}_1)\hfill & \hfill q_0^2-q_1^2-q_2^2+q_3^2\hfill \\ \hfill \hfill \end{array}\right]\end{array}$

or in opposite direction

$\begin{array}{ll}\hfill q_0& =\frac{1}{2}\sqrt{1+R_{11}+R_{22}+R_{33}}\hfill \end{array}$

$\begin{array}{ll}\hfill q_1& =\frac{1}{4q_0}\left(R_{23}-R_{32}\right)\hfill \end{array}$

$\begin{array}{ll}\hfill q_2& =\frac{1}{4q_0}\left(R_{31}-R_{13}\right)\hfill \end{array}$

$\begin{array}{ll}\hfill q_1& =\frac{1}{4q_0}\left(R_{12}-R_{21}\right)\hfill \end{array}$

If q₀ in Eq. (5.19) is close to zero then the transform from DCM to quaternion (relations 5.19–5.22) is singular. In this case we can calculate the quaternion using equivalent form (relations 5.23–5.26) without numerical problems:

$\begin{array}{ll}\hfill q_0& =\frac{1}{4T}\left(R_{32}-R_{23}\right)\hfill \end{array}$

$\begin{array}{ll}\hfill q_1& =T\hfill \end{array}$

$\begin{array}{ll}\hfill q_2& =\frac{1}{4T}\left(R_{12}+R_{21}\right)\hfill \end{array}$

$\begin{array}{ll}\hfill q_3& =\frac{1}{4T}\left(R_{13}+R_{31}\right)\hfill \end{array}$

where $T=\frac{1}{2}\sqrt{1+R_{11}-R_{22}-R_{33}}$

The relation between quaternions and Euler angles (notation 3–2–1) is obtained by matrices R_x(φ), R_y(θ), and R_z(ψ), which suits to quaternions $[cos(\phi /2)+\mathit{i}sin(\phi /2)]$, $[cos(\theta /2)+\mathit{j}sin(\theta /2)]$ in $[cos(\psi /2)+\mathit{k}sin(\psi /2)]$. The quaternion for rotation 3–2–1 is

$\begin{array}{l}\hfill \mathit{q}=[cos(\psi /2)+\mathit{k}sin(\psi /2)][cos(\theta /2)+\mathit{j}sin(\theta /2)][cos(\phi /2)+\mathit{i}sin(\phi /2)]\end{array}$

or in vector form

$\begin{array}{l}\hfill \mathit{q}=\left[\begin{array}{l}\hfill cos(\phi /2)cos(\theta /2)cos(\psi /2)+sin(\phi /2)sin(\theta /2)sin(\psi /2)\hfill \\ \hfill sin(\phi /2)cos(\theta /2)cos(\psi /2)-cos(\phi /2)sin(\theta /2)sin(\psi /2)\hfill \\ \hfill cos(\phi /2)sin(\theta /2)cos(\psi /2)+sin(\phi /2)cos(\theta /2)sin(\psi /2)\hfill \\ \hfill cos(\phi /2)cos(\theta /2)sin(\psi /2)-sin(\phi /2)sin(\theta /2)cos(\psi /2)\hfill \end{array}\right]\end{array}$

The opposite transformation is

$\begin{array}{ll}\hfill \phi & =arctan\left(\frac{2(q_1{q}_0+q_2{q}_3)}{q_0^2-q_1^2-q_2^2+q_3^2}\right)\hfill \end{array}$

$\begin{array}{ll}\hfill \theta & =-arcsin\left(2(q_1{q}_3-q_2{q}_0)\right)\hfill \end{array}$

$\begin{array}{ll}\hfill \psi & =arctan\left(\frac{2(q_3{q}_0+q_1{q}_2)}{q_0^2+q_1^2-q_2^2-q_3^2}\right)\hfill \end{array}$

Example 5.1

A sensor (magnetometer) coordinate system is rotated according to a robot coordinate system. Lets mark the sensor coordinate system as local (L) and the robot coordinate system as global (G). The orientation of the sensor according to the robot is described by two rotations. Suppose that initially (L) is aligned to (G) then rotating (L) around x-axis for α_x = 90 degree and finally around new y-axis for α_y = 45 degree. Answer the following:

1. What is the orientation of the sensor expressed by the DCM (${\mathit{R}}_G^L$) according to robot coordinates?

2. Determine Euler angles (notation 3–2–1) for this transformation.

3. Determine quaternion ${\mathit{q}}_G^L$ that describes this transformation.

4. The magnetometer measures direction vector v = [0, 0, 1]^T of the magnetic field. What is the presentation of this vector in robot coordinates?

Solution

1. Final orientation is described by the rotation matrix obtained from successive rotations where multiplication order is important.

$\begin{array}{l}\hfill \begin{array}{cc}{\mathit{R}}_G^L& ={\mathit{R}}_y({\alpha }_y){\mathit{R}}_x({\alpha }_x)\\ & =\left[\begin{array}{lll}\hfill cos({\alpha }_y)\hfill & \hfill 0\hfill & \hfill -sin({\alpha }_y)\hfill \\ \hfill 0\hfill & \hfill 1\hfill & \hfill 0\hfill \\ \hfill sin({\alpha }_y)\hfill & \hfill 0\hfill & \hfill cos({\alpha }_y)\hfill \end{array}\right]\left[\begin{array}{lll}\hfill 1\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill cos({\alpha }_x)\hfill & \hfill sin({\alpha }_x)\hfill \\ \hfill 0\hfill & \hfill -sin({\alpha }_x)\hfill & \hfill cos({\alpha }_x)\hfill \end{array}\right]\\ & =\left[\begin{array}{lll}\hfill 0.7071\hfill & \hfill 0.7071\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill \\ \hfill 0.7071\hfill & \hfill -0.7071\hfill & \hfill 0\hfill \end{array}\right]\end{array}\end{array}$

Rows of matrix ${\mathit{R}}_G^L$ are components of the sensor axis expressed in robot coordinates, which is seen from a graphical presentation of this rotation.

2. Euler angles for 3–2–1 notation are calculated from matrix $\mathit{R}={\mathit{R}}_G^L$:

$\begin{array}{ll}\hfill \phi & =arctan\left(\frac{R_{23}}{R_{33}}\right)=90\text{degree}\hfill \\ \hfill \theta & =-arcsin(R_{13})=-0\text{degree}\hfill \\ \hfill \psi & =arctan\left(\frac{R_{12}}{R_{11}}\right)=45\text{degree}\hfill \end{array}$

where the rotation matrix is obtained by ${\mathit{R}}_G^L={\mathit{R}}_x(\phi ){\mathit{R}}_y(\theta ){\mathit{R}}_z(\psi )$ and where elementary rotations R_x(φ), R_y(θ), and R_z(ψ) are defined in Eq. (5.5).

3. Quaternion ${\mathit{q}}_G^L$ is obtained by the product of quaternions for successive rotations:

$\begin{array}{l}\hfill {\mathit{q}}_G^L={\mathit{q}}_x\circ {\mathit{q}}_y\end{array}$

where q_x is according to Eq. (5.10) defined by rotation angle Δφ_x = 90 degree around rotation axis e_x = [1, 0, 0]^T and q_y with rotation angle Δφ_y = 45 degree around rotation axis e_y = [0, 1, 0]^T

$\begin{array}{l}\hfill {\mathit{q}}_x=\left[\begin{array}{l}\hfill cos\mathrm{\Delta }{\phi }_x/2\hfill \\ \hfill e_{x_1}sin\mathrm{\Delta }{\phi }_x/2\hfill \\ \hfill e_{x_2}sin\mathrm{\Delta }{\phi }_x/2\hfill \\ \hfill e_{x_3}sin\mathrm{\Delta }{\phi }_x/2\hfill \end{array}\right]=\left[\begin{array}{l}\hfill 0.7071\hfill \\ \hfill 0.7071\hfill \\ \hfill 0\hfill \\ \hfill 0\hfill \end{array}\right]\end{array}$

$\begin{array}{l}\hfill {\mathit{q}}_y=\left[\begin{array}{l}\hfill cos\mathrm{\Delta }{\phi }_z/2\hfill \\ \hfill e_{y_1}sin\mathrm{\Delta }{\phi }_y/2\hfill \\ \hfill e_{y_2}sin\mathrm{\Delta }{\phi }_y/2\hfill \\ \hfill e_{y_3}sin\mathrm{\Delta }{\phi }_y/2\hfill \end{array}\right]=\left[\begin{array}{l}\hfill 0.9239\hfill \\ \hfill 0\hfill \\ \hfill 0.3827\hfill \\ \hfill 0\hfill \end{array}\right]\end{array}$

The final quaternion (see quaternion product definition in Eq. 5.17) reads

$\begin{array}{l}\hfill {\mathit{q}}_G^L={\mathit{q}}_x\circ {\mathit{q}}_y=\left[\begin{array}{l}\hfill 0.6533\hfill \\ \hfill 0.6533\hfill \\ \hfill 0.2706\hfill \\ \hfill 0.2706\hfill \end{array}\right]\end{array}$

which suits to the rotation angle $\mathrm{\Delta }\phi =2arccos(q_0)=2arccos(0.6533)=98.41$ degree around the rotation axis $\mathit{e}=\frac{1}{sin\frac{\mathrm{\Delta }\phi }{2}}{[q_1,q_2,q_3]}^T={[0.8630,0.3574,0.3574]}^T$

4. The measured direction vector v_L = [0, 0, 1]^T is in robot coordinates:

$\begin{array}{l}\hfill {\mathit{v}}_G={\mathit{R}}_L^G{\mathit{v}}_L={[0.70711,-0.70711,0]}^T\end{array}$

where ${\mathit{R}}_L^G={({\mathit{R}}_G^L)}^{-1}={{\mathit{R}}_G^L}^T$.
The same is obtained by quaternions where the vector after rotation is

$\begin{array}{l}\hfill {\mathit{q}}_{v_G}={({\mathit{q}}_L^G)}^{-1}\circ {\mathit{q}}_{v_L}\circ {\mathit{q}}_L^G\end{array}$

where ${\mathit{q}}_{v_L}={[0,{\mathit{v}}_L^T]}^T$ and ${\mathit{q}}_L^G={({\mathit{q}}_G^L)}^{-1}$ (see Eq. 5.9). Products of quaternions are defined in Eq. (5.17). The solution is

$\begin{array}{l}\hfill {\mathit{q}}_{v_G}=\left[\begin{array}{l}\hfill 0.6533\hfill \\ \hfill 0.6533\hfill \\ \hfill 0.2706\hfill \\ \hfill 0.2706\hfill \end{array}\right]\circ \left[\begin{array}{l}\hfill 0\hfill \\ \hfill 0\hfill \\ \hfill 0\hfill \\ \hfill 1\hfill \end{array}\right]\circ \left[\begin{array}{l}\hfill 0.6533\hfill \\ \hfill -0.6533\hfill \\ \hfill -0.2706\hfill \\ \hfill -0.2706\hfill \end{array}\right]=\left[\begin{array}{l}\hfill 0\hfill \\ \hfill 0.7071\hfill \\ \hfill -0.7071\hfill \\ \hfill 0\hfill \end{array}\right]\end{array}$

which is vector v_G = [0.7071, −0.7071, 0]^T (only the vector part of quaternion is considered).

Example 5.2

Initially global coordinate frame G and local coordinate frame L are aligned; then frame L is rotated around x-axis for angle α_x = 90 degree and then again around newly obtained z-axis for angle α_z = 90 degree. Answer the following:

1. What is the orientation of frame L expressed by the DCM (${\mathit{R}}_G^L$) according to frame G?

2. Determine Euler angles (notation 3–2–1) for this transformation.

3. Determine quaternion ${\mathit{q}}_G^L$ that describes this transformation.

4. The vector in global coordinates is v_G = [0, 0, 1]^T. What is the presentation of this vector in local coordinates?

Solution

1. The DCM and graphical presentation for this rotation are

$\begin{array}{l}\hfill {\mathit{R}}_G^L=\left[\begin{array}{lll}\hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill \\ \hfill -1\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill -1\hfill & \hfill 0\hfill \end{array}\right]\end{array}$

2. Euler angles (3–2–1) are

$\begin{array}{ll}\hfill \phi & =arctan\left(\frac{R_{23}}{R_{33}}\right)=\text{undefined}\hfill \\ \hfill \theta & =-arcsin\left(R_{13}\right)=-90\text{degree}\hfill \\ \hfill \psi & =arctan\left(\frac{R_{12}}{R_{11}}\right)=\text{undefined}\hfill \end{array}$

Notice that θ = −90 degree, which means that parameterization using Euler angles is singular, and therefore φ and ψ are not defined. Using Euler angles this orientation (rotation) cannot be described.

3. Quaternion ${\mathit{q}}_G^L$ is

$\begin{array}{l}\hfill {\mathit{q}}_G^L={\mathit{q}}_x\circ {\mathit{q}}_z\end{array}$

where

$\begin{array}{ll}\hfill {\mathit{q}}_x& =\left[\begin{array}{l}\hfill 0.7071\hfill \\ \hfill 0.7071\hfill \\ \hfill 0\hfill \\ \hfill 0\hfill \end{array}\right]\hfill \\ \hfill {\mathit{q}}_z& =\left[\begin{array}{l}\hfill 0.7071\hfill \\ \hfill 0\hfill \\ \hfill 0\hfill \\ \hfill 0.7071\hfill \end{array}\right]\hfill \end{array}$

and

$\begin{array}{l}\hfill {\mathit{q}}_G^L={\mathit{q}}_x\circ {\mathit{q}}_z=\left[\begin{array}{l}\hfill 0.5\hfill \\ \hfill 0.5\hfill \\ \hfill -0.5\hfill \\ \hfill 0.5\hfill \end{array}\right]\end{array}$

which result in the rotation angle $\mathrm{\Delta }\phi =2arccos(0.5)=120$ degree around axis of rotation $\mathit{e}=\frac{1}{sin\frac{\mathrm{\Delta }\phi }{2}}{[q_1,q_2,q_3]}^T={[0.5774,-0.5774,0.5774]}^T$.

4. Vector v_G = [0, 0, 1]^T reads in local coordinates as

$\begin{array}{l}\hfill {\mathit{v}}_L={\mathit{R}}_G^L{\mathit{v}}_G={[1,0,0]}^T\end{array}$

or with quaternions

$\begin{array}{l}\hfill {\mathit{q}}_{v_L}={({\mathit{q}}_G^L)}^{-1}\circ {\mathit{q}}_{v_G}\circ {\mathit{q}}_G^L=\left[\begin{array}{l}\hfill 0\hfill \\ \hfill 1\hfill \\ \hfill 0\hfill \\ \hfill 0\hfill \end{array}\right]\end{array}$

where the vector part is v_L = [1, 0, 0]^T.

Rotational Kinematics Expressed by Quaternions

Time dependency of rigid body rotation (given by differential equation) can be derived from the product definition of two quaternions (5.17). If orientation q(t) of the rigid body at time t is known then its orientation in time q(t + Δt) can be written as

$\begin{array}{l}\hfill \mathit{q}(t+\mathrm{\Delta }t)=\mathit{q}(t)\circ \mathrm{\Delta }\mathit{q}(t)\end{array}$

where Δq(t) defined the change of the rigid body orientation from q(t) to q(t + Δt). In other words Δq(t) is the orientation of the body at time t + Δt relative to its orientation at time t. The final orientation of the body q(t + Δt) is therefore obtained by first rotating the body according to some reference frame for rotation q(t), and then also for rotation Δq(t) according to q(t). Δq(t) is defined by relation (5.10)

$\begin{array}{l}\hfill \mathrm{\Delta }\mathit{q}(t)=\left[\begin{array}{l}\hfill cos\mathrm{\Delta }\phi /2\hfill \\ \hfill e_{x}sin\mathrm{\Delta }\phi /2\hfill \\ \hfill e_{y}sin\mathrm{\Delta }\phi /2\hfill \\ \hfill e_{z}sin\mathrm{\Delta }\phi /2\hfill \end{array}\right]\end{array}$

where e(t) = [e_x, e_y, e_z]^T is the rotation axis expressed in rigid body local coordinates at time t and Δφ is the rotation angle during time period Δt. Assuming e(t) and Δφ are constant during the period Δt, the product of quaternions (5.34) can be reformulated using definition (5.17) as follows:

$\begin{array}{l}\hfill \mathit{q}(t+\mathrm{\Delta }t)=\left(cos\left(\frac{\mathrm{\Delta }\phi }{2}\right)\mathit{I}+sin\left(\frac{\mathrm{\Delta }\phi }{2}\right)\left[\begin{array}{llll}\hfill 0\hfill & \hfill -e_x\hfill & \hfill -e_y\hfill & \hfill -e_z\hfill \\ \hfill e_x\hfill & \hfill 0\hfill & \hfill e_z\hfill & \hfill -e_y\hfill \\ \hfill e_y\hfill & \hfill -e_z\hfill & \hfill 0\hfill & \hfill e_x\hfill \\ \hfill e_z\hfill & \hfill e_y\hfill & \hfill -e_x\hfill & \hfill 0\hfill \end{array}\right]\right)\mathit{q}(t)\end{array}$

where I is a 4 × 4 identity matrix. For a short interval Δt we can approximate $\mathrm{\Delta }\phi \approx \sqrt{{{\omega }_x}^2+{{\omega }_y}^2+{{\omega }_z}^2}\mathrm{\Delta }t$ where ω(t) = [ω_x, ω_y, ω_z]^T is the vector of current angular rates, which can also be written in the form $\mathit{\omega }(t)=\sqrt{{{\omega }_x}^2+{{\omega }_y}^2+{{\omega }_z}^2}\mathit{e}$. For small angles, Eq. (5.36) can be approximated by

$\begin{array}{l}\hfill \mathit{q}(t+\mathrm{\Delta }t)\approx \left(\mathit{I}+\frac{\mathrm{\Delta }t\mathbf{\Omega }}{2}\right)\mathit{q}(t)\end{array}$

where

$\begin{array}{l}\hfill \mathbf{\Omega }=\left[\begin{array}{llll}\hfill 0\hfill & \hfill -{\omega }_x\hfill & \hfill -{\omega }_y\hfill & \hfill -{\omega }_z\hfill \\ \hfill {\omega }_x\hfill & \hfill 0\hfill & \hfill {\omega }_z\hfill & \hfill -{\omega }_y\hfill \\ \hfill {\omega }_y\hfill & \hfill -{\omega }_z\hfill & \hfill 0\hfill & \hfill {\omega }_x\hfill \\ \hfill {\omega }_z\hfill & \hfill {\omega }_y\hfill & \hfill -{\omega }_x\hfill & \hfill 0\hfill \end{array}\right]\end{array}$

A differential equation that describes rigid body orientation is obtained by limiting Δt toward zero:

$\begin{array}{l}\hfill \frac{d\mathit{q}}{dt}=\underset{\mathrm{\Delta }t\to 0}{lim}\frac{\mathit{q}(t+\mathrm{\Delta }t)-\mathit{q}(t)}{\mathrm{\Delta }t}=\frac{1}{2}\mathbf{\Omega }\mathit{q}\end{array}$

where angular rates in Ω are given in the rigid body coordinates.

Rotational Kinematics Expressed by DCM

The differential equation for rigid body orientation presentation given by the DCM is derived in the following. Defining similarly as in Eq. (5.34),

$\begin{array}{l}\hfill \mathit{R}(t+\mathrm{\Delta }t)=\mathrm{\Delta }\mathit{R}(t)\mathit{R}(t)\end{array}$

where R(t) is the orientation of the rigid body at time t, R(t + Δt) is the orientation at time t + Δt, and ΔR(t) is the change of orientation (orientation of the body at time t = t + Δt) according to the orientation at time t.

Change of orientation ΔR(t) is defined as

$\begin{array}{l}\hfill \mathrm{\Delta }\mathit{R}(t)=e^{{\int }_t^{t+\mathrm{\Delta }t}{\mathbf{\Omega }}^{\prime }dt}\end{array}$

where

$\begin{array}{l}\hfill {\mathbf{\Omega }}^{\prime }=\left[\begin{array}{lll}\hfill 0\hfill & \hfill {\omega }_z\hfill & \hfill -{\omega }_y\hfill \\ \hfill -{\omega }_z\hfill & \hfill 0\hfill & \hfill {\omega }_x\hfill \\ \hfill {\omega }_y\hfill & \hfill -{\omega }_x\hfill & \hfill 0\hfill \end{array}\right]\end{array}$

and ω(t) = [ω_x, ω_y, ω_z]^T is the vector of the current angular rates of the body.

Assuming Ω′ is a constant matrix in time period Δt we can approximate ${\int }_t^{t+\mathrm{\Delta }t}{\mathbf{\Omega }}^{\prime }(t)dt\approx {\mathbf{\Omega }}^{\prime }\mathrm{\Delta }t=\mathit{B}$. The exponent in relation (5.41) written in the Taylor series becomes

$\begin{array}{l}\hfill \begin{array}{cc}\mathrm{\Delta }\mathit{R}(t)& =e^{\mathit{B}}\\ & =\left(\mathit{I}+\mathit{B}+\frac{{\mathit{B}}^2}{2!}+\frac{{\mathit{B}}^3}{3!}+\cdots \right)\\ & =\left(\mathit{I}+\mathit{B}+\frac{{\mathit{B}}^2}{2!}-\frac{{\sigma }^2\mathit{B}}{3!}-\frac{{\sigma }^2{\mathit{B}}^2}{4!}+\frac{{\sigma }^4\mathit{B}}{5!}+\cdots \right)\\ & =\left(\mathit{I}+\frac{sin\sigma }{\sigma }\mathit{B}+\frac{1-cos\sigma }{{\sigma }^2}{\mathit{B}}^2\right)\end{array}\end{array}$

where I is the 3 × 3 identity matrix and $\sigma =\mathrm{\Delta }t\sqrt{{{\omega }_x}^2+{{\omega }_y}^2+{{\omega }_z}^2}$. For small angles σ relation (5.43) approximates to

$\begin{array}{l}\hfill \mathrm{\Delta }\mathit{R}(t)=\mathit{I}+\mathit{B}=\mathit{I}+{\mathbf{\Omega }}^{\prime }\mathrm{\Delta }t\end{array}$

which can be used to predict rotation matrix (5.40):

$\begin{array}{l}\hfill \mathit{R}(t+\mathrm{\Delta }t)=\left(\mathit{I}+{\mathbf{\Omega }}^{\prime }\mathrm{\Delta }t\right)\mathit{R}(t)\end{array}$

The final differential equation is obtained by limiting Δt to zero:

$\begin{array}{l}\hfill \frac{d\mathit{R}}{dt}=\underset{\mathrm{\Delta }t\to 0}{lim}\frac{\mathit{R}(t+\mathrm{\Delta }t)-\mathit{R}(t)}{\mathrm{\Delta }t}={\mathbf{\Omega }}^{\prime }\mathit{R}\end{array}$

For the sake of completeness, also the equivalent presentation of rotation parameterization using Euler angles (notation 3–2–1) is given:

$\begin{array}{l}\hfill \begin{array}{cc}\stackrel{.}{\phi }& ={\omega }_x+{\omega }_{y}sin\phi tan\theta +{\omega }_{z}cos\phi tan\theta \\ \stackrel{.}{\theta }& ={\omega }_{y}cos\phi -{\omega }_{z}sin\phi \\ \stackrel{.}{\mathrm{\Psi }}& =\frac{{\omega }_{y}sin\phi +{\omega }_{z}cos\phi }{cos\theta }\end{array}\end{array}$

where it can be seen that the notation becomes singular (the first end the third equation of Eq. (5.47) at θ = ±π/2).

Multiview Geometry

Multiview geometry is not important only because it enables scene reconstruction but some of the properties can be exploited in the design of machine vision algorithms (e.g., image point matching and image-based camera pose estimation). Consider that the rotation matrix ${\mathit{R}}_{C_2}^{C_1}$ and translation vector ${\mathit{t}}_{C_2}^{C_1}$ describe the relative pose between two cameras (Fig. 5.3). Under the assumption that camera centers do not coincide (${\mathit{t}}_{C_2}^{C_1}\ne 0$), the relation (5.52) can be obtained after a short mathematical manipulation (cameras with identical internal parameters are assumed):

$\begin{array}{ll}\hfill {\mathit{p}}_{C_1}& ={\mathit{R}}_{C_2}^{C_1}{\mathit{p}}_{C_2}+{\mathit{t}}_{C_2}^{C_1},\hfill \end{array}$

$\begin{array}{ll}\hfill {\mathit{S}}^{-1}{\underset{\_}{\mathit{p}}}_{P_1}& \propto {\mathit{R}}_{C_2}^{C_1}{\mathit{S}}^{-1}{\underset{\_}{\mathit{p}}}_{P_2}+{\mathit{t}}_{C_2}^{C_1}\hfill \\ \hfill {\left[{\mathit{t}}_{C_2}^{C_1}\right]}_{\times }{\mathit{S}}^{-1}{\underset{\_}{\mathit{p}}}_{P_1}& \propto {\left[{\mathit{t}}_{C_2}^{C_1}\right]}_{\times }{\mathit{R}}_{C_2}^{C_1}{\mathit{S}}^{-1}{\underset{\_}{\mathit{p}}}_{P_2}\hfill \\ \hfill 0& ={\underset{\_}{\mathit{p}}}_{P_1}^T{\mathit{S}}^{-T}{\left[{\mathit{t}}_{C_2}^{C_1}\right]}_{\times }{\mathit{R}}_{C_2}^{C_1}{\mathit{S}}^{-1}{\underset{\_}{\mathit{p}}}_{P_2}\hfill \\ \hfill 0& ={\underset{\_}{\mathit{p}}}_{P_1}^T\mathit{F}{\underset{\_}{\mathit{p}}}_{P_2}\hfill \end{array}$

Notice that the cross product between the vectors a^T = [a₁a₂a₃] and b^T = [b₁b₂b₃] is written as a ×b = [a]_×b, where [a]_× is a skew-symmetric matrix:

$\begin{array}{l}\hfill {[\mathit{a}]}_{\times }=\left[\begin{array}{lll}\hfill 0\hfill & \hfill -a_3\hfill & \hfill a_2\hfill \\ \hfill a_3\hfill & \hfill 0\hfill & \hfill -a_1\hfill \\ \hfill -a_2\hfill & \hfill a_1\hfill & \hfill 0\hfill \end{array}\right]\end{array}$

The matrix F is known as a fundamental matrix that describes the epipolar constraint (5.52): the point ${\underset{\_}{\mathit{p}}}_{P_1}$ is on the line $\mathit{F}{\underset{\_}{\mathit{p}}}_{P_2}$ in the first image and the point ${\underset{\_}{\mathit{p}}}_{P_2}$ lies on the line ${\mathit{F}}^T{\underset{\_}{\mathit{p}}}_{P_1}$ in the second image. Another important relation in machine vision is ${\mathit{p}}_{C_1}^T\mathit{E}{\mathit{p}}_{C_2}=0$, where $\mathit{E}={[{\mathit{t}}_{C_2}^{C_1}]}_{\times }{\mathit{R}}_{C_2}^{C_1}$ is known as the essential matrix. The relation between the essential and fundamental matrix is E =S^TF S.

The epipolar constraint can be used to improve matching of image points in multiple images that belong to the same world point given the known pose between two calibrated cameras. Since the corresponding pair of the point ${\underset{\_}{\mathit{p}}}_{P_1}$ in the first image must lie on the line ${\mathit{F}}^T{\underset{\_}{\mathit{p}}}_{P_1}$ in the second image, the 2D searching over the entire image area is simplified to a 1D search along the epipolar line, and therefore the matching process can be sped up significantly and it can also be made more robust, since matches that do not satisfy the epipolar constraint are rejected.

Example 5.5

A set of 3D points is observed from two cameras with relative displacement described by the rotation matrix ${\mathit{R}}_{C_2}^{C_1}={\mathit{R}}_x(30^{°}){\mathit{R}}_y(60^{°})$ and translation vector ${\mathit{t}}_{C_2}^{C_1}={[4-12]}^T$. Both cameras have the same internal camera model (5.49): α_xf = α_yf = 1000, no skewness (γ = 0), and image center is in the middle of the image with the dimensions of 1024 × 768. A set of projected points into the image of the first camera is

$\begin{array}{l}\hfill {\mathit{p}}_{P_1}^T\in \{[262634],[762634],[512134],[443457],[412284]\}\end{array}$

and a set of point projected into the image of the second camera is

$\begin{array}{l}\hfill {\mathit{p}}_{P_1}^T\in \{[259409],[397153],[488513],[730569],[115214]\}\end{array}$

The points in both images are shown in Fig. 5.4. Determine all possible image point correspondences taking into account the epipolar constraint. For all the matched point pairs reconstruct the position of the points in the 3D space with respect to both camera frames.

Solution

When matching points between the images from two cameras with a known relative pose between them, the points that belong to the same world point must satisfy the epipolar constraint (5.52). Eq. (5.52) has the form ${\mathit{l}}^T\underset{\_}{\mathit{p}}=0$, that is, the homogeneous point ${\underset{\_}{\mathit{p}}}^T=[xy1]$ lies on the line l^T = [a b c], since ax + by + c = 0 is a line equation. Therefore, the epipolar constraint (5.52) can be utilized to find the epipolar lines ${\mathit{l}}_{P_2}$ in the second image for every point ${\underset{\_}{\mathit{p}}}_{P_1}$ from the first image, and vice-versa, lines ${\mathit{l}}_{P_1}$ for every point ${\underset{\_}{\mathit{p}}}_{P_2}$. The epipolar lines for the first and the second image are shown in Fig. 5.5. From Figs. 5.4 and 5.5 four possible point matches can be found graphically.

The implementation of algorithm for point matching is given in Listing 5.2 (functions rotX and rotY are Matlab implementations of Eqs. 5.3, 5.4, respectively). The resulting point matches are collected in the index matrix pairs.

Listing 5.2

Point matching from Example 5.52

1 %   I n t r i n s i c  camera parameters and image screen  s i z e

2 alphaF  =  1000;  %  alpha*f , in px/m

3 s  =   [1024;   768];   %  screen size , in px

4 c  =  s /2;   %  image centre , in px

5 S  =   [ alphaF ,  0 , c (1) ;  0 , alphaF ,  c (2) ;  0 , 0 ,  1 ] ;   %  Camera model

6

7 %  Relative pose between the cameras

8 R   =  rotX (pi/6)*rotY (pi/3) ;  t  =   [ 4 ;  −1;   2 ] ;

9

10 %  Set of points

11 pP1  =   [262 ,  634; 762 , 634; 512 , 134; 443 , 457; 412 ,  2 8 4 ] . ’ ;

12 pP2  =   [259 ,  409; 397 , 153; 488 , 513; 730 , 569; 115 ,  2 1 4 ] . ’ ;

13

14 N1  =   s i z e (pP1 ,  2) ;  N2  =   s i z e (pP2 ,  2) ;   %  Number of points

15

16 %  Fundamental matrix

17 tx  =   [0 ,  − t (3) ,   t (2) ;   t (3) ,  0 , − t (1) ;   − t (2) ,   t (1) ,   0 ] ;

18 F   =  S . ’∖ tx* R /S ;

19

20 epsilon   =  1e−2;   %  Distance error tolerance

21 pP1  =   [ pP1 ;  ones (1 ,N1) ] ;  pP2  =   [ pP2 ;  ones (1 ,N2) ] ;   %  Homogenous

22  points

23

24 %  Epipolar  l i n e s  in frame P1 associated with the points in frame P2

25 lP1  =  F*pP2 ;

26 %  Epipolar  l i n e s  in frame P2 associated with the points in frame P1

27 lP2  =  F. ’*pP1 ;

28

29 %  Find point pairs (evaluate fundamental constraint in frame P2)

30 pairs   =   [ ] ;

31 for  i   =   1:N1

32  d  =  abs( lP2 ( : , i ) . ’*pP2) ;

33  k  =  find(d <epsilon ) ;

34  if  ˜isempty(k) ,  pairs   =   [ pairs ,   [ i ;  k (1) ] ] ;  end

35 end

pairs   =

 1           2           3           5

 3           4           2           1