Skip to content
Snippets Groups Projects
Commit 72ee9eb3 authored by Olivier Bertrand's avatar Olivier Bertrand
Browse files

Add doc on orientation as tutorials

parent 73416df1
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
doc/source/tutorials/02a-orientation-background.ipynb 0 → 100644
+ 533
− 0
View file @ 72ee9eb3
Source diff could not be displayed: it is too large. Options to address this: view the blob.
doc/source/tutorials/02b-orientation-3markers.ipynb 0 → 100644
+ 177
− 0
View file @ 72ee9eb3
%% Cell type:markdown id: tags:
## Orientation from 3 markers
### Example of a marked bee
Bumblebees are eusocial insects. The bees of the specie *bombus terrestris* have their hive underground, and can access it by a hole. When leaving the hive, they perform a learning and orientation flight to remember the location of the hive entrance and decide in which direction they should forage or explore the environment. Bees can be filmed while perforing their learning flight. As we have seen before, 3 points or markers need to be placed on the bee's thorax to determine its orientation. Bees have therefore been marked with three points, forming a triangle, on their thorax
.. image:: media/marked_bumblebee.png
**Note** The dataset used in this example come from recording made at the University of Bielefeld in Martin Egelhaaf's group, by Charlotte Doussot.
### Position of the thorax's markers along time
**Note** due to occlusion during the experiment, at several time-points, the positions of the markers have not been recorded.
%% Cell type:code id: tags:
``` python
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt
import pkg_resources
%matplotlib inline
bumblebee_flight = pkg_resources.resource_filename(
'navipy', 'resources/sample_experiment/Doussot_2017/bee007.hdf')
markers = pd.read_hdf(bumblebee_flight, 'markers')
markers_thorax = [0, 1, 2]
fig, axarr = plt.subplots(1, len(markers_thorax),
figsize=(15, 3),
sharey=True)
for plt_i, cmark_name in enumerate(markers_thorax):
markers.loc[:, cmark_name].plot(ax=axarr[plt_i],
linewidth=2)
axarr[plt_i].set_title(cmark_name)
axarr[plt_i].set_xlabel('time [frame]')
axarr[plt_i].set_ylabel('position [mm]')
```
%% Output
%% Cell type:markdown id: tags:
### Yaw, pitch and roll along time
We use the markers from above. To calculate the orientation, the euler axis convention as well as how our
markers are placed relative to the three rotational axis of the thorax.
%% Cell type:code id: tags:
``` python
rotconv = 'rzyx'
triangle_mode = 'y-axis=2-1'
```
%% Cell type:markdown id: tags:
We can use get yaw-pitch-roll from markers by building the trajectory out of the markers
%% Cell type:code id: tags:
``` python
from navipy.trajectories import Trajectory
```
%% Output
%% Cell type:code id: tags:
``` python
mytrajectory = Trajectory(rotconv=rotconv).from_markers(markers, triangle_mode, markers_labels=markers_thorax)
```
%% Cell type:markdown id: tags:
The angles are in radians. Some persons prefer to read angle in degrees
%% Cell type:code id: tags:
``` python
np.rad2deg(mytrajectory.loc[:, rotconv].astype(float)).plot()
```
%% Output
<matplotlib.axes._subplots.AxesSubplot at 0x7fd2004cee80>
doc/source/tutorials/02c-orientation-2markers.ipynb 0 → 100644
+ 347
− 0
View file @ 72ee9eb3
%% Cell type:markdown id: tags:
## 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:
$$
\begin{equation}
R.v^{ref}=v^{bee}
\end{equation}
$$
here $v^{bee}$ is the vector given by the 3D coordinates of the two markers, $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.
$$\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 $v^{ref}=R^Tv^{bee}$, because $R^T=R^{-1}$
$$\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 $\alpha$, $\beta$, and $\gamma$. Thus we can only set to zeros $v_x^{ref}$, $v_y^{ref}$, or $v_z^{ref}$.
### A simple case
If we assume that the two markers are aligned with the roll axis,
i.e. $v^{ref}=(1,0,0)^T$, the problem can easily be solve as follow:
$$
\begin{align}
v_x^{bee} & = +\cos\alpha \cos\beta \\
v_y^{bee} & = +\sin\alpha \cos\beta \\
v_z^{bee} & = -\sin\beta
\end{align}
$$
$$
\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 $\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).
%% Cell type:markdown id: tags:
## 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 $R_zR_yR_x$, for x-aligned markers
* The rotation around y, with convention $R_zR_xR_y$, for y-aligned markers
* The rotation around z, with convention $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)
%% Cell type:code id: tags:
``` python
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt
from navipy.maths.homogeneous_transformations import compose_matrix
import navipy.trajectories.transformations as mtf
from navipy.trajectories.triangle import Triangle
```
%% Cell type:markdown id: tags:
Similar to the notebook about the background of the orientation, we place a triangle at a known position orientation
%% Cell type:code id: tags:
``` python
yaw_t = +3*np.pi/4
pitch_t = -1*np.pi/6
roll_t = -1*np.pi/12
angles = [yaw_t, pitch_t, roll_t]
euler_axes = 'rzyx'
# Create a triangle in a given orientation
# and get the x,y,z axis used as our two markers
triangle_mode = 'x-axis=median-from-0'
transform = compose_matrix(angles=angles,
axes=euler_axes)
markers = pd.Series(data=0,
index=pd.MultiIndex.from_product(
[[0, 1, 2], ['x', 'y', 'z']]))
markers.loc[(0, 'x')] = -1
markers.loc[(2, 'y')] = np.sin(np.pi / 3)
markers.loc[(1, 'y')] = -np.sin(np.pi / 3)
markers.loc[(1, 'x')] = np.cos(np.pi / 3)
markers.loc[(2, 'x')] = np.cos(np.pi / 3)
equilateral = Triangle(markers.loc[0],
markers.loc[1],
markers.loc[2])
equilateral.transform(transform)
_, x_axis, y_axis, z_axis = mtf.triangle2bodyaxis(
equilateral, triangle_mode)
```
%% Cell type:markdown id: tags:
If can only can get the position of two markers out of three, we have only access to a vector in 3D space and not a triangle.
We need to assume that this vector is align to one axis, and that one of the three Euler angles is known. When we do not know the angle, we can look at all possible angles, for our known angles.
%% Cell type:code id: tags:
``` python
known_angles = np.linspace(-np.pi, np.pi, 180)
```
%% Cell type:markdown id: tags:
Let's assume that the two markers are aligned with one the axes
1. along the x-axis,
2. along the y-axis, or
3. along the z-axis.
%% Cell type:code id: tags:
``` python
axis_alignement = 'x-axis'
mark0 = pd.Series(data=0, index=['x', 'y', 'z'])
mark1 = pd.Series(x_axis, index=['x', 'y', 'z'])
solution_x_axis = pd.DataFrame(index=known_angles, columns=['yaw',
'pitch',
'roll'])
for known_angle in known_angles:
angles_estimate = mtf.twomarkers2euler(
mark0, mark1, axis_alignement, known_angle, euler_axes)
solution_x_axis.loc[known_angle, :] = angles_estimate
```
%% Cell type:code id: tags:
``` python
axis_alignement = 'y-axis'
mark0 = pd.Series(data=0, index=['x', 'y', 'z'])
mark1 = pd.Series(y_axis, index=['x', 'y', 'z'])
solution_y_axis = pd.DataFrame(index=known_angles, columns=['yaw',
'pitch',
'roll'])
for known_angle in known_angles:
angles_estimate = mtf.twomarkers2euler(
mark0, mark1, axis_alignement, known_angle, euler_axes)
solution_y_axis.loc[known_angle, :] = angles_estimate
```
%% Cell type:code id: tags:
``` python
axis_alignement = 'z-axis'
mark0 = pd.Series(data=0, index=['x', 'y', 'z'])
mark1 = pd.Series(z_axis, index=['x', 'y', 'z'])
solution_z_axis = pd.DataFrame(index=known_angles, columns=['yaw',
'pitch',
'roll'])
for known_angle in known_angles:
angles_estimate = mtf.twomarkers2euler(
mark0, mark1, axis_alignement, known_angle, euler_axes)
solution_z_axis.loc[known_angle, :] = angles_estimate
```
%% Cell type:markdown id: tags:
The possible two other angles are shown below, as a function of the known angle
%% Cell type:code id: tags:
``` python
fig, axarr = plt.subplots(1, 3, figsize=(15, 4),
sharey=True)
ax = axarr[0]
solution_x_axis.plot(ax=ax)
ax.set_title('x-aligned')
ax = axarr[1]
solution_y_axis.plot(ax=ax)
ax.set_title('y-aligned')
ax = axarr[2]
solution_z_axis.plot(ax=ax)
ax.set_title('z-aligned')
for ax in axarr:
ax.set_xlabel('input angle [rad]')
ax.set_ylabel('euler angle [rad]')
```
%% Output
%% Cell type:markdown id: tags:
We observe that when the markers are aligned with the x-axis, the yaw and pitch can be determined (without having to guess the roll), because yaw and pitch are constant. However we can not obtain the roll angle.
When the two markers are aligned with the y-axis or z-axis, the pitch or the yaw needs to be apriori known to get the two other angles, repectively.
%% Cell type:markdown id: tags:
## Solving pitch aligned markers with yaw, pitch, roll convention
$v^{ref}=(0,-1,0)^T$
$$\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 $v_z^{bee}\neq0 \Rightarrow \sin\gamma\neq0$
$$\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 $\gamma$ and $\beta$ can be found as a function of $\alpha$ and $v^{bee}$
### for $v_z^{bee}=0$
$$\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 $\beta$ known, and $\beta\neq\pm\pi/2$:
$$\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 $\gamma$ known, and $\gamma\neq 0 + k2pi$:
$$\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}$$
navipy/trajectories/__init__.py
+ 14
− 5
View file @ 72ee9eb3
...@@ -382,18 +382,26 @@ class Trajectory(pd.DataFrame): ...@@ -382,18 +382,26 @@ class Trajectory(pd.DataFrame):
raise KeyError('df should contains q_2 or alpha_2') raise KeyError('df should contains q_2 or alpha_2')
return self return self
def from_markers(self, markers, triangle_mode, error=None): def from_markers(self, markers, triangle_mode,
error=None, markers_labels=[0, 1, 2]):
indeces = markers.index
# Reinit the pandas dataframe super class
# because we now know the indeces
super().__init__(index=indeces, columns=self.columns, dtype=float)
# If error is provided, we can propagate the error
if error is not None: if error is not None:
self.trajectory_error = pd.DataFrame(data=np.nan, self.trajectory_error = pd.DataFrame(data=np.nan,
index=self.index, index=self.index,
columns=self.columns) columns=self.columns)
mark0 = markers.loc[:, 0] markers2use = markers.loc[:, markers_labels]
mark1 = markers.loc[:, 1] markers2use = markers2use.dropna()
mark2 = markers.loc[:, 2] mark0 = markers2use.loc[:, markers_labels[0]]
mark1 = markers2use.loc[:, markers_labels[1]]
mark2 = markers2use.loc[:, markers_labels[2]]
x = np.zeros(9) # 3points with x,y,z x = np.zeros(9) # 3points with x,y,z
kwargs = {'triangle_mode': triangle_mode, kwargs = {'triangle_mode': triangle_mode,
'euler_axes': self.rotation_mode} 'euler_axes': self.rotation_mode}
for index_i in self.index: for index_i in markers2use.index:
# Assign mark to pos # Assign mark to pos
x[0:3] = mark0.loc[index_i, ['x', 'y', 'z']].values x[0:3] = mark0.loc[index_i, ['x', 'y', 'z']].values
x[3:6] = mark1.loc[index_i, ['x', 'y', 'z']].values x[3:6] = mark1.loc[index_i, ['x', 'y', 'z']].values
...@@ -420,6 +428,7 @@ class Trajectory(pd.DataFrame): ...@@ -420,6 +428,7 @@ class Trajectory(pd.DataFrame):
self.loc[index_i, 'location'] = position self.loc[index_i, 'location'] = position
self.loc[index_i, self.rotation_mode] = orientation self.loc[index_i, self.rotation_mode] = orientation
return self
def world2body(self, markers): def world2body(self, markers):
""" Transform markers in world coordinate to body coordinate """ Transform markers in world coordinate to body coordinate
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment