1. Introduction
The orientation of rigid bodies serves as a fundamental characteristic in describing the state of single or multi-body systems. The extraction of a single rotation component (termed “planar rotation”) from a 3D rotation plays a crucial role in many applications. In biomechanics, for example, the analysis of joint angles within anatomic body planes offers better clinical interpretability than spatial rotations. Also, for example, in lower-mobility parallel kinematic robotic chains, often an unwished rotation – termed “parasitic motion” – is targeted to be excluded, and its determination from the 3D rotation is needed. However, due to the non-Abelian structure of spatial rotations, it is known that such “planar” components cannot be determined by simple projections as in a vector space, and well-known inconsistencies arise. Despite an extensive literature and discussion about the non-uniqueness and distortion of the results, such extraction techniques continue to be used due to the absence of alternatives, lacking a consistent standardization.
In this paper, the existing extraction techniques from the literature are reviewed, and a thorough analysis of their differences and similarities is performed. Hereby, the existing approaches can be grouped into two basic procedures. In the first group, the spatial rotation is decomposed into a sequence of three elementary rotations about follow-up orthogonal coordinate axes (Euler, roll-pitch-yaw, or Bryant angles), and one of them is identified as the sought-extracted rotation, where the differences lie in the chosen sequence of rotations. In the second group, a particular coordinate axis of the rotated coordinate frame is projected onto the target plane of the initial coordinate frame, assuming that the desired extracted angle is the angle between the projected coordinate axis and a reference coordinate axis defining the zero-degree angle. The paper shows on one side that the two approaches are fully isomorphic with respect to each other depending on the rotation sequence and projected axis used. On the other side, the paper shows that using these extraction methods leads to inconsistencies and ambiguities, which makes standardization difficult.
In response to these problems, the paper presents an alternative novel efficient method for extracting a rotational component in a plane. This method decomposes a spatial Quaternion into a Quaternion about an axis normal to the extraction plane and a Quaternion about an axis within this plane. As will be shown, unlike traditional methods, the Quaternion-based approach yields unique extractions of 1D components from 3D rotations. The Quaternion decomposition method was preliminarily proposed in a previous conference paper [Reference Ghiassi, Gegenbauer, Maibaum, Mitschke, Jaeger and Kecskeméthy23] and is here expanded to its full extent.
The differences between the methods are illustrated in a virtual example and an experimental case for the extraction of knee flexural angle in the sagittal plane from gait analysis, comparing it to a Vicon measurement. In this respect, also a novel method for determining the orientation of a box based on a two-plane projection from camera recordings is proposed, which yields more precise results than the existing solutions for the Perspective 3-Point Problem (P3p) from the literature used by default in most applications (as e.g. implemented in Matlab), and which is reviewed here for better reference.
2. Review of existing planar rotation extraction methods
Single rotation components (termed in the following “planar rotations”) are, in many instances, such as navigation [Reference Schlipsing, Salmen, Lattke, Schröter and Winner48, Reference Wu, Ruan, Han, Wang and Wang56, Reference Zhang, Li, Zhang, Pang and Chen57], human motion analysis [Reference Ancillao, Verduyn, Vochten, Aertbeliën and De Schutter5], robotics [Reference Carretero, Podhorodeski, Nahon and Gosselin10, Reference Gan, Liao, Dai, Wei and Seneviratne14, Reference Sun, Yang, Huang and Dai51, Reference Zhang and Dai58], and folding of packing cartons [Reference Liu and Dai38], of higher interest than the spatial orientation itself. In the analysis of human motion, for example, the extraction of planar rotations is essential for the analysis of anatomical segment angles. Angles projected onto anatomical body planes are favored as more relevant for intuitive clinical interpretation than their three-dimensional counterparts [Reference Wren and Mitiguy54]. The sagittal plane, which divides the body into a right and a left section, is particularly relevant as an extraction plane, as human locomotion predominantly takes place in this plane [Reference Ramakrishnan and Kadaba46].
The extraction of planar rotations from spatial orientations is also of crucial importance in the field of robotics. In parallel kinematics machines with less than three rotational degrees of freedom, intended autonomous movements can be accompanied by undesired, constrained rotations about axes that do not correspond to any degree of freedom, so-called parasitic rotations. Although the reduced number of degrees of freedom offers advantages such as cost reduction due to fewer actuators, simpler kinematics, and easier control [Reference Lin, Guo and Gao36], it can also lead to problems in various applications due to unpredictability and lower accuracy [Reference Carretero, Podhorodeski, Nahon and Gosselin10, Reference Li, Chen, Chen, Wu and Hu34, Reference Liu, Che, Li and Luo37, Reference Pouliot, Gosselin and Nahon44, Reference Ruiz, Campa, Altuzarra, Herrero and Diez47, Reference Siciliano50]. Thus, several approaches are considered in the literature for eliminating parasitic rotations by initially identifying them from known spatial rotations [Reference Carretero, Podhorodeski, Nahon and Gosselin10, Reference Li, Chen, Chen, Wu and Hu34, Reference Li and Xu35].
2.1. Problem statement
Consider a frame
${\mathcal K}_{B}$
visualized by a box that undergoes a spatial rotation with respect to a space-fixed “world” reference frame
${\mathcal K}_{W}$
(Figure 1). The world frame can also be moving, in which case the rotation is then a relative motion without restriction of generality. The task is to extract a “planar” rotation angle
$\alpha ^{z}$
that describes as close as possible the “component” of the general rotation in a plane, here chosen as the
$x,y$
plane of the world frame, again without restrictions of generality.
Example used to discuss the extraction methods.
Due to the non-commutativity of spatial rotations, 3D rotations are not isomorphic to vector spaces, and hence the sought “planar” rotation (a) cannot be determined by classical vector-space projection and (b) is not unique. For this reason, several approaches exist in the literature for estimating this rotation angle, with diverging results.
For a unified comparison of existing methods, we adopt the well-known Quaternion description (also known as Euler-Rodrigues parameters) for the rotation matrix
${}^{}_{}{\textbf {R}}_{B}$
transforming vector components of the moving frame
${\mathcal K}_{B}$
with respect to the world frame
${\mathcal K}_{W}$
. By Euler’s theorem, the rotation from the world frame to the moving frame can be described by a single rotation about an axis
$\underline{u}$
by an angle
$\alpha$
(Figure 1), which is embodied by the 4-parametric Quaternion
$Q$
as [Reference Altmann3], see also Appendix A:
\begin{align} Q = \begin{bmatrix} \;q_{0}\; \\[0.5em] \underline{q} \end{bmatrix} \quad \textrm{ with }\quad q_0 = \cos \frac{\displaystyle \alpha }{\displaystyle 2} \quad ; \quad \underline{q} = \begin{bmatrix} \;q_{x}\; \\[0.5em] q_{y} \\[0.5em] q_{z} \end{bmatrix} = \sin \frac{\displaystyle \alpha }{\displaystyle 2} \; \underline{u} \ \ . \end{align}
The elements of the rotation matrix are termed as:
\begin{align}{}^{}_{}{\mathsf{R}}_{B} = \begin{bmatrix} \;\;\;\rho _{11} \;\;\;&\;\;\;\rho _{12} \;\;\;&\;\;\;\rho _{13} \;\;\; \\[0.5em] \;\;\;\rho _{21} \;\;\;&\;\;\;\rho _{22} \;\;\;&\;\;\;\rho _{23} \;\;\; \\[0.5em] \;\;\;\rho _{31} \;\;\;&\;\;\;\rho _{32} \;\;\;&\;\;\;\rho _{33} \;\;\; \\[0.5em] \end{bmatrix} \ \, \end{align}
where each column of the matrix is the projection of the corresponding axis of
$\,{\mathcal K}_{B}$
onto
$\,{\mathcal K}_{W}$
. The rotation matrix
${}^{}_{}\boldsymbol{\mathsf{R}}_{B}$
can then be expressed by the four components of the Quaternion
$Q$
as [Reference Nikravesh41], Appendix A:
[Note that the rotation matrix can be easily determined from a given Quaternion by Eq. (3), or, vice-versa, the Quaternion can be easily determined by inverse relationships for a given rotation matrix (Appendix A). Thus, both representations are fully equivalent, and the most suited one can be employed for a given application. For example, when integrating angular velocities, the Quaternion method is more immediate and natural (Appendix A), while, for given rotation matrices, the inverse relationship is assumed to be pre-computed for the rest of this paper.]
It is clear that if the spatial rotation takes place only about an axis orthogonal to the plane (here the
$z$
axis), then the rotation axis
$\underline{u}$
is also orthogonal to the plane and the sought rotation angle
$\alpha ^{z}$
is identical to the full Quaternion rotation angle
$\alpha$
. On the other hand, if the rotation vector
$\underline{u}$
lies within the
$x,y$
plane, then no rotation about the orthogonal axis
$z$
takes place. The question is, what are the planar rotation angles for rotation axes having components both in the
$x,y$
plane (
$\underline{q}^{xy}$
) and the orthogonal direction
$z$
(
$\underline{q}^{z}$
)? This is the basis of the differences between current planar rotation extraction methods.
Hereby, two groups of approaches can be identified: (1) decomposing the spatial rotation in a sequence of elementary rotations about coordinate axes and selecting one of them as the sought planar rotation angle
$\alpha ^{z}$
(here termed Euler-angle decomposition methods), and (2) projecting an axis of the rotated frame on the plane and determining the angle between the projected axis and a reference axis in the plane as the sought planar rotation angle
$\alpha ^{z}$
(here termed coordinate-axis projection methods). As will be shown, both methods are identical. In the sequel, both methods are reviewed and discussed.
2.2. Euler-angle decomposition methods
The Euler-angles approach views a spatial rotation as a sequence of three elementary rotations about the axes of a step-wise rotating orthogonal coordinate system under the condition that no two consecutive rotations share the same absolute axis. The extraction of planar rotations from spatial orientations via Euler angles is based on the assumption that a partial Euler rotation angle reflects the desired “planar” rotation angle [Reference Carretero, Podhorodeski, Nahon and Gosselin10, Reference Davis, Õunpuu, Tyburski and Gage12, Reference Kadaba, Ramakrishnan and Wootten29, Reference Li, Chen, Chen, Wu and Hu34, Reference Li and Xu35]. The choice against computationally more robust approaches such as helical angles [Reference Woltring53] or Quaternions [Reference Haug27] stems from the assumption that Euler angles are more intuitive for the extraction process. However, this approach is not unproblematic, since the sequence of Euler angle elementary rotations can significantly influence the value of the extracted “planar” angle [Reference Cheng11, Reference Dumas, Aissaoui and De Guise15, Reference Ghiassi, Gegenbauer, Maibaum, Mitschke, Jaeger and Kecskeméthy23], and the resulting angles can deviate considerably from the expectations [Reference Baker8, Reference Foti, Davis, Davids and Farrell21]. Despite these well-known problems, Euler angles are still the most common approach in the literature [Reference Ancillao, Verduyn, Vochten, Aertbeliën and De Schutter5]. Efforts to circumvent these problems have led to attempts to establish predefined Euler sequences as a standard for specific applications, such as those proposed by the International Society of Biomechanics for human motion analysis [Reference D’Lima, Leardini, Witte, Chung, Cristofolini and Wu13, Reference Wu and Cavanagh55]. While this strategy improves the comparability of study results, it does not address the inherent lack of uniqueness of the extracted angles.
To ensure the uniqueness of angle projections, sequences where the first and last rotation occurs around the same local axis (“Proper Euler angles”) are not applied for this approach. This includes sequences such as
$z$
-
$y$
-
$z$
. Instead, only rotation sequences that ensure that each rotation occurs around a different axis (“Tait-Bryan angles”) are used. This includes sequences like
$z$
-
$y$
-
$x$
etc., leading to six potential rotation sequences for planar angle extractions. Often, the sequence is chosen such that the first rotation is about the axis that is normal to the desired projection plane, followed by rotations around the remaining axes [Reference Carretero, Podhorodeski, Nahon and Gosselin10, Reference Cheng11, Reference Dumas, Aissaoui and De Guise15, Reference Ghiassi, Gegenbauer, Maibaum, Mitschke, Jaeger and Kecskeméthy23, Reference Li, Chen, Chen, Wu and Hu34, Reference Li and Xu35].
Each sequence can be interpreted in two distinct ways: extrinsically, with rotations about inertially-fixed axes (
${\mathcal K}_{W}$
in the given example), and intrinsically, with rotations about step-wise moving body-fixed axes (
${\mathcal K}_{B}$
in the given example). Both intrinsic [Reference Cheng11, Reference Dumas, Aissaoui and De Guise15, Reference Ghiassi, Gegenbauer, Maibaum, Mitschke, Jaeger and Kecskeméthy23] and extrinsic rotations [Reference Carretero, Podhorodeski, Nahon and Gosselin10, Reference Li, Chen, Chen, Wu and Hu34, Reference Li and Xu35] are utilized in the literature for planar angle extractions. As intrinsic Euler angles are mathematically equivalent to the reversed sequence of extrinsic rotations, the discussion will focus on intrinsic rotations, implicitly covering also extrinsically interpreted sequences.
The elementary rotations about follow-up
$\;x$
-,
$y$
-, and
$z$
-axes, respectively, and defined by the unknown Euler angles
$\varphi$
,
$\Theta$
and the sought angle
$\alpha ^{z}$
, respectively, are:
\begin{align} \textrm{Rot}[ x, \varphi ] & = \left[\begin{array}{ccc} 1 & 0 & 0 \\[3pt] 0 & \cos \varphi & -\sin \varphi \\[3pt] 0 & \sin \varphi &{+}\cos \varphi \end{array}\right]\!;\ \textrm{Rot}[ y, \Theta ] = \left[\begin{array}{ccc} {+}\cos \Theta & 0 & \sin \Theta \\[3pt] 0 & 1 & 0 \\[3pt] -\sin \Theta & 0 & \cos \Theta \end{array}\right]\!;\ \textrm{Rot}[ z, \alpha ^{z} ] & = \left[\begin{array}{ccc} \cos \alpha ^{z} & -\sin \alpha ^{z} & 0 \\[3pt] \sin \alpha ^{z} &{+}\cos \alpha ^{z} & 0 \\[3pt] 0 & 0 & 1 \end{array}\right] \end{align}
Due to the non-Abelian structure inherent in spatial rotations, each Euler sequence, represented by a non-commutative multiplication of the elementary rotations, leads to distinct definitions of the rotation matrix
${}^{}_{}\boldsymbol{\mathsf{R}}_{B}$
. For the six possible Tait-Bryan angles employed in the literature for extraction, the rotation matrices shown in Eq. (5) to Eq. (10) are obtained. To maintain clarity and facilitate further discussion, only the relevant elements of the resulting matrices for determining the sought angle
$\alpha ^{z}$
are shown. Furthermore, the additional index on
${}^{}_{}\boldsymbol{\mathsf{R}}_{B}$
and
$\alpha ^{z}$
serves to indicate the chosen Euler sequence and assists in distinguishing the extracted angles based on the extraction method.
\begin{align} \boldsymbol{\mathsf{R}}^{\textrm{zyx}}_B & = \textrm{Rot}[ z, \alpha ^{z}_{\textrm{zyx}} ]\cdot \textrm{Rot}[ y, \Theta ]\cdot \textrm{Rot}[ x, \varphi ] = \left[ \begin{array}{@ c@ c@ c@{\hspace*{15pt}}} {+} \cos \alpha ^{z}_{\textrm{zyx}} \cos \Theta & * & * \\[3pt] {+}\sin \alpha ^{z}_{\textrm{zyx}}\cos \Theta & * & * \\[3pt] * & * & * \end{array}\right] \end{align}
\begin{align} \boldsymbol{\mathsf{R}}^{\textrm{zxy}}_B & = \textrm{Rot}[ z, \alpha ^{z}_{\textrm{zxy}} ]\cdot \textrm{Rot}[ x, \varphi ]\cdot \textrm{Rot}[ y, \Theta ] = \left[ \begin{array}{@c@c@{}c@{}} * & -\sin \alpha ^{z}_{\textrm{zxy}}\cos \varphi &*\\[3pt] *& {+} \cos \alpha ^{z}_{\textrm{zxy}}\cos \varphi &*\\[3pt] * & * & * \end{array}\right] \end{align}
\begin{align} \boldsymbol{\mathsf{R}}^{\textrm{xyz}}_B & = \textrm{Rot}[ x, \varphi ]\cdot \textrm{Rot}[ y, \Theta ]\cdot \textrm{Rot}[ z, \alpha ^{z}_{\textrm{xyz}} ] = \left[\begin{array}{@ c@{\quad}c@{}c@{}} {+}\cos \alpha ^{z}_{\textrm{xyz}}\cos \Theta & -\sin \alpha ^{z}_{\textrm{xyz}}\cos \Theta & * \\[3pt] * & * & *\\[3pt] * & * & * \end{array}\right] \end{align}
\begin{align} \boldsymbol{\mathsf{R}}^{\textrm{yxz}}_B & = \textrm{Rot}[ y, \Theta ]\cdot \textrm{Rot}[ x, \varphi ]\cdot \textrm{Rot}[ z, \alpha ^{z}_{\textrm{yxz}} ] = \left[ \begin{array}{@ c@{\quad}c@{}c@{}} * & * & * \\[3pt] {+}\sin \alpha ^{z}_{\textrm{yxz}}\cos \varphi & {+}\cos \alpha ^{z}_{\textrm{yxz}}\cos \varphi &*\\[3pt] * & * & * \end{array}\right] \end{align}
\begin{align} \boldsymbol{\mathsf{R}}^{\textrm{xzy}}_B & = \textrm{Rot}[ x, \varphi ]\cdot \textrm{Rot}[ z, \alpha ^{z}_{\textrm{xzy}} ]\cdot \textrm{Rot}[ y, \Theta ] = \left[ \begin{array}{@{}c@{}c@ c@{}} *&-\sin \alpha ^{z}_{\textrm{xzy}}&*\\[3pt] * & * & *\\[3pt] * & * & * \end{array}\right] \end{align}
\begin{align}\boldsymbol{\mathsf{R}}^{\textrm{yzx}}_B & = \textrm{Rot}[ y, \Theta ]\,\cdot \textrm{Rot}[ z, \alpha ^{z}_{\textrm{yzx}} ]\cdot \textrm{Rot}[ x, \varphi ] = \left[ \begin{array}{@{}c@{}c@{}c@{}} * & * & *\\[3pt] {+}\sin \alpha ^{z}_{\textrm{yzx}}&*&*\\[3pt] * & * & * \end{array}\right] \end{align}
The sought “planar” rotation angle
$\alpha ^{z}$
is then obtained by coefficient comparison between the rotation matrix Eq. (2) and Eq. (5) to Eq. (10), respectively, as:
\begin{align} \alpha ^{\textrm{z}}_{\textrm{zyx}} &= \textrm{atan2} \big(\pm \rho _{21},\ \ \pm \rho _{11} \big) & &\text{[5, 9, 54, 55]} \end{align}
\begin{align} \alpha ^{\textrm{z}}_{\textrm{zxy}} & = \textrm{atan2} \big ({-} \pm \,\rho _{12},\ \ \pm \rho _{22} \big ) & &\text{[36, 10, 11, 13, 12, 15, 29, 46, 54]} \end{align}
\begin{align} \alpha ^{\textrm{z}}_{\textrm{xyz}} & = \textrm{atan2} \big ({-} \pm \,\rho _{12},\ \ \pm \rho _{11} \big ) && \text{[5, 8, 16, 54]} \end{align}
\begin{align} \alpha ^{\textrm{z}}_{\textrm{yxz}} & = \textrm{atan2} \big (\pm \rho _{21},\ \ \pm \rho _{22} \big ) && \text{[8]} \end{align}
\begin{align} \alpha ^{\textrm{z}}_{\textrm{xzy}} & = \textrm{asin} \big ({-}\rho _{12} \big) &&\text{[49]} \end{align}
\begin{align} \alpha ^{\textrm{z}}_{\textrm{yzx}} & = \textrm{asin} \big (\rho _{21}\big ) &&\text{[8]}, \end{align}
where the
$\pm$
sign corresponds to the sign of the cosine of the second elementary rotation, and the references next to the expressions show the applications from the literature for each extraction method. It is essential to be aware of potential singularities that may arise when the second rotation is close to
$90^{\circ }$
, in which case the “planar” rotation angle
$\alpha ^{z}$
cannot be determined.
2.3. Coordinate-axis projection methods
The second group of extraction methods assumes that the desired extracted “planar” angle can be viewed as the angle between a projected coordinate axis of the moving
${\mathcal K}_{B}$
onto the target plane and a reference coordinate direction in the plane for the zero angle definition [Reference Falbriard, Meyer, Mariani, Millet and Aminian17, Reference Koll, Kugler, Kluge, Reinfelder, Jensen, Schuldhaus, Lochmann and Eskofier33] (Figure 2). As will be shown here (to our knowledge for the first time), the procedure yields exactly the same results as the Euler-angle decomposition for a corresponding sequence.
Out of the three axes of the moving frame
${\mathcal K}_{B}$
, the axis normal to the projection plane in the non-rotated state (here the
$z$
-axis) cannot be used, as it would be “invisible” when no rotation takes place or when the rotation is already a “planar” rotation about the plane normal. Therefore, any choice of the other two axes (here, the
$x$
- and
$y$
-axes) can be made. As the columns of
${}^{}_{}\boldsymbol{\mathsf{R}}_{B}$
are already the projection of axes of
${\mathcal K}_{B}$
onto
${\mathcal K}_{W}$
, the projected (moving)
$x$
and
$y$
axes, denoted as
$\overline{\underline{x}}$
and
$\overline{\underline{y}}$
, are in terms of
${}^{}_{}\boldsymbol{\mathsf{R}}_{B}$
\begin{align} \overline{\underline{x}} = \begin{bmatrix} \;\overline{x}_{x}\;\\[0.5em] \overline{x}_{y} \end{bmatrix} = \begin{bmatrix} \;\rho _{11}\;\\[0.5em] \;\rho _{21}\; \end{bmatrix} \quad \textrm{ and }\quad \overline{\underline{y}} = \begin{bmatrix} \;\overline{y}_{x}\;\\[0.5em] \overline{y}_{y} \end{bmatrix} = \begin{bmatrix} \;\rho _{12}\;\\[0.5em] \;\rho _{22}\; \end{bmatrix} \ \ . \end{align}
The axes defining zero degrees are in the projection plane
\begin{align} \overline{\underline{e}}_{x} = \begin{bmatrix} \;1 \;\\[0.5em] 0 \end{bmatrix} \quad \textrm{ and }\quad \overline{\underline{e}}_{y} = \begin{bmatrix} 0 \\[0.5em] \;1\; \end{bmatrix} \ \ . \end{align}
Visualization of the axis projection method.
The resulting angle extractions (termed as
$\alpha ^{\textrm{z}}_{\textrm{Ax}}$
and
$\alpha ^{\textrm{z}}_{\textrm{Ay}}$
, respectively) then result as:
\begin{align} \alpha ^{\textrm{z}}_{\textrm{Ax}} ={+}\textrm{sgn}\left (\;\overline{x}_{y}\;\right )\,\textrm{acos}\bigg (\;\frac{\overline{\underline{e}}_{x}^{\textrm{T}}\;\overline{\underline{x}}}{\|\,\overline{\underline{e}}_{x}\,\|\cdot \|\,\overline{\underline{x}}\,\|}\;\bigg )=\textrm{sgn}\left (\;\rho _{21}\;\right )\,\textrm{acos}\left (\;\frac{\rho _{11}}{\sqrt{\rho _{11}^2+\rho _{21}^2}}\;\right )\;\;\text{[17]} \end{align}
\begin{align} \alpha ^{\textrm{z}}_{\textrm{Ay}} = -\textrm{sgn}\left (\;\overline{y}_{x}\;\right )\, \textrm{acos}\bigg (\;\frac{\overline{\underline{e}}_{y}^{\textrm{T}}\;\overline{\underline{y}}}{\|\,\overline{\underline{e}}_{y}\,\|\cdot \|\,\overline{\underline{y}}\,\|}\;\bigg )= -\textrm{sgn}\left (\;\rho _{12}\;\right )\,\textrm{acos}\left (\;\frac{\rho _{22}}{\sqrt{\rho _{12}^2+\rho _{22}^2}}\;\right )\;\;\text{[33]} \end{align}
where the references next to the expressions show the applications from the literature for each extraction method.
Using the relationship
\begin{align} \textrm{acos}\bigg (\;\frac{A}{\sqrt{A^2+B^2}}\;\bigg ) = \textrm{atan2}\big (\;\;\,B\;\;\,,\;\;\,A\;\;\,) \end{align}
one obtains
\begin{eqnarray} \alpha ^{\textrm{z}}_{\textrm{Ax}} &=& \textrm{atan2}\big (\pm \rho _{21},\ \pm \rho _{11}\big ) \end{eqnarray}
\begin{eqnarray} \alpha ^{\textrm{z}}_{\textrm{Ay}} &=& \textrm{atan2}\big (\mp \rho _{12},\ \pm \rho _{22} \big ) \end{eqnarray}
which are identical to the Euler
$z$
-
$y$
-
$x$
and
$z$
-
$x$
-
$y$
extraction angles, respectively, including singularities:
\begin{align} \alpha ^{\textrm{z}}_{\textrm{Ax}} \equiv \alpha ^{z}_{\textrm{zyx}} \quad \textrm{ and }\quad \alpha ^{\textrm{z}}_{\textrm{Ay}} \equiv \alpha ^{z}_{\textrm{zxy}} \end{align}
3. Alternative novel Quaternion projection method
As an alternative to Euler-angle extraction, consider the decomposition of the total Quaternion (Figure 1)
\begin{align} Q \;=\; Q^{\textrm{z}} \circ Q^{\textrm{xy}} \end{align}
where “
$\circ$
” denotes Quaternion composition (Appendix A) and where the Quaternion “components”
\begin{eqnarray} \,Q^{\textrm{z}} \;&=&\; \begin{bmatrix} \;\,q_{0}^{\textrm{z}}\;\,\\[0.5em] \underline{q}^{\textrm{z}} \end{bmatrix} \quad \textrm{ with }\quad \underline{q}^{\textrm{z}} \;=\; \begin{bmatrix} 0 \\[0.5em] 0 \\[0.5em] \;\,q_{z}^{\textrm{z}}\;\, \end{bmatrix} \end{eqnarray}
\begin{eqnarray} Q^{\textrm{xy}} \;&=&\; \begin{bmatrix} \;q_{0}^{\textrm{xy}}\;\\[0.5em] \underline{q}^{\textrm{xy}} \end{bmatrix} \quad \textrm{ with }\quad \underline{q}^{\textrm{xy}} \;=\; \begin{bmatrix} \;q_{x}^{\textrm{xy}} \;\\[0.5em] q_{y}^{\textrm{xy}} \\[0.5em] 0 \end{bmatrix} \end{eqnarray}
are still unknown. The sought quantity is the extracted planar rotation
$\alpha ^{\textrm{z}}_{\textrm{Q}}$
of the first component
\begin{align} q_{0}^{\textrm{z}}= \cos{\left (\; \frac{\alpha ^{\textrm{z}}_{\textrm{Q}}}{2} \;\right ) } \quad \textrm{ and }\quad q_{z}^{\textrm{z}} = \sin{\left (\; \frac{\alpha ^{\textrm{z}}_{\textrm{Q}}}{2} \;\right ) } \end{align}
while the second component is an unknown rotation about an axis within the plane. Using the Quaternion inverse
\begin{align} \left \{ Q^{\textrm{xy}} \right \}^{-1} =\; \overline{Q}^{\textrm{\,xy}} = \,\begin{bmatrix} q_{0}^{\textrm{xy}} \\[0.5em] \;-\underline{q}^{\textrm{xy}} \; \end{bmatrix} \end{align}
and the Quaternion composition rule leads to
\begin{align} Q^{\textrm{z}} \;=\; Q \circ \left \{ Q^{\textrm{xy}} \right \}^{-1} =\; \begin{bmatrix} q_{0} q_{0}^{\textrm{xy}} + \underline{q} \cdot \underline{q}^{\textrm{xy}} \\[0.5em] \; - q_{0} \underline{q}^{\textrm{xy}} + q_{0}^{\textrm{xy}} \underline{q} - \underline{q} \times \underline{q}^{\textrm{xy}} \; \end{bmatrix} \end{align}
from where the following four scalar equations are obtained:
\begin{eqnarray} \; q_{0}^{\textrm{z}} \; &=& \; q_{0}q_{0}^{\textrm{xy}} \; + q_{x}q_{x}^{\textrm{xy}} \;+ q_{y}q_{y}^{\textrm{xy}} \end{eqnarray}
\begin{eqnarray} \;\;\, 0 \; &=& \; q_{x}q_{0}^{\textrm{xy}} \;- q_{0}q_{x}^{\textrm{xy}} \;\, + q_{z}q_{y}^{\textrm{xy}} \end{eqnarray}
\begin{eqnarray} \;\; 0 \; &=& \; q_{y}q_{0}^{\textrm{xy}} \;- q_{z}q_{x}^{\textrm{xy}} \;- q_{0}q_{y}^{\textrm{xy}} \end{eqnarray}
\begin{eqnarray} q_{z}^{\textrm{z}} \; &=& \; q_{z}q_{0}^{\textrm{xy}} \;\, + q_{y}q_{x}^{\textrm{xy}} - q_{x}q_{y}^{\textrm{xy}} \end{eqnarray}
Determining
$q_{x}^{\textrm{xy}}$
and
$q_{y}^{\textrm{xy}}$
from Eqs. (32) and (33) and inserting in Eq. (31) and Eq. (34) gives
\begin{align} q_{x}^{\textrm{xy}} \;\; = \;\; \frac{\qquad \; q_{0}q_{x} + q_{y}q_{z}\qquad \; }{q_{0}^{2} + q_{z}^{2}}\,q_{0}^{\textrm{xy}} \qquad \qquad \qquad \quad \;\;\; \qquad \quad \;\;\; \left [\;\;=\quad \pm \frac{\;q_{0}q_{x} + q_{y}q_{z}\;}{\sqrt{\;q_{0}^{2} + q_{z}^{2}\;}}\;\;\right ] \end{align}
\begin{align} q_{y}^{\textrm{xy}} \;\; = \;\; \frac{\qquad \; q_{0}q_{y} - q_{x}q_{z}\qquad \;}{q_{0}^{2} + q_{z}^{2}}\,q_{0}^{\textrm{xy}} \qquad \qquad \qquad \quad \;\;\; \qquad \quad \;\;\; \left [\;\;=\quad \pm \frac{\;q_{0}q_{y} - q_{x}q_{z}\;}{\sqrt{\;q_{0}^{2} + q_{z}^{2}\;}}\;\;\right ] \end{align}
\begin{align} q_{0}^{\textrm{z}}\; \;\; = \;\; \frac{q_{0} \left ( \;q_{0}^{2} + q_{x}^{2} + q_{y}^{2} + q_{z}^{2} \;\right ) }{q_{0}^{2} + q_{z}^{2}}\,q_{0}^{\textrm{xy}} \;\; = \;\; \frac{q_{0}}{\;q_{0}^{2} + q_{z}^{2}\;}\,q_{0}^{\textrm{xy}} \qquad \qquad \; \left [\;\;=\quad \pm \frac{\qquad q_{0}\qquad }{\sqrt{\;q_{0}^{2} + q_{z}^{2}\;}}\, \,\right ] \end{align}
\begin{align} \;\, q_{z}^{\textrm{z}}\; \;\; = \;\; \frac{\;q_{z} \left ( q_{0}^{2} + q_{x}^{2} + q_{y}^{2} + q_{z}^{2} \;\right ) }{q_{0}^{2} + q_{z}^{2}}\,q_{0}^{\textrm{xy}} \;\; = \;\; \frac{q_{z}}{\; q_{0}^{2} + q_{z}^{2}\;}\,q_{0}^{\textrm{xy}} \qquad \qquad \;\! \left [\;\;=\quad \pm \frac{\qquad \, q_{z}\qquad }{\sqrt{\;q_{0}^{2} + q_{z}^{2}\;}}\;\right ] \end{align}
The expression in brackets follows from the normalization achieved by squaring and adding Eq. (37) and Eq. (38) and considering the normalizing condition
$(q_{0}^{\textrm{z}})^{2} + (q_{z}^{\textrm{z}})^{2} = 1$
yielding
\begin{align} q_{0}^{\textrm{xy}} \;=\; \pm \,\sqrt{\;q_{0}^{2} + q_{z}^{2}\;} \end{align}
Thus, one obtains by dividing the expressions in the brackets of Eq. (37) and Eq. (38)
\begin{align} \frac{\;q_{z}^{\textrm{z}}\;}{\;q_{0}^{\textrm{z}}\;} \;=\; \frac{\;\pm q_z\;}{\;\pm q_{0}\;} \end{align}
in which all variables of the second Quaternion component are eliminated, and with Eq. (28) it holds
\begin{align} \alpha ^{\textrm{z}}_{\textrm{Q}} = 2\cdot \textrm{atan2}\big (\pm q_{z},\ \pm q_{0} \big ) \ \ . \end{align}
It is remarkable that the planar rotation about the planar
$z$
axis is just the
$z$
component of the original Quaternion vector, scaled by the reciprocal of the real part of the original Quaternion. Thus, the extraction results as a simple scaled pseudo-projection of the original Quaternion vector to the axis orthogonal to the plane. A further remarkable property from Eq. (41) is that, although there are formally two solutions, these are separated by 360
$^{\circ }$
, thus actually the plus sign is unique for a full rotation. Even more remarkably, switching the order of components in Eq. (25), i.e., placing the
$z$
rotation as second component of the decomposition, only leads to a change of sign in the cross product in Eq. (30), which again leads only to a change of the sign in the numerator of Eq. (35) and Eq. (36), respectively. Thus, the outcome for
$\alpha ^{\textrm{z}}_{\textrm{Q}}$
remains the same, and one obtains the result that the decomposition Eq. (25) is commutative with respect to the planar rotation component. This makes the Quaternion extraction method a quasi-vector space with respect to the extracted angle
$\alpha ^{z}$
. Finally, it can be observed that the solution of Eq. (37) and Eq. (38) becomes singular when both components
$q_{0}$
and
$q_{z}$
vanish at the same time. This corresponds to a rotation of 180
$^{\circ }$
about an axis in the projection plane. In this case, the space is rotated face-down, and if the rotation axis is skewed with respect to the
$x$
and
$y$
directions, then a spurious rotation about the
$z$
appears to happen (Figure 3), although there is no such one. Such a case can however be excluded from the planar rotation extraction case, as it does not make sense to try to identify
$z$
rotations when such a large rotation takes place only normal to the sought rotation axis.
Rotation of a rectangle ABCD by 180
$^{\circ }$
about its diagonal in the projection plane.
4. Comparison and discussion
The practical implications of the different planar angle extraction techniques of the literature, as well as the new Quaternion-based method, are illustrated in the following by two case examples: (1) a virtual rotation of a box generated by an arbitrary time development of the spatial angular velocity vector in body-fixed coordinates, and (2) an application from biomechanics, in which the sagittal knee angle is determined from experimental inertial measurement unit (IMU) measurements and compared to the gold-standard Vicon results from simultaneously infrared tracked retro-reflective markers.
4.1. Example 1: Virtual 3D rotation of a box
Let the kinematic input of a box be provided by the body-fixed angular velocity
${}^{B}_{}\underline{\omega }_{B}$
\begin{align}{}^{B}_{}\underline{\omega }_{B} = \begin{bmatrix} \sin \big (\pi \tfrac{\tau }{\pi }\big ) \\[0.5em] \tfrac{3}{4}\sin \big (\tfrac{\pi }{6}\tau -\tfrac{\pi }{4}\big ) \\[0.5em] \;\tfrac{3}{2}\sin \big (\tfrac{2\pi }{3}\tau -\pi \big )\; \end{bmatrix} \end{align}
for the dimensionless time
$0 \leq \tau \leq 6$
. The resulting time development
$Q\left ( \tau \right )$
of the resulting Quaternion can be easily and precisely determined by Runge-Kutta integration of fourth order (Appendix A) with a step size of 2 ms. The progression of the box orientation over dimensionless time is shown in Figure 4, where the poses are “stretched” in translational direction for visual reasons to avoid overlaps. The upper panel displays the Quaternion vector with direction equal to the rotation vector
$\underline{u}$
and magnitude equal to sine of half rotation angle, where Quaternion vectors close to the projection plane are colored gray. The second panel displays the total spatial rotation of the box. The other three panels display the rotation of the box when applying only the planar rotation resulting from the Quaternion extraction and the extraction by Euler sequences
$z$
-
$x$
-
$y$
and
$z$
-
$y$
-
$x$
, respectively. The resulting progression of the planar rotation angles
$\alpha ^{z}$
is shown in Figure 5 for all extraction methods.
“Stretched” rotation points over time with the extractions
$\alpha ^{z}_Q$
,
$\alpha ^{z}_{zxy}$
, and
$\alpha ^{z}_{zyx}$
.
Extracted planar angle
$\alpha ^{z}$
for all extraction methods.
One can see that at the beginning, all planar extraction methods behave similarly, but deviate substantially as rotation progresses. A closer analysis reveals the causes for similarity and divergence: At the beginning, rotations are small and thus close to infinitesimal relationships, whereas rotations are commutative and thus lie in a vector space. Thus, the extraction process corresponds approximately to a proper vector projection, and all extraction methods yield close to equal results. For the other regions, results of the Euler extraction methods diverge substantially, and even between two sequences with equal locations of the extracted planar rotation in the Euler sequence (e.g.
$x$
-
$y$
-
$z$
vs.
$y$
-
$x$
-
$z$
). This shows that the supposedly intuitive Euler-angle extraction leads to the unintuitive result that the extracted angle
$\alpha ^{z}$
changes significantly depending on the sequence of other two rotations, here
$x$
and
$y$
. Moreover, the Euler extraction methods produce inconsistent results at poses A, B, and C, where the rotation takes place about an axis within the projection plane and thus no
$z$
-rotation would be expected (Figure 6). Thus, the Euler extraction methods are intuitive only for small rotation angles but become erratic and inconsistent for large rotations depending on the rotation sequence, in particular also on the choice of the sequence of the other (uninvolved) two. This is not the case for the Quaternion extraction method, which yields consistent and unique results (zero planar rotation angle when rotation takes place about an axis in the projection plane) and a smoother rotation trajectory.
Rotated box with its rotation axis entirely within the projection plane.
4.2. Example 2: Extraction of sagittal knee angle from 3D IMU measurements
As a second example, the measurement of the knee sagittal angle for human walking along a track using IMUs is regarded. The results are validated with respect to the current gold-standard technique for capturing human motion within the lab based on retro-reflective markers attached to the subject’s skin at specific anatomic reference points and subsequently recorded by a synchronized set of cameras within the room [Reference Altman and Davis2, Reference Arndt, Wolf, Liu, Nester, Stacoff, Jones, Lundgren and Lundberg7, Reference Nigg, De Boer and Fisher40]. As known, marker-based measurements excel with precision and robustness, but have certain limitations such as the necessity for specialized laboratories, restricted mobility, high costs, and the need for specially trained personnel [Reference Ancillao, Tedesco, Barton and O’Flynn4, Reference van der Krogt, Sloot and Harlaar52]. On the other hand, IMUs are at advantage regarding low cost, suitability for outdoor measurements, and easiness and speed to attach. However, they also feature a couple of shortcomings, such as integration drift due to sensor errors, undefined orientation with respect to limbs, and unknown (varying) tilt offsets between IMU and anatomical axes.
The experimental setup is shown in Figure 7. Subjects were outfitted with reflective markers by a standardized plug-in gait protocol [Reference Ferreira18]. A movement measurement system (Vicon MX, Vicon Motion Systems Ltd UK, 100 Hz) comprising eight cameras, supplemented by a synchronized video camera (Basler 602 fc, 50 Hz), was utilized. All raw kinematic data of the Vicon reference system underwent filtering using a Woltring filter. A video camera positioned laterally to the walking track with its image plane parallel to the sagittal plane served to determine the constant offset between the relative sensor angle and the actual knee angle. In addition, two IMUs (Myon Aktos-T, myon AG, Schwarzenberg, Switzerland, 2000Hz) were placed in a box and affixed using double-sided adhesive tape to the right leg at the shank adjacent to the musculus tibialis anterior, and laterally to the thigh. Two further calibrated DSLR cameras (EOS 2000D, Canon) with known absolute orientations were positioned to determine the IMU box orientation at the start. In order to validate the proposed method against a gold-standard reference, the knee angles of ten test subjects were assessed during five trials of walking along a designated track, leading to 60 trials in total, employing both retro-reflective markers and the method delineated in this paper. An analog signal was employed to concurrently trigger both DSLR cameras, IMU sensor recordings, the Vicon system, and the laterally placed video camera, ensuring synchronization across all systems and marking the beginning of the individual trial.
Illustration of the experimental process for knee sagittal angle measurement featuring the same subject twice in the same scene: left scene: 20 sec standstill phase for initial target orientation; right scene: dynamic assessment during walking motion.
Five steps were performed to optimize the precision of IMU-based tracking of the sagittal joint angle:
-
1) determination of precise initial IMU orientation using two CCD cameras (Appendix B)
-
2) identification of constant error offset in angular velocity (see below)
-
3) integration of IMU-measured angular velocity using fourth-order Runge-Kutta (Appendix A)
-
4) extraction of planar angles of shank and thigh rotation according to methods in Sections 2 and 3
-
5) offset shift of relative sagittal knee angle to anatomic angle (see below)
Step 1: Initial IMU box orientation
A new method was developed to determine the precise orientation of the IMU box with respect to the track world frame (Appendix B). The method uses a Newton iteration to determine the pose, which matches the projections of a checkerboard face on two camera planes with known orientation (Figure 8). Recordings from the triggered CCD cameras during a standstill phase for 20 s were utilized to determine the initial orientation of the sensors. The recorded average accuracy was
$0.37^{\circ } \pm 0.15^{\circ }$
compared to classical methods such as in MATLAB with an accuracy of
$2.03^{\circ } \pm 0.76^{\circ }$
.
Left: exploded view of the target with an inertial measurement unit including a gyroscope. Middle: targets positioned on the observed leg, highlighting the relative sensor angle (
$\alpha$
) and knee angle (
$\beta$
). Right: lateral video camera frame used for a one-time knee angle estimation.
Step 2: Compensation of constant sensor error offset
The main cause of sensor error was found to be a constant offset for the measured angular velocity. This constant error offset
$\underline{\delta }$
was determined for each sensor as the average value during the standstill phase of 20 s. Then, the value was subtracted for the actual sensor measurement
${}^{T}_{}\underline{\omega }_{}^{\ast }(t)$
to obtain the corrected body-fixed angular velocity
${}^{T}_{}\underline{\omega }_{}(t)$
relative to the sensor coordinate frame
${\mathcal K}_{T}$
as:
\begin{align}{}^{T}_{}\underline{\omega }_{}(t) ={}^{T}_{}\underline{\omega }_{}^{\ast }(t) - \underline{\delta } \end{align}
Step 3: Integration of IMU-measured angular velocity
Using the initial IMU orientation obtained from step 1, a fourth-order Runge-Kutta integration method for determining the resulting Quaternions for the IMU sensors of shank
$Q_{S}(t)$
and thigh
$Q_{T}(t)$
including normalization of the resulting Quaternions after each integration step was applied (Appendix A).
Step 4: Extraction of planar rotation angles
From the resulting Quaternions of shank
$Q_{S}(t)$
and thigh
$Q_{T}(t)$
, respectively, the corresponding sagittal angles
$\alpha ^{\textrm{z}}_S(t)$
and
$\alpha ^{\textrm{z}}_T(t)$
of shank and thigh, respectively, were determined for all methods described in the previous sections, specifically Eq. (11) to Eq. (16) for Euler-angle decompositions and Eq. (41) for Quaternion decomposition.
Step 5: Offset shift of relative sagittal knee angle to anatomic angle
The IMU relative planar angle
$\alpha ^{\textrm{z}}(t)$
at the knee was computed as the difference between the planar angles of thigh and shank
\begin{align} \alpha ^{\textrm{z}}(t) = \alpha ^{\textrm{z}}_S(t) - \alpha ^{\textrm{z}}_T(t) \end{align}
Between the IMU planar relative angle
$\alpha ^{\textrm{z}}(t)$
and the “true” anatomical sagittal knee angle
$\beta ^{\textrm{z}}(t)$
, a constant offset
$\beta ^{\textrm{z}}_0$
(Figure 8) was determined such that:
\begin{align} \beta ^{\textrm{z}}(t) = \alpha ^{\textrm{z}}(t) - \beta ^{\textrm{z}}_0 \end{align}
The constant offset
$\beta ^{\textrm{z}}_0$
was estimated by an experienced orthopedic professional visually assessing the sagittal knee angle at an arbitrarily chosen time point within the gait cycle at the laterally positioned camera and comparing this to the Vicon value at this point, which reached an average accuracy of
$2.1^{\circ }$
compared to the Vicon measurement.
Resulting sagittal knee values for all planar rotation extraction methods and comparison to Vicon
The sagittal knee angle for the selected gait cycle was measured using Vicon and the methods presented in this section. Vicon measurements and sensor data were time-normalized to 100% for the described gait cycle. The IMU-sampling rate and the Runge-Kutta step size were 1 ms and 2 ms, respectively. The root-mean-square error (RMSE) was calculated for each trial by comparing the calculated sagittal knee angle with the result from Vicon. Figure 9 displays the resulting average sagittal knee angle over a trial for one person, while the RMSEs for all sagittal angle extraction methods across all 60 trials are displayed in Table I.
Root mean square error of all planar rotation extraction methods.
Progression of the sagittal knee angle for one trial of a single participant during a gait cycle.
Clearly, the Quaternion extraction exhibits the lowest deviation of
$2.8^{\circ }$
, closely approaching the gold standard. Conversely, all Euler-angle decompositions present a significantly higher RMSE, with a particularly pronounced deviation in Euler sequences where the extracted angle about the global
$z$
-axis is the middle angle of the sequence (
$x$
-
$z$
-
$y$
and
$y$
-
$z$
-
$x$
).
From Figure 9, one can observe that the Quaternion extraction follows best the Vicon measurement, up to some wobbling effects, which are attributed to gross soft-tissue motions around heel strike (yellow area), and a slight overshoot at maximal knee flexion (dark gray area), which is attributed to skin stretching and muscle bulging at these stages. On the other hand, one can see strong deviations in the Euler extraction methods, which can reach an order of magnitude of 10
$^{\circ }$
in the worst case. Also, the wobbling effects are more pronounced in the Euler extraction methods, which is due to their sensibility to slight tilt rotations of the IMU sensor on the skin, which are better compensated by the Quaternion extraction method. Similar results were obtained when determining first the 3D relative orientation between the two IMUs and then extracting the planar sagittal knee angle from this, as shown in Figure 10.
Progression of the sagittal knee angle determined from relative IMU orientation.
Altogether, the newly introduced Quaternion extraction is visibly more robust and precise for knee flexural angle determination than the traditional Euler extraction methods, and, due to its simplicity and uniqueness, a promising technology for also other sagittal anatomical angle extractions (e.g., hip, elbow, and torso) from IMU measurements for applications in biomechanics.
Future research might focus on enhancing sensor placement, developing software-based detection and compensation mechanisms for soft-tissue artifact effects, and determining the sagittal plane during free walking or running from measurement data.
5. Conclusion
A review of existing methods for extracting “planar” rotation components from 3D rotations, such as those needed, for example, for the determination of sagittal anatomical joint rotations from IMU measurements in biomechanics or of parasitic motions in parallel kinematics manipulators, is presented. The existing methods can be grouped into two categories: one using a decomposition of the 3D rotation into a sequence of elementary follow-up rotations about orthogonal coordinate axes for different rotation sequences, identifying one of them as the sought planar rotation (termed Euler-angle decomposition methods), and one using a projection of one of the coordinate axes of the rotated frame onto the target plane and determining the planar rotation as the angle between the projected moving axis and an axis of the base frame (termed coordinate decomposition methods). Both groups are shown to be equivalent. Moreover, it is shown that the existing methods are highly sensitive with respect to the choice of the sequence of partial rotations, and that they lead to ambiguities when the rotations are not small and inconsistencies when a rotation about an axis in the plane takes place. A novel approach to resolve these difficulties is presented based on the decomposition of the overall rotation Quaternion in a component normal to the target plane and a component in the plane. Three remarkable properties of the new Quaternion planar rotation extraction method are proven: (1) that it corresponds to a scaled projection of the full Quaternion vector onto the target planar rotation axis (divided by the original Quaternion real part); (2) that it is unique; and (3) that it is commutative with respect to the location of the planar rotation in the decomposition of the full rotation concerning the value of the extracted planar rotation angle, that is, if it placed at the beginning (inertially-fixed) or at the end (body-fixed). This makes the Quaternion extraction method a simple and effective quasi-vector space projection with respect to the extracted planar rotation angle, a property that has not been shown or known before. Two case examples, one for a virtual rotation of a box driven by a time-dependent angular velocity input, the second involving an experimental setup for measuring the sagittal knee angle by IMUs compared to the gold-standard Vicon, illustrate the inconsistencies of the existing approaches and validate the superiority of new Quaternion decomposition approach. Moreover, a new method for determining the orientation of a box with checkerboard face by two CCD cameras is presented, which yields higher accuracy by a factor of 5 with respect to existing P3Pp methods.
Acknowledgements
We thank Prof. Dr. med Marcus Jäger for his support in estimating the knee angles with the recordings of the lateral-placed video camera. We would also like to thank Joey Maibaum for creating the Figure 4. Additionally, our gratitude goes to all ten participants who patiently assisted us during the measurements. Their cooperation was invaluable to the success of our study.
Author contributions
Mehdi Ghiassi: contribution and implementation of the Quaternion decomposition; design and conduction of the measurements; generation of the results; and drafting of the manuscript; Andrés Kecskeméthy: conception and design of the research; development of the Quaternion decomposition; contribution to the drafting of the manuscript; and proofreading.
Financial support
This research received no specific grant from any funding agency, commercial, or not-for-profit sectors.
Competing interests
The authors declare no conflicts of interest exist.
Ethical approval
Not applicable.
Appendix A: Quaternion relations
This section summarizes for better reference well-known relationships for representing rotations and their infinitesimal properties by Quaternions.
Rotation matrix. Consider a rotation
${}^{}_{}\boldsymbol{\mathsf{R}}_{B}$
from a base coordinate frame
${\mathcal K}_{W}$
to a rotated coordinate frame
${\mathcal K}_{B}$
(Figure 11). The rotation can be expressed by the SO(3) parametrization (“Special Orthogonal group in dimension 3”) as an orthogonal matrix
\begin{align}{}^{}_{}\boldsymbol{\mathsf{R}}_{B} = \begin{bmatrix} \;\rho _{11} \;&\; \rho _{12} \;&\; \rho _{13}\; \\[0.5em] \;\rho _{21} \;&\; \rho _{22} \;&\; \rho _{23}\; \\[0.5em] \;\rho _{31} \;&\; \rho _{32} \;&\; \rho _{33}\; \end{bmatrix} \end{align}
By Euler’s theorem, any rotation can be expressed as a rotation about an axis
$\underline{u}$
by an angle
$\alpha$
as:
\begin{align}{}^{}_{}\boldsymbol{\mathsf{R}}_{B} = \boldsymbol{\mathsf{I}}_{3} + \sin \alpha \; \widetilde{\underline{u}} + (1-\cos \alpha ) \; \widetilde{\underline{u}}^2 \end{align}
where
$\boldsymbol{\mathsf{I}}_{3}$
is the three-dimensional identity matrix and
$\widetilde{\underline{u}}$
is the cross-product matrix for a vector
$\underline{u} ={[ u_x, u_y, u_z ]}^{\mathrm{T}}$
as:
\begin{align} \widetilde{\underline{u}} = \begin{bmatrix} {-} 0 \;&\; -u_z \;&{-}u_y \;\\[0.5em] {-}u_z \;&{-} 0 \;&\; -u_x \;\\[0.5em] \;-u_y \;&{-}u_x \;&{-} 0 \;\\[0.5em] \end{bmatrix} \end{align}
By introducing half-angles, a set of parameters, also termed Euler-Rodrigues parameters, can be introduced as [Reference Altmann3]:
\begin{align} q_0 = \cos \frac{\alpha }{2} \quad \textrm{and}\quad \underline{q} = \sin \frac{\alpha }{2} \; \underline{u} \end{align}
which can be collected in the so-called
$4\times 1$
Quaternion:
\begin{align} Q = \begin{bmatrix} \;q_{0}\; \\[0.5em] \underline{q} \end{bmatrix} \quad \textrm{ with }\quad \underline{q} = \begin{bmatrix} \;q_{x}\; \\[0.5em] q_{y} \\[0.5em] q_{z} \end{bmatrix} \end{align}
with
$q_0$
the real part and
$\underline{q}$
the vector part, and the normalizing condition:
\begin{align} q_0^2 + \|\,\underline{q}\,\| = 1 \ . \end{align}
By inserting Eq. (49) into Eq. (47), the following relation holds [Reference Nikravesh41]:
\begin{align}{}^{}_{}\boldsymbol{\mathsf{R}}_{B}(q_0,\underline{q}) = \boldsymbol{\mathsf{I}}_{3} + 2 \; (q_{0} \, \widetilde{\underline{q}} + \widetilde{\underline{q}}^{2}) \end{align}
where
$\widetilde{\underline{q}}$
is the cross-product matrix for the vector
$\underline{q}$
as:
\begin{align} \widetilde{\underline{q}} = \begin{bmatrix} {+} 0 \;&\; -q_z \;&{+}q_y \;\\[0.5em] {+}q_z \;&{+} 0 \;&\; -q_x \;\\[0.5em] \;-q_y \;&{+}q_x \;&{+} 0 \;\\[0.5em] \end{bmatrix} \end{align}
which gives rise to the explicit relationship for the rotation matrix:
\begin{align}{}^{}_{}\boldsymbol{\mathsf{R}}_{B}(q_0,\underline{q}) = \left[\begin{array}{c@{\quad}c@{\quad}c} 1-2\,(q_{y}^2+q_{z}^2) & 2\,({-}q_{0}q_z+q_{x}q_y) & 2\,({+}q_{0}q_y+q_{x}q_z) \\[3pt] 2\,({+}q_{0}q_z+q_{x}q_y) & 1-2\,(q_{x}^2+q_{z}^2)& 2\,({-}q_{0}q_x+q_{y}q_z) \\[3pt] 2\,({-}q_{0}q_y+q_{x}q_z) & 2\,({+}q_{0}q_x+q_{y}q_z) & 1-2\,(q_{x}^2+q_{y}^2) \end{array}\right] \end{align}
Thus, the rotation matrix for given Quaternion s computed by 12 multiplications and 12 additions.
Illustration example for Quaternion relationships.
Likewise, for the inverse problem, the sought Quaternion can be determined using the following procedure:
Where
$\epsilon ^2$
is a small quantity, for example,
$\epsilon ^2 = 0.01$
. Thus, the inverse problem can be computed by only one square root, 1 division, 4 multiplications, and maximally 7 additions.
Composition of rotations. By spherical trigonometry, it can be shown that the composition of two Quaternions
\begin{align} Q^{\textrm{I}} = \begin{bmatrix} \;q_{0}^{\textrm{I}}\; \\[0.5em] \;\underline{q}^{\textrm{I}}\; \end{bmatrix} \qquad \qquad Q^{\textrm{II}} = \begin{bmatrix} \;q_{0}^{\textrm{II}}\; \\[0.5em] \;\underline{q}^{\textrm{II}}\; \end{bmatrix} \end{align}
where
$Q^{\textrm{I}}$
represents a first rotation, followed by a second rotation embodied by
$Q^{\textrm{II}}$
, is given by [Reference Altmann3]:
\begin{align} Q = Q^{\textrm{I}}\circ Q^{\textrm{II}} = \begin{bmatrix} q_{0}^{\textrm{I}}q_{0}^{\textrm{II}} - \underline{q}^{\textrm{I}}\cdot \underline{q}^{\textrm{I}} \\[0.5em] q_{0}^{\textrm{I}}\underline{q}^{\textrm{II}} + q_{0}^{\textrm{II}}\underline{q}^{\textrm{I}}+\underline{q}^{\textrm{I}}\times \underline{q}^{\textrm{II}} \end{bmatrix} \ \ . \end{align}
Note that changing the order of the two rotations just changes the sign of the cross products, all other terms remaining symmetric with respect to the parameters of the two rotations. Likewise, the inverse
$Q^{-1}$
of a Quaternion is given by its conjugate
\begin{align} Q^{-1} = \overline{Q} = \begin{bmatrix} \;q_{0}\; \\[0.5em] -\underline{q} \end{bmatrix} \ \, \end{align}
and the neutral element
$Q_e$
(zero rotation) results as:
\begin{align} Q_e = \begin{bmatrix} \;1\; \\[0.5em] \underline{0} \end{bmatrix} \ \ . \end{align}
First-order differential relationships. By letting the angle
$\alpha$
approach zero, the Quaternion for an infinitesimal rotation is given by:
\begin{align} Q(\delta \alpha ) = Q_e + \delta Q = \begin{bmatrix} 1\\[0.5em] \; \delta \underline{q} \; \end{bmatrix} \end{align}
where
$\delta Q$
is the infinitesimal rotation increment [Reference Kecskeméthy, Lenarčič and Galletti31]
\begin{align} \delta Q = \begin{bmatrix} 0\\[0.5em] \; \delta \underline{q} \; \end{bmatrix} \end{align}
with
\begin{align} \delta \underline{q} = \underline{u} \cdot \frac{\delta \alpha }{2} \ \ . \end{align}
Thus, the infinitesimal increment
$\delta \underline{q}$
represents half of an infinitesimal rotation angle
$\delta \alpha$
about an instantaneous axis direction
$\underline{u}$
. Dividing
$\delta \underline{q}$
by the corresponding infinitesimal time interval
${\mathrm{d}}t$
yields the angular velocity
\begin{align} \underline{\omega } = \delta \dot{\alpha }\underline{u} = 2 \cdot \delta \dot{\underline{q}} \ \ . \end{align}
By using the Quaternion omposition formula Eq. (56) with the infinitesimal Quaternion either at the end or the beginning, one obtains for the differential
${\textrm{d}} Q$
of the Quaternion
\begin{align} dQ = \begin{bmatrix} \;dq_0 \;\\[0.5em] \;d\underline{q}\; \end{bmatrix} \ \, \end{align}
that is, the infinitesimal change of the numerical values of Q, the relationship
\begin{align} Q +{{\textrm{d}} Q} = \begin{bmatrix} \;q_0 \;\;&\;\; -\underline{q}^{\textrm{T}}\;\\[0.5em] \;\underline{q} \;\;&\;\; q_0\boldsymbol{\mathsf{I}}_3 \pm \widetilde{\underline{q}}\; \end{bmatrix} \begin{bmatrix} \;1\;\\[0.5em] \;\delta \underline{q}\; \end{bmatrix} \qquad \quad \begin{matrix} +\; : \; \textrm{body-fixed}\;\;\;\;\;\;\!\\[0.5em] -\; : \; \textrm{inertially-fixed} \end{matrix} \end{align}
As the finite Quaternion on both sides cancel each other, one obtains
\begin{align} \begin{bmatrix} \;dq_0 \;\\[0.5em] \;d\underline{q}\; \end{bmatrix} = \begin{bmatrix} \;q_0 \;\;&\;\; -\underline{q}^{\textrm{T}}\;\\[0.5em] \;\underline{q} \;\;&\;\; q_0\boldsymbol{\mathsf{I}}_3 \pm \widetilde{\underline{q}}\; \end{bmatrix} \begin{bmatrix} \;0\;\\[0.5em] \;\delta \underline{q}\; \end{bmatrix} \qquad \quad \begin{matrix} +\; : \; \textrm{body-fixed}\;\;\;\;\;\;\!\\[0.5em] -\; : \; \textrm{inertially-fixed} \end{matrix} \end{align}
where the matrix
\begin{align} B = \begin{bmatrix} \;q_0 \;\;&\;\; -\underline{q}^{\textrm{T}}\;\\[0.5em] \;\underline{q} \;\;&\;\; q_0\boldsymbol{\mathsf{I}}_3 \pm \widetilde{\underline{q}}\; \end{bmatrix} \end{align}
is orthogonal [Reference Kecskeméthy, Lenarčič and Galletti31]. Dividing both sides by the time increment
${\textrm{d}}t$
gives in terms of the angular velocity
\begin{align} \begin{bmatrix} \;\dot{q}_0 \;\\[0.5em] \;\dot{\underline{q}}\; \end{bmatrix} = \quad \, \frac{1}{2} \begin{bmatrix} \;-\underline{q}^{\textrm{T}}\;\\[0.5em] \;q_0\boldsymbol{\mathsf{I}}_3 \pm \widetilde{\underline{q}}\; \end{bmatrix} \;\;\underline{\omega } \quad \qquad \quad \begin{matrix} +\; : \; \textrm{body-fixed}\;\;\;\;\;\;\!\\[0.5em] -\; : \; \textrm{inertially-fixed} \end{matrix} \end{align}
with the
$4 \times 3$
matrix
\begin{align}{\boldsymbol{\mathsf{A}}(\,Q\,)} = \frac{1}{2} \begin{bmatrix} \;-\underline{q}^{\textrm{T}}\;\\[0.5em] \;q_0\boldsymbol{\mathsf{I}}_3 \pm \widetilde{\underline{q}}\; \end{bmatrix} \end{align}
mapping angular velocity either in body-fixed or inertially-fixed coordinates to first-order differentials of the Quaternions.
Runge-Kutta integration of angular velocities. With the relationship between angular velocity and time-differential of the Quaternion, it is easy to adapt the classical 4th order Runge-Kutta scheme to a given function of the angular velocity
$\underline{\omega } \,( \, t \, )$
with respect to time for a time step-size
$h$
as [Reference Andrle and Crassidis6]:
\begin{align} Q(t+ h ) \approx Q(t) + \Psi (t) \cdot h \end{align}
with
\begin{align} \Psi (t) = \frac{1}{6} \, \{ \;K_1(t) + 2\, K_2(t) + 2\, K_3(t) + K_4(t)\; \} \end{align}
and
\begin{align} K_1 &= \left [\;\; \boldsymbol{\mathsf{A}}\,\left (\, Q(t) \,\right ) \rule{0pt}{2ex}\rule [-2ex]{0pt}{0pt}\right ]\;\underline{\omega } \, (\,t ) \end{align}
\begin{align} K_2 &= \left [\;\; \boldsymbol{\mathsf{A}}\,\left (\, Q(t) \,\right )\;+\; \frac{h}{2}\,\boldsymbol{\mathsf{A}}\left (\, K_1(t)\,\right )\;\; \right ]\;\underline{\omega } \, (\,t + \frac{h}{2}\, ) \end{align}
\begin{align} K_3 &= \left [\;\; \boldsymbol{\mathsf{A}}\,\left (\, Q(t) \,\right )\;+\; \frac{h}{2}\,\boldsymbol{\mathsf{A}}\left (\, K_2(t)\,\right )\;\; \right ]\;\underline{\omega } \, (\,t + \frac{h}{2}\, ) \end{align}
\begin{align} K_4 &= \left [\;\; \boldsymbol{\mathsf{A}}\,\left (\, Q(t) \,\right )\;+ \,\boldsymbol{\mathsf{A}}\left (\, K_3(t)\,\right )\;\;\rule{0pt}{2ex}\rule [-2ex]{0pt}{0pt}\right ]\;\underline{\omega } \, (\,t + h \,\, ) \end{align}
where after each time step, the resulting approximation of the quaternion is normalized by dividing by its magnitude in order to maintain consistency. Note that when sampling the angular velocity at a sampling time
$\Delta t$
, the minimal time step size for Runge-Kutta integrator is twice the sampling time (
$h = 2 \Delta t$
).
Contrary to common assumption, the integration of angular velocity to final rotations itself does not create a drift, but the reason for the drift lies solely in the sensor errors of the angular velocity. To see this, we integrate by the Runge-Kutta scheme for a motion defined by the three rotation angles in the sequence
$\textrm{Rot}[ x, \varphi ]\;\cdot \;\textrm{Rot}[ y, \Theta ]\;\cdot \;\textrm{Rot}[ z, \psi ]$
as:
\begin{align} \underline{\Phi } (\tau ) = \begin{bmatrix} \varphi \\[1ex] \Theta \\[1ex] \psi \end{bmatrix} = \begin{bmatrix} \pi \sin \;(\;\pi \,\tau \;) \\[1ex] \tfrac{\pi }{4} \sin \; (\;\tfrac{3}{2}\pi \,\tau \; ) \\[1ex] \tfrac{\pi }{2} \sin \;(\;2\pi \,\tau \;) \end{bmatrix} \qquad ; \qquad \underline{\dot{\Phi }} (\tau ) = \begin{bmatrix} \dot{\varphi }\\[1ex] \dot{\Theta } \\[1ex] \dot{\psi } \end{bmatrix} = \begin{bmatrix} \pi ^2 \cos \; (\;\pi \,\tau \; ) \\[1ex] \tfrac{3\pi ^2}{8} \cos \; (\;\tfrac{3}{2}\pi \,\tau \; ) \\[1ex] \pi ^2 \cos \; (\;2\pi \,\tau \; ) \end{bmatrix} \end{align}
over the dimensionless time
$0\leq \tau \leq 6$
, with a step size of 2 ms. The angular velocity
${}^{B}_{}\underline{\omega }_{B}(\tau )$
in body-fixed coordinates is given by the transformation matrix [Reference Kecskeméthy, Lenarčič and Galletti31]
\begin{align} \boldsymbol{\mathsf{T}} = \begin{bmatrix} \ \ \ \cos \,\psi \,\!\cos \,\Theta & \ \ \ \sin \, \psi \ & \quad 0\quad \\ -\sin \,\psi \, \cos \,\Theta & \ \ \ \cos \, \psi \ & \quad 0 \quad \\ \ \ \ \sin \,\Theta \ & \ \ \ \quad 0 \quad & \quad 1\quad \end{bmatrix} \end{align}
as
\begin{align}{}^{B}_{}\underline{\omega }_{B}(\tau ) = \boldsymbol{\mathsf{T}}(\tau ) \; \underline{\dot{\Phi }}(\tau ) \ \ . \end{align}
Starting from the initial Quaternion
$Q\;(\,\tau =0\;) = [1, {\underline{0}}^{\mathrm{T}}]^{\mathrm{T}}$
, the time-scheme Eq. (70) is applied, and time function
$Q(\tau )$
of the Quaternion is computed. By computing the corresponding rotation matrix
${}^{}_{}\boldsymbol{\mathsf{R}}_{B}(\tau )$
according to Eq. (54), the corresponding angles
$\underline{\Phi }^*(\tau )$
are determined by inverse relationship from the rotation matrix to the Euler angles. Figure 12a) and Figure 12b) show the corresponding time histories of the computed versus the given angles and the error between the two, respectively. One can see that the curves are virtually identical and the maximal error is less than
$10^{-8}$
, which is negligible.
Case analysis of Runge-Kutta integration.
Appendix B: Estimation of box orientation by two CCD cameras
The presented method employs image recognition algorithms to determine the initial orientation of a box with a checkerboard print on the large visible face. Obtaining the relative orientation between a calibrated camera and an object through projective observations of model points is regarded as a solved problem in geometric computer vision systems, known as the Perspective n-Point problem (PnP) [Reference Abidi and Chandra1, Reference Fischler, Bolles, Fischler and Firschein19, Reference Horaud, Conio, Leboulleux and Lacolle28, Reference Quan and Lan45]. The minimal PnP case, which allows for a limited number of possible solutions, involves observing three (n = 3) model points in a nondegenerate configuration, commonly referred to as P3P. Since Grunert’s pioneering documentation of a P3P solution in 1841 [Reference Grunert24], many P3P-solutions based on this approach have emerged [Reference Gao, Hou, Tang and Cheng22, Reference Haralick, Lee, Ottenburg and Nolle25, Reference Ke and Roumeliotis30, Reference Kneip, Scaramuzza and Siegwart32]. In this section, the P3P method is briefly reviewed, and a more accurate approach using two cameras and the projective information of the edges of the checkerboard face is introduced and compared to the existing P3P method.
Perspective-3-Point problem: graphical representation of key concepts.
The P3P method determines the absolute orientation
$\boldsymbol{\mathsf{R}}_{T}$
of the frame
${\mathcal K}_{T}$
with respect to an inertial frame
${\mathcal K}_{I}$
can be summarized as follows (Figure 13). Let the given model points
$O$
,
$S$
, and
$P$
be captured by a calibrated camera and chosen such that the lengths of the segments
$\overline{OP}$
,
$\overline{OS}$
, and
$\overline{SP}$
, denoted as
$s$
,
$p$
, and
$o$
, respectively, are known, and that
$\overline{OP}$
and
$\overline{OS}$
run parallel to the
$x$
- and
$z$
-axes of
${\mathcal K}_{T}$
, respectively.
This parallelism ensures that once the vectors
$\underline{k}_{j}$
with
$j\in \left \{ O, P, S \right \}$
, which run from the center of projection
$C$
to the actual world points, are known, the sought-after absolute orientation
$\boldsymbol{\mathsf{R}}_{T}$
can be formed. The desired vectors
$\underline{k}_{j}$
can be described as follows:
\begin{align} \underline{k}_{j} = \Lambda _{j} \cdot \underline{u}_{j} \end{align}
where
$\Lambda _{j}$
represents the unknown length and
$\underline{u}_{j}$
denotes the unknown direction of vector
$\underline{k}_{j}$
.
For a calibrated camera, the principal points
$A^{I}$
, which represents the perpendicular point of the image sensor to the center of projection
$C$
, and
$d^{I}$
, which denotes the distance between the image sensor and the center of projection, are known. Furthermore, the positions of the projections
$O^{I}$
,
$S^{I}$
, and
$P^{I}$
of the model points can be read. As a result, the vectors
$\underline{h}_{j}$
with
$ j\in \left \{ O, P, S \right \}$
, which point from the projection of the points on the image sensor to the center of projection, are known as:
\begin{align} \underline{h}_{O} = \begin{bmatrix} \; A^{I}_{x}-O^{I}_{x} \; \\[0.5em] A^{I}_{y}-O^{I}_{y} \\[0.5em] d^{I} \end{bmatrix} \quad \textrm{ and }\quad \underline{h}_{P} = \begin{bmatrix} \; A^{I}_{x}-P^{I}_{x} \; \\[0.5em] A^{I}_{y}-P^{I}_{y} \\[0.5em] d^{I} \end{bmatrix} \quad \textrm{ and }\quad \underline{h}_{S} = \begin{bmatrix} \; A^{I}_{x}-S^{I}_{x} \; \\[0.5em] A^{I}_{y}-S^{I}_{y} \\[0.5em] d^{I} \end{bmatrix} \end{align}
Given that the unknown vectors
$\underline{k}_{j}$
and known vectors
$\underline{h}_{j}$
of the respective points are parallel to each other, the directions
$\underline{u}_{j}$
of the respective vectors
$\underline{k}_{j}$
can be derived. This leaves only the lengths
$\Lambda _{j}$
as the sought-after unknowns, which can be determined using the cosine theorem with
\begin{align} s^2 &= \Lambda _{O}^{2} \,+\,\Lambda _{P}^{2} \,+\, 2\Lambda _{O}\Lambda _{P} \cos (\epsilon ) \qquad\, \textrm{with}\qquad \cos (\epsilon )\,=\,\underline{u}_{O}\, \cdot \,\underline{u}_{P}\, \end{align}
\begin{align} p^2 &= \Lambda _{O}^{2} \,+\, \Lambda _{S}^{2} \,+\, 2\Lambda _{O}\Lambda _{S} \cos (\gamma ) \qquad \textrm{with}\qquad \cos (\gamma )\,=\,\underline{u}_{O}\, \cdot\, \underline{u}_{S} \end{align}
\begin{align} o^2 &= \Lambda _{P}^{2} \,+\, \Lambda _{S}^{2} \,+\, 2\Lambda _{P}\Lambda _{S} \cos (\sigma ) \qquad\, \textrm{with}\qquad \cos (\sigma )\,=\,\underline{u}_{P}\, \cdot\, \underline{u}_{S} \end{align}
These three inhomogeneous quadratic equations are then solved for the distances
$\Lambda _{j}$
, from which the orientation
$\boldsymbol{\mathsf{R}}_{T}$
can be determined. Most methods in the literature differ primarily in their approach to solving this system of three inhomogeneous quadratic equations. Despite the known limitations of this solution approach in achieving the required accuracy for pose estimation [Reference Persson and Nordberg43], such algorithms continue to be employed in several software solutions, including openCV [42] and MATLAB [39]. However, they are not always very accurate, which is the reason an alternative approach is presented here.
Target and its projection on two image sensors for estimation of gyroscopes initial orientation.
For the alternative method, a flat checkerboard pattern is tracked by two cameras, as illustrated in Figure 14. The reason for employing a checkerboard pattern is that in this way, one has redundant information about the edge directions of the grids, which minimizes errors. The edges of this checkerboard pattern are designed to be parallel to the axes of the gyroscope (see Figure 8 ). This device will henceforth be referred to as the “target,” and the sought orientation will be denoted again as
${}^{}_{}\boldsymbol{\mathsf{R}}_{T}$
. The target is simultaneously photographed by two cameras, referred to as camera I and camera II, whose absolute orientations
$\boldsymbol{\mathsf{R}}_{c}$
, principal point
$A^{c}$
, and image distance
$d^{c}$
with
$c \in \left \{ I, I\! I \right \}$
are known through camera calibration.
The checkerboard pattern aids in the precise determination of the projections of the points
$O$
,
$S$
, and
$P$
, named
$O^{I}$
,
$S^{I}$
, and
$P^{I}$
, respectively, for camera I and
$O^{II}$
,
$S^{II}$
, and
$P^{II}$
, respectively, for camera II. For this, the corner points of the checkerboard pattern are determined in the images of the cameras with subpixel accuracy. First, the Harris Corner Detector is utilized for pixel-level precision, as it identifies regions in the image exhibiting substantial intensity variation in all directions [Reference Harris and Stephens26]. Following this, the Förstner Corner Detector is used to refine the detected points’ positions to sub-millimeter accuracy, using the principle that complementary tangent lines of the checkerboard squares edges intersect at a single point for an ideal corner [Reference Forstner20]. The calculation was executed using the findChessboardCorners function from the OpenCV library [42].
Choosing again partial points
$O$
,
$P$
, and
$S$
among the corner points of checkerboard squares such that
\begin{align} \overrightarrow{OP} = \Delta \underline{p} = s\;\underline{u}_{z} \quad \textrm{ and }\quad \overrightarrow{OS} = \Delta \underline{s} = p\;\underline{u}_{x} \end{align}
run parallel to the
$x$
- and
$z$
-axes of
${\mathcal K}_{T}$
, respectively, one obtains for the coordinates of these two unit directions in the inertial (“world”) frame:
\begin{align}{}^{W}_{}\underline{u}_{x} = \boldsymbol{\mathsf{R}}_{T} \cdot \underline{e}_{x} \quad \textrm{ and }\quad{}^{W}_{}\underline{u}_{z} = \boldsymbol{\mathsf{R}}_{T} \cdot \underline{e}_{z} \end{align}
where
\begin{align} \underline{e}_{x} = \begin{bmatrix} 1\\[0.5em] \;0\;\\[0.5em] \;0\; \end{bmatrix} \quad \textrm{and}\quad \underline{e}_{y} = \begin{bmatrix} 0\\[0.5em] 1\\[0.5em] \;0\; \end{bmatrix} \quad \textrm{and}\quad \underline{e}_{z} = \begin{bmatrix} 0\\[0.5em] \;0\;\\[0.5em] 1 \end{bmatrix} \end{align}
The vectors
$\Delta \underline{p}$
and
$\Delta \underline{s}$
can also be described using the vectors
$\underline{k}_{j}$
with
$j\in \left \{ O, P, S \right \}$
(see Figure 13) as follows:
\begin{align} \Delta \underline{p}^{c} = \underline{k}_{P}^{c} - \underline{k}_{O}^{c} \quad \textrm{ and }\quad \Delta \underline{s}^{c} = \underline{k}_{S}^{c} - \underline{k}_{O}^{c} \end{align}
where
$c$
denotes the respective camera
$\left ( c \in \left \{ I, I\! I \right \} \right )$
to which the vectors refer and in whose coordinates the vectors are described. The two-dimensional projection of
$\Delta \underline{p}$
and
$\Delta \underline{s}$
onto the corresponding image sensor of camera
$c$
, which runs parallel to the
$xy$
-plane of the corresponding camera, is referred to as
$\Delta \underline{\overline{p}}^{c}$
and
$\Delta \underline{\overline{s}}^{c}$
and can be determined by the method of similar triangles as:
\begin{align} \frac{\Delta \overline{p}^{c}_{i}}{d^{c}} = \frac{k_{P,i}^{c}}{k_{P,z}^{c}} - \frac{k_{O,i}^{c}}{k_{O,z}^{c}} \end{align}
\begin{align} \frac{\Delta \overline{s}^{c}_{i}}{d^{c}} = \frac{k_{S,i}^{c}}{k_{S,z}^{c}} - \frac{k_{O,i}^{c}}{k_{O,z}^{c}} \end{align}
with
$i \in \left \{ x, y \right \}$
. By introducing the vectors
$\underline{r}^{c}_{O}$
and
$\underline{r}^{c}_{C}$
, which point from
${\mathcal K}_{W}$
to the point
$C$
of camera
$c$
and point
$O$
respectively (see Figure 13), the vectors
$\underline{k}_{j}$
can be described as:
\begin{align} \underline{k}_{O}^{c} \,=\, \underline{r}^{c}_{O} - \underline{r}^{c}_{C} \qquad \;\;\, \end{align}
\begin{align} \underline{k}_{P}^{c} \,=\, \underline{r}^{c}_{O} + s\;\underline{u}^{c}_{z} - \underline{r}^{c}_{C} \end{align}
\begin{align} \underline{k}_{S}^{c} \;=\; \underline{r}^{c}_{O} + p\;\underline{u}^{c}_{x} - \underline{r}^{c}_{C} \end{align}
where
$\underline{u}^{c}_{x}$
,
$\underline{u}^{c}_{z}$
are the two unit directions of
${\mathcal K}_{T}$
in the respective camera frame
${\mathcal K}_{c}$
. Substituting Eq. (89) to Eq. (91) into Eq. (87) and Eq. (88), it follows that
\begin{align} \frac{\Delta \overline{p}^{c}_{i}}{d^{c}} = \left [ ^{c}\!u_{z,i} - ^{c}\!u_{z,z} \frac{r_{O,i}^{c}-r_{C,i}^{c}}{r_{O,z}^{c}-r_{C,z}^{c}} \right ] \cdot \,\frac{s}{r_{O,z}^{c}+su_{z,z}-r_{C,z}^{c}} \end{align}
\begin{align} \frac{\Delta \overline{s}^{c}_{i}}{d^{c}} = \left [ ^{c}\!u_{x,i} - ^{c}\!u_{x,z} \frac{r_{O,i}^{c}-r_{C,i}^{c}}{r_{O,z}^{c}-r_{C,z}^{c}} \right ] \cdot \,\frac{p}{r_{O,z}^{c}+pu_{x,z}-r_{C,z}^{c}} \end{align}
The method of similar triangles reveals that
\begin{align} \frac{r_{O,i}^{c}-r_{C,i}^{c}}{r_{O,z}^{c}-r_{C,z}^{c}} = \frac{\Delta a^{c}_{i}}{d^{c}} \end{align}
where
$\Delta a^{c}_{i}$
describes the distance of the projection of point
$O$
to the principal point
$A^c$
(see Figure 14). Substituting equations Eq. (92) and Eq. (93) with
$i = x$
and
$i = y$
into themselves gives two binding equations for each camera
\begin{align} g_{1} &\equiv \; \Delta \overline{p}^{I}_{y} \cdot \big [\;\,u^{I}_{z,x} \cdot d^{I} -\, u^{I}_{z,z} \cdot \Delta a^{I}_{x}\;\;\big ] -\Delta \overline{p}^{I}_{x}\cdot \big [\;\,u^{I}_{z,y} \cdot d^{I} -\, u^{I}_{z,z} \cdot \Delta a^{I}_{y}\;\;\big ] \;=\; 0 \end{align}
\begin{align} g_{2} &\equiv \; \Delta \overline{s}^{I}_{y}\; \cdot \big [\;\,u^{I}_{x,x} \cdot d^{I} -\, u^{I}_{x,z} \cdot \Delta a^{I}_{x}\;\,\big ] -\Delta \overline{s}^{I}_{x}\;\cdot \big [\;\,u^{I}_{x,y} \cdot d^{I} -\, u^{I}_{x,z} \cdot \Delta a^{I}_{y}\;\,\big ] \;=\; 0 \end{align}
\begin{align} g_{3} &\equiv \; \Delta \overline{p}^{I\! I}_{y}\cdot \big [\,\,u^{I\! I}_{z,x} \cdot d^{I\! I} -\, u^{I\! I}_{z,z} \cdot \Delta a^{I\! I}_{x}\,\,\big ] -\Delta \overline{p}^{I\! I}_{x}\cdot \big [\,\,u^{I\! I}_{z,y} \cdot d^{I\! I} -\, u^{I\! I}_{z,z} \cdot \Delta a^{I\! I}_{y}\,\,\big ] \;=\; 0 \end{align}
\begin{align} g_{4} &\equiv \; \Delta \overline{s}^{I\! I}_{y}\cdot \big [\,\,u^{I\! I}_{x,x} \cdot d^{I\! I} -\, u^{I\! I}_{x,z} \cdot \Delta a^{I\! I}_{x}\,\,\big ] -\Delta \overline{s}^{I\! I}_{x}\cdot \big [\,\,u^{I\! I}_{x,y} \cdot d^{I\! I} -\, u^{I\! I}_{x,z} \cdot \Delta a^{I\! I}_{y}\,\,\big ] \;=\; 0 \end{align}
which gives an overconstrained system of 4 equations for three independent rotation parameters
\begin{align} \underline{g}\big (\,q_{0},\underline{q}\,\big ) = \underline{0} \end{align}
whose Jacobian with respect to infinitesimal rotation increments
$\delta \underline{\varphi } = \underline{u} \cdot \delta \varphi$
can be shown to be
while
$\underline{e}_{\,c, x}$
,
$\underline{e}_{\,c, y}$
, and
$\underline{e}_{\,c, z}$
represent the
$x$
,
$y$
, and
$z$
axes of the cameras
$c$
$\left ( c\in \left \{ I, I\! I \right \} \right )$
in
${\mathcal K}_{W}$
and
$\underline{u}_{x}$
and
$\underline{u}_{z}$
are given in
${\mathcal K}_{W}$
. Using again the matrix
$\boldsymbol{\mathsf{A}}(\,Q\,)$
mapping infinitesimal rotation increments to Quaternion differentials (see Appendix A), one obtains the following iteration procedure from the Newton-Raphson root-finding procedure of Eq. (99):
\begin{align} Q^{i+1} \;=\; Q^{i} - \boldsymbol{\mathsf{A}}\big (\,Q^{i}\,\big )\;\boldsymbol{\mathsf{J}}^{\dagger }_{g \varphi } \big (\,Q^{i}\,\big )\;\underline{g} \big (\,Q^{i}\,\big ) \end{align}
where
$\boldsymbol{\mathsf{J}}^{\dagger }_{g \varphi }$
is the pseudo inverse of the Jacobian of Eq. (99).
The new method was validated by an experiment and compared to the existing method using a box with a checkerboard pattern on one side. The box was placed in space with ten defined orientations in sequential order. Two calibrated cameras (EOS 2000D, Canon) with known absolute orientations were positioned to capture the checkerboard pattern from each orientation of the box. To create comparable conditions to those expected in a gait lab, the checkerboard pattern measured 30
$\times$
45 mm, with individual squares having a side length of 7.5 mm. The cameras were placed at a distance of 700 to 800 mm from the boxes.
First, the box orientations were determined using the image of one camera and the camera calibration routine integrated into MATLAB (MATLAB version 9.9.0 (R2023b)), which utilizes the first algorithm [39]. Subsequently, the same box orientation was determined by the new algorithm with two cameras. The corresponding error for each pose was calculated as the rotation angle between the estimated orientations and the real orientations of the target.
The initial orientation determination via computer vision algorithms using MATLAB’s toolbox in conjunction with a single camera yielded an accuracy of
$2.03^{\circ } \pm 0.76^{\circ }$
. On the other hand, the new algorithm yielded a precision of
$0.37^{\circ } \pm 0.15^{\circ }$
with an average of 3 Newton iterations for initial values roughly within 30
$^{\circ }$
of the target orientation. In conclusion, the new algorithm is quite stable and outperforms the precision of existing methods by a factor of 5.
Open access
