diff --git a/navipy/maths/test_euler.py b/navipy/maths/test_euler.py
index dad2039a00c6613a5778f1e5fbea170f538925c8..e509cd8f434393dd25038b63a6eb8fed6016c09d 100644
--- a/navipy/maths/test_euler.py
+++ b/navipy/maths/test_euler.py
@@ -1,119 +1,23 @@
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
diff --git a/navipy/maths/test_homogeneous_transformations.py b/navipy/maths/test_homogeneous_transformations.py
index b6dd662b683c056d40ae3cf8e469c7f2ce5c4ec8..2677bb4d1cce0dbf85cb1a8ed4f1832baa68867a 100644
--- a/navipy/maths/test_homogeneous_transformations.py
+++ b/navipy/maths/test_homogeneous_transformations.py
@@ -1,7 +1,6 @@
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()
-
diff --git a/navipy/maths/test_quaternion.py b/navipy/maths/test_quaternion.py
index 6554425288afcf3345479bf647ede9b4b40875fe..a21cad1fc7562e51047c141f93d8fefe9774799b 100644
--- a/navipy/maths/test_quaternion.py
+++ b/navipy/maths/test_quaternion.py
@@ -86,4 +86,3 @@ class TestQuaternions(unittest.TestCase):
if __name__ == '__main__':
unittest.main()
-
diff --git a/navipy/processing/mcode.py b/navipy/processing/mcode.py
index dcb20d466471c4e51820406ccc56575877b8298d..2f57d16ee6e2ca9ea8064be286f2d3ac0b9081b0 100644
--- a/navipy/processing/mcode.py
+++ b/navipy/processing/mcode.py
@@ -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
diff --git a/navipy/processing/test_OpticFlow.py b/navipy/processing/test_OpticFlow.py
index 81afce86c94dd02d4b7d4d2b3b2c0a24b40b600b..2669ad00405998cd0760f1166d9c7b052a33c8d6 100644
--- a/navipy/processing/test_OpticFlow.py
+++ b/navipy/processing/test_OpticFlow.py
@@ -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'),