Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
Orientation from two markers
============================
We have seen in the previous section, how to describe the orientation of a rigid body marked by three markers, i.e. forming a local reference frame (X,Y,Z). Having three markers is not always possible, due to physical constrains such as space and camera resolutions. The task is then to find the solutions (they are not unique) to the system:
.. math::
\begin{equation}
R.v^{ref}=v^{bee}
\end{equation}
here :math:`v^{bee}` is the vector given by the 3D coordinates of the two markers, :math:`v^{ref}` is the vector given by the 3D coordinates of the two markers when the orientation of the agent is null, i.e. the orientation of the matrix is equal to the identity matrix.
The system has 3 equations and 9 unknown variables: the elements of the orientation matrix.
**Note** Why 9 unknown variables if we have three rotation angles? The rotation angles appear in the orientation matrix in cosine and sine function, both nonlinear function. Therefore the sine and the cosine of the rotation angle have to be treated separatly. Moreover the multiplication of two cosines, two sine, or one cosine and one sine has to be treated has variables, because multiplication is... nonlinear.
.. math::
\begin{align}
v_x^{bee} & = +v_x^{ref} \cos\alpha \cos\beta + v_y^{ref}(\cos\alpha \sin\beta \sin\gamma - \sin\alpha \cos\gamma) + v_z^{ref} (\cos\alpha \sin\beta \cos\gamma + \sin\alpha \sin\gamma) \\
v_y^{bee} & = +v_x^{ref} \sin\alpha \cos\beta + v_y^{ref}(\sin\alpha \sin\beta \sin\gamma + \cos\alpha \cos\gamma) + v_z^{ref} ( \sin\alpha \sin\beta \cos\gamma - \cos\alpha \sin\gamma )\\
v_z^{bee} & = -v_x^{ref}\sin\beta + v_y^{ref} \cos\beta \sin\gamma + v_z^{ref} \cos\beta \cos\gamma
\end{align}
or equivalently we can look at :math:`v^{ref}=R^Tv^{bee}`, because :math:`R^T=R^{-1}`
.. math::
\begin{align}
v_x^{ref} & = +v_x^{bee} \cos\alpha \cos\beta +v_y^{bee} \sin\alpha \cos\beta -v_z^{bee}\sin\beta \\
v_y^{ref} & = +v_x^{bee}(\cos\alpha \sin\beta \sin\gamma - \sin\alpha \cos\gamma) +v_y^{bee}(\sin\alpha \sin\beta \sin\gamma + \cos\alpha \cos\gamma)+ v_z^{bee} \cos\beta \sin\gamma \\
v_z^{ref} & = +v_x^{bee}(\cos\alpha \sin\beta \cos\gamma + \sin\alpha \sin\gamma) + v_y^{bee} ( \sin\alpha \sin\beta \cos\gamma - \cos\alpha \sin\gamma )+v_z^{bee} \cos\beta \cos\gamma
\end{align}
To simplify the problem, we need to remove terms in the system of equations. Removing terms is easily done by letting certain variables to be zero. We want to determine :math:`\alpha`, :math:`\beta`, and :math:`\gamma`. Thus we can only set to zeros :math:`v_x^{ref}`, :math:`v_y^{ref}`, or :math:`v_z^{ref}`.
A simple case
-------------
If we assume that the two markers are aligned with the roll axis,
i.e. :math:`v^{ref}=(1,0,0)^T`, the problem can easily be solve as follow:
.. math::
\begin{align}
v_x^{bee} & = +\cos\alpha \cos\beta \\
v_y^{bee} & = +\sin\alpha \cos\beta \\
v_z^{bee} & = -\sin\beta
\end{align}
.. math::
\begin{align}
\tan\alpha & = \frac{\pm v_y^{bee}}{\pm v_x^{bee}} &\quad \text{from L1 and L2} \\
\tan\beta & = \frac{-v_z^{bee}}{\pm\sqrt{v_y^{bee}+v_x^{bee} }} &\quad\text{from all}
\end{align}
We remark that the solution does not depend of the angle :math:`\gamma`, \
the roll angle. Thus, when we do not care about the full orientation of \
a body but simply care about pitch and yaw, two markers are sufficients.
Other cases
-----------
The two markers may be aligned with the pitch (or yaw axis) of the body. \
In such situation, the problem is slightly more complex. We can, still \
solve the system of equation, but can not find a solution independent of \
the pitch (or yaw angle), we need to guess the pitch (or yaw angle).
Angles from two markers
-----------------------
Here we illustrate, how the solution of the system in three cases: \
roll-aligned, pitch-aligned, and yaw-aligned, varies as a function of \
the a priori known angle.
Note that the a priori known angle is the third angle of a convention internally \
used by the orientation estimater, namely:
* The rotation around x, with convention :math:`R_zR_yR_x`, for x-aligned markers
* The rotation around y, with convention :math:`R_zR_xR_y`, for y-aligned markers
* The rotation around z, with convention :math:`R_yR_xR_z`, for x-aligned markers
Thus the none of the yaw pitch roll angles may match of the a priori \
known angle (except for x-aligned markers, because the internal convention is the yaw pitch roll convention)
.. literalinclude:: examples/two_markers.py
:lines: 5, 13, 43-44
here the alignment is defined by a string:
.. literalinclude:: examples/two_markers.py
:lines: 36, 47, 58
.. plot:: orientation/examples/two_markers.py
Solving pitch aligned markers with yaw, pitch, roll convention
--------------------------------------------------------------
:math:`v^{ref}=(0,-1,0)^T`
.. math::
\begin{align}
v_x^{bee} & = -\cos\alpha \sin\beta \sin\gamma + \sin\alpha \cos\gamma &\quad \text{from L1}\\
v_y^{bee} & = -\sin\alpha \sin\beta \sin\gamma - \cos\alpha \cos\gamma &\quad \text{from L2}\\
v_z^{bee} & = -\cos\beta \sin\gamma &\quad \text{from L3}
\end{align}
for :math:`v_z^{bee}\neq0 \Rightarrow \sin\gamma\neq0`
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
.. math::
\begin{align}
\cos\gamma & = v_x^{bee}\sin\alpha -v_y^{bee}\cos\alpha &\quad \text{from L1 and L2} \\
\cos\beta & = -\frac{v_z^{bee}}{\sin\gamma } &\quad\text{from L3}
\end{align}
From the two last equation :math:`\gamma` and :math:`\beta` can be found as a function of :math:`\alpha` and :math:`v^{bee}`
for :math:`v_z^{bee}=0`
~~~~~~~~~~~~~~~~~~~~~~~
.. math::
\begin{align}
\tan\alpha & = -\frac{v_x^{bee}}{v_y^{bee}} &\quad \text{from L1 and L2}\\
\cos\gamma & =\pm\sqrt{ \left(v_x^{bee}\right)^2 + \left(v_y^{bee}\right)^2 } &\quad \text{from L1 and L2}\\
\end{align}
for :math:`\beta` known, and :math:`\beta\neq\pm\pi/2`:
.. math::
\begin{align}
\sin\gamma & = \frac{v_z^{bee}}{-\cos\beta} \\
\tan\left(\alpha+\theta\right) & = \frac{v_x^{bee}}{-v_y^{bee}} \\
\tan\theta&=\frac{v_z^{bee}\tan\beta}{\pm\sqrt{1-\sin^2\gamma}}
\end{align}
for :math:`\gamma` known, and :math:`\gamma\neq 0 + k2pi`:
.. math::
\begin{align}
\sin\left(\alpha+\theta\right) & = \frac{\cos\gamma}{\sqrt{ \left(v_x^{bee}\right)^2+\left(v_y^{bee}\right)^2}} \\
\cos\beta & = -\frac{v_z^{bee}}{\sin\gamma } \\
\tan\theta&=\frac{v_y^{bee}}{v_x^{bee}}
\end{align}