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

Merge branch 'Luise' of gitlab.ub.uni-bielefeld.de:olivier.bertrand/navipy into Luise

parents b243f38c 94de4565
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
navipy/maths/test_euler.py
+ 8
− 102
View file @ b13f87e6
import numpy as np
# import navipy.maths.euler as euler
import navipy.maths.euler as euler
from navipy.maths.constants import _AXES2TUPLE
import unittest
# from navipy.maths.euler import matrix
from navipy.maths.euler import angle_rate_matrix
from navipy.maths.euler import angular_velocity
from navipy.maths.euler import matrix
from navipy.maths.euler import R1, R2, R3
# from navipy.maths.euler import angle_rate_matrix as old_angle_rate_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)}
class TestEuler(unittest.TestCase):
"""
def test_from_matrix(self):
condition = dict()
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)
condition[key] = np.allclose(rotation_0,
rotation_1)
if condition[key] is False:
print('axes', key, 'failed')
self.assertTrue(np.all(np.array(condition.values())))
"""
def test_from_matrix_new(self):
"""
tests if the matrix and from_matrix (decompose) function
from the euler angles works correctly.
- take some angles
- take some angles
- build matrix from angles (euler.matrix)
- decompose obtained matrix to angles (euler.from_matrix)
- use obtained angles to build euler matrix (euler.matrix)
......@@ -204,15 +108,17 @@ class TestEuler(unittest.TestCase):
euler.angle_rate_matrix function
"""
for a, b, c, d, e, f, g in [(None, 2, 6, 8, 7, 8, 'rxyz'),
(9.0, 8, 2, 3, 3, 5, 7),
(9.0, 'er', 2, 3, 3, 5, 'sxyz'),
(5.0, 4.0, None, 8.0, 8.0, 4.0, 'rxyz'),
(9.0, 8.0, 7.0, np.nan, 6.0, 9.0, 'rxyz'),
(np.nan, 8.0, 7.0, '0', 6.0, 9.0, 'rxyz'),
(4.0, 2.0, 1.0, 3.0, 'w', 2.0, 'rxyz'),
(1.0, 2.0, 3.0, 4.0, None, 4, 'rxyz'),
(5.0, 4.0, 3.0, 8.0, 8.0, 4.0, 'l')]:
(1.0, 2.0, 3.0, 4.0, None, 4, 'rxyz')]:
with self.assertRaises(TypeError):
euler.angular_velocity(a, b, c, d, e, f, g)
with self.assertRaises(ValueError):
euler.angular_velocity(5.0, 4.0, 3.0, 8.0, 8.0, 4.0, 'l')
def test_rotMatrix(self):
"""the stationary rotation matrix should be
the transpose of the rotational
......
navipy/maths/test_homogeneous_transformations.py
+ 0
− 2
View file @ b13f87e6
import unittest
import numpy as np
import navipy.maths.homogeneous_transformations as ht
import navipy.maths.euler as euler
import navipy.maths.random as random
import navipy.maths.euler as euler
......@@ -269,4 +268,3 @@ class TestHT(unittest.TestCase):
if __name__ == '__main__':
unittest.main()
navipy/maths/test_quaternion.py
+ 0
− 1
View file @ b13f87e6
......@@ -86,4 +86,3 @@ class TestQuaternions(unittest.TestCase):
if __name__ == '__main__':
unittest.main()
navipy/processing/mcode.py
+ 4
− 173
View file @ b13f87e6
......@@ -11,6 +11,7 @@ from navipy.maths.coordinates\
import numpy as np
import pandas as pd
from navipy.maths.euler import angular_velocity
from navipy.maths.constants import _AXES2TUPLE
def spherical_to_Vector(sp):
......@@ -180,175 +181,6 @@ def OFSubroutineRotate(vec, rotAx, alpha):
return rotVec
def old_optic_flow(scene, viewing_directions, velocity, distance_channel=3):
""" optic flow
:param scene: ibpc
:param viewing_directions: viewing direction of each pixel
(azimuth,elevation)
:param velocity: pandas series
(x,y,z,alpha,beta,gamma,dx,dy,dz,dalpha,dbeta,dgamma)
:distance channel: distance"""
check_scene(scene)
if distance_channel not in range(4):
raise ValueError('distance channel out of range')
if not isinstance(velocity, pd.Series):
raise TypeError('velocity should be a pandas Series')
if velocity is None:
raise ValueError("velocity must not be None")
if velocity.empty:
raise Exception('velocity must not be empty')
if not isinstance(velocity.index, pd.core.index.MultiIndex):
raise Exception('velocity must have a multiindex containing \
the convention used')
index = velocity.index
convention = index.get_level_values(0)[-1]
if convention != 'rxyz':
raise ValueError("to calculate the optic flow,\
angles must be in rxyz convention")
if 'x' not in velocity.index.get_level_values(1):
raise ValueError('missing index x')
if 'y' not in velocity.index.get_level_values(1):
raise ValueError('missing index y')
if 'z' not in velocity.index.get_level_values(1):
raise ValueError('missing index z')
if 'alpha_0' not in velocity.index.get_level_values(1):
raise ValueError('missing index alpha_0')
if 'alpha_1' not in velocity.index.get_level_values(1):
raise ValueError('missing index alpha_1')
if 'alpha_2' not in velocity.index.get_level_values(1):
raise ValueError('missing index alpha_2')
if np.any(pd.isnull(velocity)):
raise ValueError('velocity must not contain nan')
if viewing_directions is None:
raise ValueError("viewing direction must not be None")
if (not isinstance(viewing_directions, list)) and\
(not isinstance(viewing_directions, np.ndarray)):
raise TypeError("angels must be list or np.ndarray")
if not is_numeric_array(viewing_directions):
raise TypeError("viewing_direction must be of numerical type")
if viewing_directions.shape[1] != 2:
raise Exception("second dimension of viewing\
direction must be of size two")
distance = np.squeeze(scene[:, :, distance_channel, 0])
# distance += distance
elevation = viewing_directions[..., __spherical_indeces__['elevation']]
azimuth = viewing_directions[..., __spherical_indeces__['azimuth']]
yaw = velocity[convention]['alpha_0']
pitch = velocity[convention]['alpha_1']
roll = velocity[convention]['alpha_2']
dyaw = velocity[convention]['dalpha_0']
dpitch = velocity[convention]['dalpha_1']
droll = velocity[convention]['dalpha_2']
if ((np.abs(dyaw) > np.pi/2 and 2*np.pi - np.abs(dyaw) > np.pi/2) or
(np.abs(dpitch) > np.pi/2 and 2*np.pi - np.abs(dpitch) > np.pi/2) or
(np.abs(droll) > np.pi/2 and 2*np.pi - np.abs(droll) > np.pi/2)):
raise ValueError('rotation exceeds 90°, computation aborted')
u = [velocity['location']['dx'],
velocity['location']['dy'],
velocity['location']['dz']]
v = np.linalg.norm(u)
if(v == 0):
u = [0, 0, 0]
else:
u = u/np.linalg.norm(u)
M = compose_matrix(angles=[yaw, 0, 0], translate=None,
perspective=None, axes='rzyx')[:3, :3]
yawNow = np.dot(M, [1, 0, 0])
M = compose_matrix(angles=[yaw + dyaw, 0, 0], translate=None,
perspective=None, axes='rzyx')[:3, :3]
yawNext = np.dot(M, [1, 0, 0])
rYaw = np.cross(yawNow, yawNext)
# now rotation around y-axis
M = compose_matrix(angles=[0, pitch, 0], translate=None,
perspective=None, axes='rzyx')[:3, :3]
pitchNow = np.dot(M, [1, 0, 0])
M = compose_matrix(angles=[0, pitch+dpitch, 0], translate=None,
perspective=None, axes='rzyx')[:3, :3]
pitchNext = np.dot(M, [1, 0, 0])
rPitch = np.cross(pitchNow, pitchNext)
# and roatation around x-axis
M = compose_matrix(angles=[0, 0, roll], translate=None,
perspective=None, axes='rzyx')[:3, :3]
rollNow = np.dot(M, [0, 0, 1])
M = compose_matrix(angles=[0, 0, roll+droll], translate=None,
perspective=None, axes='rzyx')[:3, :3]
rollNext = np.dot(M, [0, 0, 1])
rRoll = np.cross(rollNow, rollNext)
rof = np.zeros((azimuth.shape[0], elevation.shape[0]))
hof = np.zeros((azimuth.shape[0], elevation.shape[0]))
vof = np.zeros((azimuth.shape[0], elevation.shape[0]))
for i, a in enumerate(azimuth):
for j, e in enumerate(elevation):
spline = OFSubroutineSpToVector([a, e])
M = compose_matrix(angles=[yaw, pitch, roll], translate=None,
perspective=None, axes='rzyx')[:3, :3]
spline = np.dot(M, spline)
p = distance[j, i]
if(p == 0):
# if object touches the enviroment, OpticFlow dosnt need to be
# scaled -> distance=1
p = 1
# the Translation-part of the Optic Flow:
dotvu = np.dot(u, v)
splinetrans = np.transpose(spline)
opticFlowT = -(dotvu-(np.dot(dotvu, splinetrans))*spline)/p
# check if there actualy is a rotation and if compute
# surface-normal and scale it with angle between vectors
# negative because flow relative to observer
if np.linalg.norm(rYaw) <= 0.000001:
opticFlowRyaw = 0
else:
normrYaw = np.linalg.norm(rYaw)
rYawN = np.dot(rYaw/normrYaw, np.arcsin(normrYaw))
opticFlowRyaw = -np.cross(rYawN, spline)
if np.linalg.norm(rPitch) <= 0.000001:
opticFlowRpitch = 0
else:
normrPitch = np.linalg.norm(rPitch)
rPitchN = np.dot(rPitch/normrPitch, np.arcsin(normrPitch))
opticFlowRpitch = -np.cross(rPitchN, spline)
if np.linalg.norm(rRoll) <= 0.000001:
opticFlowRroll = 0
else:
normrRoll = np.linalg.norm(rRoll)
rRollN = rRoll/normrRoll*np.arcsin(normrRoll)
opticFlowRroll = -np.cross(rRollN, spline)
# combine the rotations
opticFlowR = opticFlowRyaw+opticFlowRpitch+opticFlowRroll
# and add Translation and Rotation to get the Optic Flow
opticFlow = opticFlowT+opticFlowR
# Transform OF from Cartesian coordinates to Spherical coordinates
# according to method
(OF_rho, OF_phi, OF_epsilon) =\
cartesian_to_spherical_vectors(opticFlow,
[yaw, pitch, roll],
[a, e])
rof[j, i] = OF_rho
hof[j, i] = OF_phi
vof[j, i] = OF_epsilon
return rof, hof, vof
def optic_flow(scene, viewing_directions, velocity, distance_channel=3):
""" optic flow
:param scene: ibpc
......@@ -371,9 +203,8 @@ def optic_flow(scene, viewing_directions, velocity, distance_channel=3):
the convention used')
index = velocity.index
convention = index.get_level_values(0)[-1]
if convention != 'rxyz':
raise ValueError("to calculate the optic flow,\
angles must be in rxyz convention")
if convention not in _AXES2TUPLE.keys():
raise ValueError("the chosen convention is not supported")
if 'x' not in velocity.index.get_level_values(1):
raise ValueError('missing index x')
if 'y' not in velocity.index.get_level_values(1):
......@@ -449,7 +280,7 @@ def optic_flow(scene, viewing_directions, velocity, distance_channel=3):
# combine the rotations
opticFlowR = angular_velocity(yaw, pitch, roll,
dyaw, dpitch, droll, 'rxyz')
dyaw, dpitch, droll, convention)
# and add Translation and Rotation to get the Optic Flow
opticFlow = opticFlowT+opticFlowR
......
navipy/processing/test_OpticFlow.py
+ 1
− 2
View file @ b13f87e6
......@@ -101,8 +101,7 @@ class TestCase(unittest.TestCase):
rof, hof, vof = mcode.optic_flow(self.scene,
self.viewing_directions,
velocity)
for convention in ['ryxz', 'ryzx', 'rxzy', 'rzyx',
'rzxy', 'alsjf', '233', 9, -1]:
for convention in ['alsjf', '233', 9, -1]:
tuples = [('location', 'x'), ('location', 'y'),
('location', 'z'), ('location', 'dx'),
('location', 'dy'), ('location', 'dz'),
......
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