Skip to content
Snippets Groups Projects
Commit cc7e1161 authored by Luise Odenthal's avatar Luise Odenthal
Browse files

added all functions to euler, but test not checked yet

parent 3570f30b
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
doc/source/tutorials/opticFlow_tutorial.ipynb
+ 343
− 14
View file @ cc7e1161
%% Cell type:markdown id: tags:
# Using the Optic Flow function
This Tutorial explains how to use the optic flow function of the navipy toolbox.
The function computes the optic for a positions with an already rendered scene.
Therefore if the optic flow of a whole trajectory is wanted, this function has to be called in a loop for each step.
So to get the optic flow we need:
* the viewing direction (in radians)
* the already rendered scene
* velocity of the bee including the accaleration
The constructor of the optic_flow function is the following:
%% Cell type:code id: tags:
``` python
optic_flow(scene, viewing_directions, velocity, distance_channel=3)
```
%% Output
---------------------------------------------------------------------------
NameError Traceback (most recent call last)
<ipython-input-1-b9f43b3f8149> in <module>()
----> 1 optic_flow(scene, viewing_directions, velocity, distance_channel=3)
NameError: name 'optic_flow' is not defined
%% Cell type:markdown id: tags:
* scene: ibpc
* param viewing_directions: viewing direction of each pixel
(azimuth,elevation) in radians
* param velocity: pandas series
(x,y,z,alpha,beta,gamma,dx,dy,dz,dalpha,dbeta,dgamma)
* distance channel: distance; possible choices= 0,1,2,3; default :3
%% Cell type:markdown id: tags:
The scenes must be of h x w 4 x 1 where h is the hight of image and w the width. The third dimension is needed for the different channels. We have R, G, B, D. Where R is the red, G is the green, B is the blue and D is the distance channel. Here is an example for how to initialize a random scene with 180 x 180 dimension.
For an example on how to get a rendered scene please refer to the other tutorials.
%% Cell type:code id: tags:
``` python
import numpy as np
scene = np.random.random((180, 360, 4, 1))
```
%% Cell type:markdown id: tags:
To set up the viewing direction with a 180 x 180 scene, the following can be done:
%% Cell type:code id: tags:
``` python
elevation = np.arange(-np.pi/2, np.pi/2, 2*(np.pi/360))
azimuth = np.arange(0, 2*np.pi, 2*(np.pi/360))
viewing_directions = np.zeros((180, 2))
viewing_directions[:, 0] = elevation
viewing_directions[:, 1] = azimuth[0:180]
```
%% Cell type:markdown id: tags:
The velocity contains the velocity of the current position as well as the acceleration (difference of velocity to the old position). The velocity must be a pandas dataframe with a multiindex, where the angles index must contain the convention to be used.
The conventions that can be used are:
'sxyz', 'sxyx', 'sxzy','sxzx', 'syzx', 'syzy',
'syxz', 'syxy', 'szxy', 'szxz', 'szyx', 'szyz',
'rzyx', 'rxyx', 'ryzx', 'rxzx', 'rxzy', 'ryzy',
'rzxy', 'ryxy', 'ryxz', 'rzxz', 'rxyz', 'rzyz'
Here is an example on how to initilize it. In this example the bee is rotating around the x axis (yaw) and stays constant otherwise.
%% Cell type:code id: tags:
``` python
import pandas as pd
# only the yaw is changes aka the bee is tilted up
positions = np.array([[-20, -20, 2.6, 1.57079633, 0, 0],
[-20, -20, 2.6, 1.90079633, 0, 0]])
x = positions[0, 0]
y = positions[0, 1]
z = positions[0, 2]
yaw = positions[0, 3]
pitch = positions[0, 4]
roll = positions[0, 5]
dx = positions[1, 0]-positions[0, 0]
dy = positions[1, 1]-positions[0, 1]
dz = positions[1, 2]-positions[0, 2]
dyaw = positions[1, 3]-positions[0, 3]
dpitch = positions[1, 4]-positions[0, 4]
droll = positions[1, 5]-positions[0, 5]
tuples = [('location', 'x'), ('location', 'y'),
('location', 'z'), ('location', 'dx'),
('location', 'dy'), ('location', 'dz'),
('rxyz', 'alpha_0'), ('rxyz', 'alpha_1'),
('rxyz', 'alpha_2'), ('rxyz', 'dalpha_0'),
('rxyz', 'dalpha_1'), ('rxyz', 'dalpha_2')]
index = pd.MultiIndex.from_tuples(tuples,
names=['position', 'orientation'])
velocity = pd.Series(index=index)
velocity['location']['x'] = x
velocity['location']['y'] = y
velocity['location']['z'] = z
velocity['rxyz']['alpha_0'] = yaw
velocity['rxyz']['alpha_1'] = pitch
velocity['rxyz']['alpha_2'] = roll
velocity['location']['dx'] = dx
velocity['location']['dy'] = dy
velocity['location']['dz'] = dz
velocity['rxyz']['dalpha_0'] = dyaw
velocity['rxyz']['dalpha_1'] = dpitch
velocity['rxyz']['dalpha_2'] = droll
```
%% Cell type:markdown id: tags:
So then the optic flow function can be called as follows:
%% Cell type:code id: tags:
``` python
from navipy.processing import mcode
rof, hof, vof = mcode.optic_flow(scene, viewing_directions,
velocity)
```
%% Cell type:markdown id: tags:
Here is an example that loads a scene from the database and calculates the horizontal, vertical and rotational optic flow
%% Cell type:code id: tags:
``` python
# Load the necessary modules
from navipy.processing import mcode
from navipy.database import DataBaseLoad
from matplotlib.image import imsave
import numpy as np
import os
import matplotlib.pyplot as plt
# Load the database, and specify the
# the output directory to save the list
# of images
import pkg_resources
%matplotlib inline
database = pkg_resources.resource_filename(
'navipy',
'resources/database.db')
mydb = DataBaseLoad(database)
my_scene = mydb.scene(rowid=2)
rof, hof, vof = mcode.optic_flow(my_scene, viewing_directions,
velocity)
f, axarr = plt.subplots(2, 2, figsize=(15, 4))
plt.subplot(221)
to_plot_im = my_scene[:, :, :3, 0].astype(float)
to_plot_im = my_scene[:, :, :3, 0]
to_plot_im -= to_plot_im.min()
to_plot_im /= to_plot_im.max()
to_plot_im = to_plot_im * 255
to_plot_im = to_plot_im.astype(np.uint8)
to_plot_dist = my_scene[:, :, 3, 0]
plt.imshow(to_plot_im)
plt.subplot(222)
plt.imshow(hof, cmap='gray')
plt.subplot(223)
plt.imshow(vof, cmap='gray')
plt.subplot(224)
plt.imshow(rof, cmap='gray')
```
%% Output
<matplotlib.image.AxesImage at 0x7f5282afeef0>
<matplotlib.image.AxesImage at 0x7fa2a919fe48>
%% Cell type:markdown id: tags:
# Example with a trajectory
%% Cell type:code id: tags:
``` python
from scipy.io import loadmat
import numpy as np
import os
from navipy.trajectories import Trajectory
import pkg_resources
# Use the trafile from the resources
# You can adapt this code, by changing trajfile
# with your own trajectory file
trajfile = pkg_resources.resource_filename(
'navipy',
'resources/sample_experiment/Lobecke_JEB_2018/Y101_OBFlight_0001.mat')
csvtrajfile, _ = os.path.splitext(trajfile)
csvtrajfile = csvtrajfile+'.csv'
mymat = loadmat(trajfile)
# matlab files are loaded in a dictionary
# we need to identify, under which key the trajectory has been saved
print(mymat.keys())
key = 'trajectory'
# Arrays are placed in a tupple. We need to access the level
# of the array itself.
mymat = mymat[key][0][0][0]
# The array should be a numpy array, and therefore as the .shape function
# to display the array size:
print(mymat.shape)
# In this example the array has 7661 rows and 4 columns
# the columns are: x,y,z, yaw. Therefore pitch and roll were assumed to be constant (here null)
# We can therefore init a trajectory with the convention for rotation
# often yaw-pitch-roll (i.e. 'rzyx') and with indeces the number of sampling points we have
# in the trajectory
rotconvention = 'rzyx'
indeces = np.arange(0,mymat.shape[0])
mytraj = Trajectory(rotconv = rotconvention, indeces=indeces)
# We now can assign the values
mytraj.x = mymat[:,0]
mytraj.y = mymat[:,1]
mytraj.z = mymat[:,2]
mytraj.alpha_0 = mymat[:,3]
mytraj.alpha_1 = 0
mytraj.alpha_2 = 0
# We can then save mytraj as csv file for example
mytraj.to_csv(csvtrajfile)
mytraj.head()
```
%% Output
dict_keys(['__globals__', '__version__', 'trajectory', 'cylinders', 'nests', '__header__'])
(7661, 4)
conv rzyx
location rzyx
x y z alpha_0 alpha_1 alpha_2
0 3.056519 -214.990482 9.330593 2.79751 0 0
1 4.611665 -215.020314 8.424138 2.80863 0 0
2 4.556650 -214.593236 9.185016 2.81407 0 0
3 4.643091 -213.829769 10.542035 2.82704 0 0
4 4.647302 -214.431592 7.461187 2.82896 0 0
%% Cell type:markdown id: tags:
Then we need the velocity to calculate the optic flow
%% Cell type:code id: tags:
``` python
vel = mytraj.velocity()
vel.head()
```
%% Output
dx dy dz dalpha_0 dalpha_1 dalpha_2
0 NaN NaN NaN NaN NaN NaN
1 1.555146 -0.029831 -0.906455 0.01112 0.0 0.0
2 -0.055015 0.427078 0.760878 0.00544 0.0 0.0
3 0.086441 0.763467 1.357019 0.01297 0.0 0.0
4 0.004211 -0.601823 -3.080847 0.00192 0.0 0.0
%% Cell type:markdown id: tags:
Iterate through the trajectory to get the optic flow of each scene
%% Cell type:code id: tags:
``` python
tuples = [('location', 'x'), ('location', 'y'),
('location', 'z'), ('location', 'dx'),
('location', 'dy'), ('location', 'dz'),
('rxyz', 'alpha_0'), ('rxyz', 'alpha_1'),
('rxyz', 'alpha_2'), ('rxyz', 'dalpha_0'),
('rxyz', 'dalpha_1'), ('rxyz', 'dalpha_2')]
index = pd.MultiIndex.from_tuples(tuples,
names=['position', 'orientation'])
for i in range(1,10):
velocity = pd.Series(index=index)
velocity['location']['x'] = mytraj.x[i]
velocity['location']['y'] = mytraj.y[i]
velocity['location']['z'] = mytraj.z[i]
velocity['rxyz']['alpha_0'] = mytraj.alpha_0[i]
velocity['rxyz']['alpha_1'] = mytraj.alpha_1[i]
velocity['rxyz']['alpha_2'] = mytraj.alpha_2[i]
velocity['location']['dx'] = vel.dx[i]
velocity['location']['dy'] = vel.dy[i]
velocity['location']['dz'] = vel.dz[i]
velocity['rxyz']['dalpha_0'] = vel.dalpha_0[i]
velocity['rxyz']['dalpha_1'] = vel.dalpha_1[i]
velocity['rxyz']['dalpha_2'] = vel.dalpha_2[i]
my_scene = mydb.scene(rowid=i)
rof, hof, vof = mcode.optic_flow(my_scene, viewing_directions,
velocity)
```
%% Cell type:code id: tags:
``` python
```
......
navipy/maths/new_euler.py
+ 94
− 89
View file @ cc7e1161
import numpy as np
import navipy.maths.constants as constants
import navipy.maths.quaternion as quat
from navipy.scene import is_numeric_array
c = np.cos
s = np.sin
def getAxes(axes='rxyz'):
if axes not in list(constants._AXES2TUPLE.keys()):
raise Exception("the chosen convention is not supported")
_AXES2TUPLE = {
'sxyz': (0, 0, 1, 2), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 0, 2, 1),
'sxzx': (0, 0, 2, 0), 'syzx': (0, 1, 2, 0), 'syzy': (0, 1, 2, 1),
'syxz': (0, 1, 0, 2), 'syxy': (0, 1, 0, 1), 'szxy': (0, 2, 0, 1),
'szxz': (0, 2, 0, 2), 'szyx': (0, 2, 1, 0), 'szyz': (0, 2, 1, 2),
'rzyx': (1, 2, 1, 0), 'rxyx': (1, 0, 1, 0), 'ryzx': (1, 1, 2, 0),
'rxzx': (1, 0, 2, 0), 'rxzy': (1, 0, 2, 1), 'ryzy': (1, 1, 2, 1),
'rzxy': (1, 2, 0, 1), 'ryxy': (1, 1, 2, 1), 'ryxz': (1, 1, 0, 2),
'rzxz': (1, 2, 0, 2), 'rxyz': (1, 0, 1, 2), 'rzyz': (1, 2, 1, 2)}
return _AXES2TUPLE[axes]
_AXES2TUPLE = {
'sxyz': (0, 0, 1, 2), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 0, 2, 1),
'sxzx': (0, 0, 2, 0), 'syzx': (0, 1, 2, 0), 'syzy': (0, 1, 2, 1),
'syxz': (0, 1, 0, 2), 'syxy': (0, 1, 0, 1), 'szxy': (0, 2, 0, 1),
'szxz': (0, 2, 0, 2), 'szyx': (0, 2, 1, 0), 'szyz': (0, 2, 1, 2),
'rzyx': (1, 2, 1, 0), 'rxyx': (1, 0, 1, 0), 'ryzx': (1, 1, 2, 0),
'rxzx': (1, 0, 2, 0), 'rxzy': (1, 0, 2, 1), 'ryzy': (1, 1, 2, 1),
'rzxy': (1, 2, 0, 1), 'ryxy': (1, 1, 2, 1), 'ryxz': (1, 1, 0, 2),
'rzxz': (1, 2, 0, 2), 'rxyz': (1, 0, 1, 2), 'rzyz': (1, 2, 1, 2)}
def R1(a):
......@@ -50,19 +44,9 @@ def R3(a):
def rotation_matrix(ai, aj, ak, axes='rxyz'):
if not isinstance(ai, float) and not isinstance(ai, int):
raise TypeError("euler angle must be of type float")
if not isinstance(aj, float) and not isinstance(aj, int):
raise TypeError("euler angle must be of type float")
if not isinstance(ak, float) and not isinstance(ak, int):
raise TypeError("euler angle must be of type float")
if np.isnan(np.array([ai], dtype=np.float64)) or\
np.isnan(np.array([aj], dtype=np.float64)) or\
np.isnan(np.array([ak], dtype=np.float64)):
raise ValueError("quaternions must not be nan or none")
if axes not in list(constants._AXES2TUPLE.keys()):
if axes not in list(_AXES2TUPLE.keys()):
raise Exception("the chosen convention is not supported")
r, i, j, k = getAxes(axes)
r, i, j, k = _AXES2TUPLE[axes]
matrixes = [R1, R2, R3]
Rijk = np.dot(matrixes[i](ai),
np.dot(matrixes[j](aj),
......@@ -73,7 +57,7 @@ def rotation_matrix(ai, aj, ak, axes='rxyz'):
return np.transpose(Rijk)
def from_matrix(matrix, axes='sxyz'):
def from_matrix_new(matrix, axes='sxyz'):
"""Return Euler angles from rotation matrix for specified axis sequence.
axes : One of 24 axis sequences as string or encoded tuple
......@@ -82,51 +66,72 @@ def from_matrix(matrix, axes='sxyz'):
"""
if not isinstance(matrix, np.ndarray) and not isinstance(matrix, list):
raise TypeError("matrix must be np.array or list")
if axes not in list(constants._AXES2TUPLE.keys()):
if axes not in list(_AXES2TUPLE.keys()):
raise Exception("the chosen convention is not supported")
if np.any(np.isnan(np.array(matrix, dtype=np.float64))):
raise ValueError('posorient must not contain nan')
# if np.any(np.isnan(np.array(matrix, dtype=np.float64))):
# raise ValueError('posorient must not contain nan')
if not is_numeric_array(matrix):
raise ValueError("matrix must contain numeric values")
try:
firstaxis, parity, \
repetition, frame = constants._AXES2TUPLE[axes.lower(
)]
except (AttributeError, KeyError):
constants._TUPLE2AXES[axes] # validation
firstaxis, parity, repetition, frame = axes
i = firstaxis
j = constants._NEXT_AXIS[i + parity]
k = constants._NEXT_AXIS[i - parity + 1]
M = np.array(matrix, dtype=np.float64, copy=False)[:3, :3]
if repetition:
sy = np.sqrt(M[i, j] * M[i, j] + M[i, k] * M[i, k])
if sy > constants._EPS:
ax = np.arctan2(M[i, j], M[i, k])
ay = np.arctan2(sy, M[i, i])
az = np.arctan2(M[j, i], -M[k, i])
else:
ax = np.arctan2(-M[j, k], M[j, j])
ay = np.arctan2(sy, M[i, i])
az = 0.0
else:
cy = np.sqrt(M[i, i] * M[i, i] + M[j, i] * M[j, i])
if cy > constants._EPS:
ax = np.arctan2(M[k, j], M[k, k])
ay = np.arctan2(-M[k, i], cy)
az = np.arctan2(M[j, i], M[i, i])
else:
ax = np.arctan2(-M[j, k], M[j, j])
ay = np.arctan2(-M[k, i], cy)
az = 0.0
if parity:
ax, ay, az = -ax, -ay, -az
if frame:
ax, az = az, ax
return ax, ay, az
u = None
rot, i, j, k = _AXES2TUPLE[axes]
if rot:
matrix = np.transpose(matrix)
new = list(axes)
new[0] = 's'
axes = ''.join(new)
if axes == 'sxyx':
u = [np.arctan2(matrix[1, 0], matrix[2, 0]),
np.arccos(matrix[0, 0]),
np.arctan2(matrix[0, 1], -matrix[0, 2])]
if axes == 'sxyz':
u = [np.arctan2(matrix[1, 2], matrix[2, 2]),
-np.arcsin(matrix[0, 2]),
np.arctan2(matrix[0, 1], matrix[0, 0])]
if axes == 'sxzx':
u = [np.arctan2(matrix[2, 1], -matrix[1, 0]),
np.arccos(matrix[0, 0]),
np.arctan2(matrix[0, 2], matrix[0, 1])]
if axes == 'sxzy':
u = [np.arctan2(-matrix[2, 1], matrix[1, 1]),
np.arcsin(matrix[0, 0]),
np.arctan2(-matrix[0, 2], matrix[0, 0])]
if axes == 'syxy':
u = [np.arctan2(matrix[0, 1], -matrix[2, 1]),
np.arccos(matrix[1, 1]),
np.arctan2(-matrix[1, 0], matrix[1, 2])]
if axes == 'syxz':
u = [np.arctan2(-matrix[0, 2], matrix[2, 2]),
np.arcsin(matrix[1, 2]),
np.arctan2(-matrix[1, 0], matrix[1, 1])]
if axes == 'syzx':
u = [np.arctan2(matrix[2, 0], matrix[0, 0]),
-np.arcsin(matrix[1, 0]),
np.arctan2(matrix[1, 2], matrix[1, 1])]
if axes == 'syzy':
u = [np.arctan2(matrix[2, 1], matrix[0, 1]),
np.arccos(matrix[1, 1]),
np.arctan2(matrix[1, 2], -matrix[1, 0])]
if axes == 'szxy':
u = [np.arctan2(matrix[0, 1], matrix[1, 1]),
-np.arcsin(matrix[2, 1]),
np.arctan2(matrix[2, 0], matrix[2, 2])]
if axes == 'szxz':
u = [np.arctan2(matrix[0, 2], matrix[1, 2]),
np.arccos(matrix[2, 2]),
np.arctan2(matrix[2, 0], -matrix[2, 1])]
if axes == 'szyx':
u = [np.arctan2(-matrix[1, 0], matrix[0, 0]),
np.arcsin(matrix[2, 0]),
np.arctan2(-matrix[2, 1], matrix[2, 2])]
if axes == 'szyz':
u = [np.arctan2(matrix[1, 2], -matrix[0, 2]),
np.arccos(matrix[2, 2]),
np.arctan2(matrix[2, 1], matrix[2, 0])]
if u is None:
print("conv", axes, matrix)
return u
def from_quaternion(quaternion, axes='sxyz'):
......@@ -135,11 +140,11 @@ def from_quaternion(quaternion, axes='sxyz'):
if not isinstance(quaternion, np.ndarray) and\
not isinstance(quaternion, list):
raise TypeError("quaternions must be np.array or list")
if np.any(np.isnan(np.array(quaternion, dtype=np.float64))):
raise ValueError('posorient must not contain nan')
if axes not in list(constants._AXES2TUPLE.keys()):
# if np.any(np.isnan(np.array(quaternion, dtype=np.float64))):
# raise ValueError('posorient must not contain nan')
if axes not in list(_AXES2TUPLE.keys()):
raise Exception("the chosen convention is not supported")
return from_matrix(quat.matrix(quaternion), axes)
return from_matrix_new(quat.matrix(quaternion), axes)
def angular_rate_matrix(ai, aj, ak, axes='rxyz'):
......@@ -155,13 +160,13 @@ def angular_rate_matrix(ai, aj, ak, axes='rxyz'):
raise TypeError("euler angle must be of type float")
if not isinstance(ak, float) and not isinstance(ak, int):
raise TypeError("euler angle must be of type float")
if np.isnan(np.array([ai], dtype=np.float64)) or\
np.isnan(np.array([aj], dtype=np.float64)) or\
np.isnan(np.array([ak], dtype=np.float64)):
raise ValueError("quaternions must not be nan or none")
if axes not in list(constants._AXES2TUPLE.keys()):
# if np.isnan(np.array([ai], dtype=np.float64)) or\
# np.isnan(np.array([aj], dtype=np.float64)) or\
# np.isnan(np.array([ak], dtype=np.float64)):
# raise ValueError("quaternions must not be nan or none")
if axes not in list(_AXES2TUPLE.keys()):
raise Exception("the chosen convention is not supported")
s, i, j, k = getAxes(axes)
s, i, j, k = _AXES2TUPLE[axes]
ei = np.zeros(3)
ej = np.zeros(3)
ek = np.zeros(3)
......@@ -202,15 +207,15 @@ def angular_velocity(ai, aj, ak, dai, daj, dak, axes='rxyz'):
raise TypeError("euler angle time derivative must be of type float")
if not isinstance(dak, float) and not isinstance(dak, int):
raise TypeError("euler angle time derivative must be of type float")
if np.isnan(np.array([ai], dtype=np.float64)) or\
np.isnan(np.array([aj], dtype=np.float64)) or\
np.isnan(np.array([ak], dtype=np.float64)):
raise ValueError("quaternions must not be nan or none")
if np.isnan(np.array([dai], dtype=np.float64)) or\
np.isnan(np.array([daj], dtype=np.float64)) or\
np.isnan(np.array([dak], dtype=np.float64)):
raise ValueError("quaternions must not be nan or none")
if axes not in list(constants._AXES2TUPLE.keys()):
# if np.isnan(np.array([ai], dtype=np.float64)) or\
# np.isnan(np.array([aj], dtype=np.float64)) or\
# np.isnan(np.array([ak], dtype=np.float64)):
# raise ValueError("quaternions must not be nan or none")
# if np.isnan(np.array([dai], dtype=np.float64)) or\
# np.isnan(np.array([daj], dtype=np.float64)) or\
# np.isnan(np.array([dak], dtype=np.float64)):
# raise ValueError("quaternions must not be nan or none")
if axes not in list(_AXES2TUPLE.keys()):
raise Exception("the chosen convention is not supported")
rotM = angular_rate_matrix(ai, aj, ak, axes)
vel = np.dot(rotM, [dai, daj, dak])
......
navipy/maths/test_euler.py
+ 305
− 45
View file @ cc7e1161
import numpy as np
import navipy.maths.euler as euler
import navipy.maths.constants as constants
# import navipy.maths.euler as euler
import navipy.maths.new_euler as new_euler
# import navipy.maths.constants as constants
import unittest
# from navipy.maths.euler import matrix
from navipy.maths.new_euler import angular_rate_matrix
......@@ -8,14 +9,127 @@ from navipy.maths.new_euler import angular_velocity
from navipy.maths.new_euler import rotation_matrix
from navipy.maths.new_euler import R1, R2, R3
# from navipy.maths.euler import angle_rate_matrix as old_angular_rate_matrix
from navipy.maths.euler import angular_velocity as old_angular_velocity
from navipy.maths.euler import matrix as old_rotation_matrix
# from navipy.maths.euler import angular_velocity as old_angular_velocity
# from navipy.maths.euler import matrix as old_rotation_matrix
c = np.cos
s = np.sin
"""
[['sxyz','rzyx'(mirrored), 'Z3Y2X1'(transposed)],
['sxyx', 'rxyx', 'X3Y2X1'(transposed)],
['sxzy', 'ryzx'(mirrored), 'Y3Z2X1'(transposed)],
['sxzx', 'rxzx', 'X3Z2X1'(transposed)],
['syzx', 'rxzy'(mirrored), 'X3Z2Y1'(transposed)],
['syzy', 'ryzy', 'Y3Z2Y1'(transposed)],
['syxz', 'rzxy'(mirrored), 'Z3X2Y1'(transposed)],
['syxy', 'ryxy', 'Y3X2Y1'(transposed)],
['szxy', 'ryzx'(mirrored), 'Y3X2Z1'(transposed)],
['szxz', 'rzxz', 'Z3X2Z1'(transposed)],
['szyx', 'rxyz'(mirrored), 'X3Y2Z1'(transposed)],
['szyz', 'rzyz', 'Z3Y2Z1'(transposed)],
['rzyx', 'szyx', 'Z1Y2X3'],
['rxyx', 'sxyx', 'X1Y2X3'],
['ryzx', 'syzx', 'Y1Z2X3'],
['rxzx', 'sxzx', 'X1Z2X3'],
['rxzy', 'sxzy', 'X1Z2Y3'],
['ryzy', 'syzy', 'Y1Z2Y3'],
['rzxy', 'szxy', 'Z1X2Y3'],
['ryxy', 'syxy', 'Y1X2Y3'],
['ryxz', 'syxz', 'Y1X2Z3'],
['rzxz', 'szxz', 'Z1X2Z3'],
['rxyz', 'sxyz', 'X1Y2Z3'],
['rzyz', 'szyz', 'Z1Y2Z3']]
"""
ConvTrans = [['sxyz', 'rzyx', 1, 0, 'Z3Y2X1', 0, 1],
['sxyx', 'sxyx', 0, 1, 'X3Y2X1', 0, 1],
['sxzy', 'sxzy', 0, 1, 'Y3Z2X1', 0, 1],
['sxzx', 'sxzx', 0, 1, 'X3Z2X1', 0, 1],
['syzx', 'syzx', 0, 1, 'X3Z2Y1', 0, 1],
['syzy', 'syzy', 0, 1, 'Y3Z2Y1', 0, 1],
['syxz', 'syxz', 0, 1, 'Z3X2Y1', 0, 1],
['syxy', 'syxy', 0, 1, 'Y3X2Y1', 0, 1],
['szxy', 'szxy', 0, 1, 'Y3X2Z1', 0, 1],
['szxz', 'szxz', 0, 1, 'Z3X2Z1', 0, 1],
['szyx', 'szyx', 0, 1, 'X3Y2Z1', 0, 1],
['szyz', 'szyz', 0, 1, 'Z3Y2Z1', 0, 1],
['rzyx', 'szyx', 0, 0, 'X1Y2Z3', 1, 1],
['rxyx', 'sxyx', 0, 0, 'X3Y2X1', 0, 0],
['ryzx', 'syzx', 0, 0, 'X3Z2Y1', 0, 0],
['rxzx', 'sxzx', 0, 0, 'X3Z2X1', 0, 0],
['rxzy', 'sxzy', 0, 0, 'Y3Z2X1', 0, 0],
['ryzy', 'syzy', 0, 0, 'Y3Z2Y1', 0, 0],
['rzxy', 'szxy', 0, 0, 'Y3X2Z1', 0, 0],
['ryxy', 'ryxy', 1, 1, 'Y1X2Y3', 1, 1],
['ryxz', 'syxz', 0, 0, 'Z3X2Y1', 0, 0],
['rzxz', 'szxz', 0, 0, 'Z3X2Z1', 0, 0],
['rxyz', 'sxyz', 0, 0, 'Z3Y2X1', 0, 0],
['rzyz', 'szyz', 0, 0, 'Z3Y2Z1', 0, 0]]
_AXES2TUPLEwiki = {'Z3Y2X1': (2, 1, 0, 2, 1, 0),
'X3Y2X1': (0, 1, 0, 2, 1, 0),
'Y3Z2X1': (1, 2, 0, 2, 1, 0),
'X3Z2X1': (0, 2, 0, 2, 1, 0),
'X3Z2Y1': (0, 2, 1, 2, 1, 0),
'Y3Z2Y1': (1, 2, 1, 2, 1, 0),
'Z3X2Y1': (2, 0, 1, 2, 1, 0),
'Y3X2Y1': (1, 0, 1, 2, 1, 0),
'Y3X2Z1': (1, 0, 2, 2, 1, 0),
'Z3X2Z1': (2, 0, 2, 2, 1, 0),
'X3Y2Z1': (0, 1, 2, 2, 1, 0),
'Z3Y2Z1': (2, 1, 2, 2, 1, 0),
'Z1Y2X3': (2, 1, 0, 0, 1, 2),
'X1Y2X3': (0, 1, 2, 0, 1, 2),
'Y1Z2X3': (1, 2, 0, 0, 1, 2),
'X1Z2X3': (0, 2, 0, 0, 1, 2),
'X1Z2Y3': (0, 2, 1, 0, 1, 2),
'Y1Z2Y3': (1, 2, 1, 0, 1, 2),
'Z1X2Y3': (2, 0, 1, 0, 1, 2),
'Y1X2Y3': (1, 0, 1, 0, 1, 2),
'Y1X2Z3': (1, 0, 2, 0, 1, 2),
'Z1X2Z3': (2, 0, 2, 0, 1, 2),
'X1Y2Z3': (0, 1, 2, 0, 1, 2),
'Z1Y2Z3': (2, 1, 2, 0, 1, 2)}
def rot_M_wiki_R1(a):
R1 = np.array([[1, 0, 0],
[0, c(a), -s(a)],
[0, s(a), c(a)]])
return R1
def rot_M_wiki_R2(a):
R1 = np.array([[c(a), 0, s(a)],
[0, 1, 0],
[-s(a), 0, c(a)]])
return R1
def rot_M_wiki_R3(a):
R1 = np.array([[c(a), -s(a), 0],
[s(a), c(a), 0],
[0, 0, 1]])
return R1
def rotation_matrix_wiki(ai, aj, ak, conv='Z3Y2X1'):
i, j, k, a_i, a_j, a_k = _AXES2TUPLEwiki[conv]
angles = [ai, aj, ak]
matrixes = [rot_M_wiki_R1, rot_M_wiki_R2, rot_M_wiki_R3]
Rijk = np.dot(matrixes[i](angles[a_i]),
np.dot(matrixes[j](angles[a_j]),
matrixes[k](angles[a_k])))
return Rijk
class TestEuler(unittest.TestCase):
"""
def test_from_matrix(self):
condition = dict()
for key, _ in constants._AXES2TUPLE.items():
for key in list(constants._AXES2TUPLE.keys()):
print("key", key)
rotation_0 = euler.matrix(1, 2, 3, key)
ai, aj, ak = euler.from_matrix(rotation_0, key)
rotation_1 = euler.matrix(ai, aj, ak, key)
......@@ -24,12 +138,107 @@ class TestEuler(unittest.TestCase):
if condition[key] is False:
print('axes', key, 'failed')
self.assertTrue(np.all(np.array(condition.values())))
"""
def test_betweenconvention(self):
"""
Test orientation from one convention to another
"""
def test_from_matrix_new(self):
condition = dict()
for key in list(new_euler._AXES2TUPLE.keys()):
# print("key", key)
rotation_0 = new_euler.rotation_matrix(1, 2, 3, key)
# print("roation_0", rotation_0)
[ai, aj, ak] = new_euler.from_matrix_new(rotation_0, key)
# print("res", ai, aj, ak)
rotation_1 = new_euler.rotation_matrix(ai, aj, ak, key)
condition[key] = np.allclose(rotation_0,
rotation_1)
if condition[key] is False:
print('axes', key, 'failed from matrix')
self.assertTrue(np.all(np.array(condition.values())))
"""
def est_wiki_conv(self):
a = 1
b = 2
c = 3
Convs = [['sxyz', 'Z3Y2X1'],
['sxyx', 'X3Y2X1'],
['sxzy', 'Y3Z2X1'],
['sxzx', 'X3Z2X1'],
['syzx', 'X3Z2Y1'],
['syzy', 'Y3Z2Y1'],
['syxz', 'Z3X2Y1'],
['syxy', 'Y3X2Y1'],
['szxy', 'Y3X2Z1'],
['szxz', 'Z3X2Z1'],
['szyx', 'X3Y2Z1'],
['szyz', 'Z3Y2Z1'],
['rzyx', 'Z1Y2X3'],
['rxyx', 'X1Y2X3'],
['ryzx', 'Y1Z2X3'],
['rxzx', 'X1Z2X3'],
['rxzy', 'X1Z2Y3'],
['ryzy', 'Y1Z2Y3'],
['rzxy', 'Z1X2Y3'],
['ryxy', 'Y1X2Y3'],
['ryxz', 'Y1X2Z3'],
['rzxz', 'Z1X2Z3'],
['rxyz', 'X1Y2Z3'],
['rzyz', 'Z1Y2Z3']]
r_ref = rotation_matrix(a,b,c,'sxyz')
r_wiki=euler.matrix(a,b,c,'rxyz')[:3,:3]
r_ref =np.flip(r_ref,axis=1)
r_ref =np.flip(r_ref,axis=0)
#print("r ref", r_ref)
#print("r wiki", r_wiki)
vec = [1,2,3]
for i, _ in constants._AXES2TUPLE.items():
r_wiki=euler.matrix(a,b,c,i)[:3,:3]
#r_wiki_t=rotation_matrix_wiki(a,b,c,'X3Y2Z1')
#r_wiki=np.transpose(r_wiki)
#r_new = rotation_matrix(a,b,c,'sxyz')
#r_new_t = rotation_matrix(a,b,c,'rxyz')
#new_vec_wiki=np.dot(r_wiki,vec)
#new_vec= np.dot(r_new, vec)
#print("matrices",
# r_wiki_t, r_wiki, r_new,r_new_t)
#break
if np.allclose(r_wiki, r_ref):
#print("conv found", i, r_wiki)
def est_convTrans(self):
ai = 0.1
aj = 0.2
ak = 0.3
for i in ConvTrans:
Molivier=euler.matrix(ai, aj, ak, i[1])[:3,:3]
Mwiki = rotation_matrix_wiki(ai, aj, ak, i[4])
Mmy=rotation_matrix(ai, aj, ak, i[0])
if i[3]==1:
Molivier=np.transpose(Molivier)
if i[2]==1:
Molivier = np.flip(Molivier,axis=0)
Molivier = np.flip(Molivier,axis=1)
if i[6]==1:
Mwiki=np.transpose(Mwiki)
if i[5]==1:
Mwiki = np.flip(Mwiki,axis=0)
Mwiki = np.flip(Mwiki,axis=1)
print("conv test", Mwiki, Mmy,i)
print("conv test", Molivier, Mmy,i)
self.assertTrue(np.allclose(Mwiki, Mmy))
self.assertTrue(np.allclose(Molivier, Mmy))
"""
# def test_betweenconvention(self):
# """
# Test orientation from one convention to another
# """
""" condition = dict()
refkey = 'rxyz'
for key, _ in constants._AXES2TUPLE.items():
rotation_0 = euler.matrix(1, 2, 3, refkey)
......@@ -40,9 +249,29 @@ class TestEuler(unittest.TestCase):
if condition[key] is False:
print('axes', key, 'failed')
self.assertTrue(np.all(np.array(condition.values())))
"""
def test_betweenconvention_new(self):
"""
Test orientation from one convention to another
"""
condition = dict()
refkey = 'rxyz'
for key in list(new_euler._AXES2TUPLE.keys()):
print("between key", key)
rotation_0 = new_euler.rotation_matrix(1, 2, 3, refkey)
print("betewwen matrix", rotation_0)
[ai, aj, ak] = new_euler.from_matrix_new(rotation_0, key)
print("between", ai, aj, ak)
rotation_1 = new_euler.rotation_matrix(ai, aj, ak, key)
condition[key] = np.allclose(rotation_0,
rotation_1)
if condition[key] is False:
print('axes', key, 'failed between')
self.assertTrue(np.all(np.array(condition.values())))
def test_from_quaternion(self):
angles = euler.from_quaternion([0.99810947, 0.06146124, 0, 0])
angles = new_euler.from_quaternion([0.99810947, 0.06146124, 0, 0])
self.assertTrue(np.allclose(angles, [0.123, 0, 0]))
def test_from_quaternion_params(self):
......@@ -50,42 +279,44 @@ class TestEuler(unittest.TestCase):
(5.0, 4.0, None, 8.0),
(1.0, 2.0, 3.0, 'w')]:
with self.assertRaises(ValueError):
euler.from_quaternion([a, b, c, d])
new_euler.from_quaternion([a, b, c, d])
with self.assertRaises(Exception):
euler.from_quaternion([9.0, 8.0, 7.0, 0.3], 'w')
new_euler.from_quaternion([9.0, 8.0, 7.0, 0.3], 'w')
"""
def test_matrix_params(self):
for a, b, c, d in [(None, 2, 6, 8), (5.0, 4.0, None, 8.0)]:
for a, b, c, d in [(None, 2, 6, 'rxyz'), (5.0, 4.0, None, 'syxz')]:
with self.assertRaises(TypeError):
euler.matrix(a, b, c, d)
for a, b, c, d in [(9.0, np.nan, 2, 3)]:
with self.assertRaises(ValueError):
euler.matrix(a, b, c, d)
# for a, b, c, d in [(9.0, np.nan, 2, 'rxyz')]:
# with self.assertRaises(ValueError):
# euler.matrix(a, b, c, d)
with self.assertRaises(Exception):
euler.matrix(9.0, 8.0, 7.0, 'w')
"""
def test_from_matrix_params(self):
for a, b, c, d in [(None, 2, 6, 8), (9.0, np.nan, 2, 3),
(5.0, 4.0, None, 8.0)]:
with self.assertRaises(ValueError):
euler.from_matrix([a, b, c, d])
new_euler.from_matrix([a, b, c, d])
with self.assertRaises(Exception):
euler.from_matrix(9.0, 8.0, 7.0, 'w')
new_euler.from_matrix(9.0, 8.0, 7.0, 'w')
def test_angle_rate_matrix_params(self):
for a, b, c, d in [(None, 2, 6, 8), (5.0, 4.0, None, 8.0)]:
for a, b, c, d in [(None, 2, 6, 'rxyz'), (5.0, 4.0, None, 'sxyx')]:
with self.assertRaises(TypeError):
euler.angle_rate_matrix(a, b, c, d)
new_euler.angle_rate_matrix(a, b, c, d)
for a, b, c, d in [(9.0, np.nan, 2, 3)]:
with self.assertRaises(ValueError):
euler.angle_rate_matrix(a, b, c, d)
new_euler.angle_rate_matrix(a, b, c, d)
with self.assertRaises(Exception):
euler.angle_rate_matrix(9.0, 8.0, 7.0, 'w')
new_euler.angle_rate_matrix(9.0, 8.0, 7.0, 'w')
def test_angular_velocity_params(self):
for a, b, c, d, e, f, g in [(None, 2, 6, 8, 7, 8, 'b'),
......@@ -96,12 +327,23 @@ class TestEuler(unittest.TestCase):
(1.0, 2.0, 3.0, 4.0, None, 4, 'w'),
(5.0, 4.0, 3.0, 8.0, 8.0, 4.0, 'l')]:
with self.assertRaises(TypeError):
euler.angle_rate_matrix(a, b, c, d, e, f, g)
new_euler.angle_rate_matrix(a, b, c, d, e, f, g)
def test_old_vs_newXZX(self):
""" example from wikipedia
"""
c = np.cos
def test_angular_velocity_params_new(self):
for a, b, c, d, e, f, g in [(None, 2, 6, 8, 7, 8, 'b'),
(9.0, 8, 2, 3, 3, 5, 7),
(5.0, 4.0, None, 8.0, 8.0, 4.0, 2.0),
(9.0, 8.0, 7.0, np.nan, 6.0, 9.0, 'w'),
(4.0, 2.0, 1.0, 3.0, 'w', 2.0, 9),
(1.0, 2.0, 3.0, 4.0, None, 4, 'w'),
(5.0, 4.0, 3.0, 8.0, 8.0, 4.0, 'l')]:
with self.assertRaises(TypeError):
new_euler.angle_rate_matrix(a, b, c, d, e, f, g)
# def test_old_vs_newXZX(self):
# """ example from wikipedia
# """
""" c = np.cos
s = np.sin
ai = 0.10
aj = 0.20
......@@ -126,34 +368,60 @@ class TestEuler(unittest.TestCase):
self.assertTrue(np.allclose(R2, R2_old))
self.assertTrue(np.allclose(R2, MwikiXZX))
self.assertTrue(np.allclose(R2_old, MwikiXZX))
"""
"""
def test_old_vs_newXYZ(self):
ai = 0.10
aj = 0.20
ak = 0.70
R1 = rotation_matrix(ai, aj, ak, 'rxyz')
R2 = rotation_matrix(ai, aj, ak, 'sxyz')
R1_old = old_rotation_matrix(ai, aj, ak, 'rxyz')
R2_old = old_rotation_matrix(ai, aj, ak, 'sxyz')
self.assertTrue(np.allclose(R1, np.transpose(R2)))
self.assertTrue(np.allclose(R1_old, np.transpose(R2_old)))
self.assertTrue(np.allclose(R1, R1_old))
self.assertTrue(np.allclose(R2, R2_old))
for c in new_euler._AXES2TUPLE:
R1 = rotation_matrix(ai, aj, ak, 'rxyz')
R2 = rotation_matrix(ai, aj, ak, 'sxyz')
R1_old = old_rotation_matrix(ai, aj, ak, 'rxyz')[:3,:3]
R2_old = old_rotation_matrix(ai, aj, ak, 'sxyz')[:3,:3]
if not np.allclose(R1, R1_old):
print("wrong conf old vs new", c)
if not np.allclose(R2, R2_old):
print("wrong conf old vs new", c)
# self.assertTrue(np.allclose(R1, np.transpose(R2)))
# self.assertTrue(np.allclose(R1_old, np.transpose(R2_old)))
# self.assertTrue(np.allclose(R1, R1_old))
# self.assertTrue(np.allclose(R2, R2_old))
# print("R1", R1)
# print("R2", R2)
# print("R1 old", R1_old)
# print("R2 old", R2_old)
"""
def test_rotMatrix(self):
"""the stationary rotation matrix should be the transpose of the rotational
"""the stationary rotation matrix should be
the transpose of the rotational
with the same axis order"""
ai = 0.10
aj = 0.20
ak = 0.70
R1 = rotation_matrix(ai, aj, ak, 'rxyz')
R2 = rotation_matrix(ai, aj, ak, 'sxyz')
# print("test trans",R1, R2)
self.assertTrue(np.allclose(R1, np.transpose(R2)))
# def test_rotvelOldvsNew(self):
# """the stationary rotation matrix should be the
# transpose of the rotational
# with the same axis order"""
"""
ai = 0.10
aj = 0.20
ak = 0.70
dai = 0.01
daj = 0.03
dak = 0.5
vel = angular_velocity(ai, aj, ak, dai, daj, dak, 'sxyz')
vel_old = old_angular_velocity(ai, aj, ak, dai, daj, dak, 'sxyz')
print("vel test", vel, vel_old)
self.assertTrue(np.allclose(vel, vel_old))
"""
def test_convertE(self):
"""
the rotation matrix can be obtained by Rijk=E'ijk*(Eijk)^(-1)
......@@ -248,21 +516,13 @@ class TestEuler(unittest.TestCase):
dai = 0
daj = 0
dak = 0.01
vel_old = old_angular_velocity(ai, aj, ak, dai, daj, dak, 'szxz')
vel_old_r = old_angular_velocity(ai, aj, ak, dai, daj, dak, 'rzxz')
Rijk_old = old_rotation_matrix(ai, aj, ak, 'szxz')[:3, :3]
Rijk_r_old = old_rotation_matrix(ai, aj, ak, 'szxz')[:3, :3]
Rijk = rotation_matrix(ai, aj, ak, 'szxz')
Rijk_r = rotation_matrix(ai, aj, ak, 'rzxz')
vel = angular_velocity(ai, aj, ak, dai, daj, dak, 'szxz')
vel_r = angular_velocity(ai, aj, ak, dai, daj, dak, 'rzxz')
vel_p = np.dot(Rijk, vel)
vel_new = np.dot(Rijk_r, vel_p)
vel_p_old = np.dot(Rijk_old, vel_old)
vel_new_old = np.dot(Rijk_r_old, vel_p_old)
self.assertTrue(np.allclose(vel_p, vel_r))
self.assertTrue(np.allclose(vel_p_old, vel_old_r))
self.assertTrue(np.allclose(vel_new_old, vel_old))
self.assertTrue(np.allclose(vel_new, vel))
"""
......
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