diff --git a/navipy/maths/coordinates.py b/navipy/maths/coordinates.py
index e670d9e8d6f80aa74f66929aeb28b599ea63b3b8..d6809c3fb77b7a96621760becb8ed914e373355f 100644
--- a/navipy/maths/coordinates.py
+++ b/navipy/maths/coordinates.py
@@ -2,6 +2,10 @@
Conversion between coordinates systems
"""
import numpy as np
+from navipy.maths.homogeneous_transformations import rotation_matrix
+from navipy.scene import is_numeric_array
+from navipy.maths.homogeneous_transformations\
+ import compose_matrix
def cartesian_to_spherical(x, y, z):
@@ -21,3 +25,72 @@ def spherical_to_cartesian(elevation, azimuth, radius=1):
z = np.sin(elevation)
cartesian = radius * cartesian
return x, y, z
+
+
+def cartesian_to_spherical_vectors(opticFlow, angles, viewing_direction):
+ """Now we need the optic Flow in the spherical coordinates.
+ A vector in cartesian coordinates can be transform as one in
+ the spherical coordinates following the transformation:
+
+ A_rho =+A_x.*cos(epsilon).*cos(phi)
+ +A_y.*cos(epsilon).*sin(phi)
+ +A_z.*sin(epsilon)
+ A_epsilon=-A_x.*sin(epsilon).*cos(phi)
+ -A_y.*sin(epsilon).*sin(phi)
+ +A_z.*cos(epsilon)
+ A_phi =-A_x.*sin(phi)+A_y.*cos(phi)
+
+ for epsilon in [-pi/2 +pi/2] and phi in [0 2pi]
+ reverse tajectory, needed because the frame x,y,z is expressed in
+ the orientation Yaw=pitch=roll=0"""
+ if opticFlow is None:
+ raise ValueError("opticFlow must not be None")
+ if angles is None:
+ raise ValueError("angles must not be None")
+ if viewing_direction is None:
+ raise ValueError("viewing direction must not be None")
+ if (not isinstance(opticFlow, np.ndarray)):
+ raise TypeError("vector must be of type np.ndarray")
+ if opticFlow.shape[0] != 3:
+ raise Exception("first dimension of optic flow vector\
+ must have size three")
+ if not is_numeric_array(opticFlow):
+ raise TypeError("opticFlow must be of numerical type")
+ if (not isinstance(viewing_direction, list)) and\
+ (not isinstance(viewing_direction, np.ndarray)):
+ raise TypeError("angles must be list or np.ndarray")
+ if not is_numeric_array(viewing_direction):
+ raise TypeError("viewing_direction must be of numerical type")
+ if len(viewing_direction) != 2:
+ raise Exception("first dimension of viewing\
+ direction must be of size two")
+
+ vec = opticFlow
+ ypr=angles
+ M = compose_matrix(scale=None, shear=None, angles=ypr, translate=None,
+ perspective=None, axes='rzyx')[:3, :3]
+ vec = np.dot(np.transpose(M), vec)
+ opticFlow = vec
+
+ OFT_x = opticFlow[0]
+ OFT_y = opticFlow[1]
+ OFT_z = opticFlow[2]
+
+ epsilon = viewing_direction[1]
+ phi = viewing_direction[0]
+
+ # print("OFTs", opticFlow)
+ # print("ep,phi", epsilon, phi)
+
+ rofterm1 = +OFT_x*np.cos(epsilon)*np.cos(phi)
+ rofterm2 = +OFT_y*np.cos(epsilon)*np.sin(phi)
+ rofterm3 = +OFT_z*np.sin(epsilon)
+ rof = rofterm1+rofterm2+rofterm3
+
+ vofterm1 = -OFT_x*np.sin(epsilon)*np.cos(phi)
+ vofterm2 = -OFT_y*np.sin(epsilon)*np.sin(phi)
+ vofterm3 = OFT_z*np.cos(epsilon)
+ vof = vofterm1+vofterm2+vofterm3
+ hof = -OFT_x*np.sin(phi) + OFT_y*np.cos(phi)
+
+ return [rof, hof, vof]
diff --git a/navipy/processing/mcode.py b/navipy/processing/mcode.py
index f162474fb93027014972d6ea0f25447e4183a64e..10a277831b16dd4e181e79d786ec5e570219e7cd 100644
--- a/navipy/processing/mcode.py
+++ b/navipy/processing/mcode.py
@@ -4,7 +4,10 @@ Motion code
from navipy.scene import check_scene
from navipy.scene import __spherical_indeces__
from navipy.scene import is_numeric_array
-from navipy.maths.homogeneous_transformations import rotation_matrix
+from navipy.maths.homogeneous_transformations\
+ import compose_matrix
+from navipy.maths.coordinates\
+ import spherical_to_cartesian, cartesian_to_spherical_vectors
import numpy as np
import pandas as pd
@@ -82,10 +85,13 @@ NOTE: this is NOT the normal spheic coordinate system!
raise ValueError("azimuth must be float or integer")
if (not isinstance(sp[1], float)) and (not isinstance(sp[1], int)):
raise ValueError("elevation must be float or integer")
+ """
vector = np.zeros((3,))
vector = [np.dot(np.cos(sp[1]), np.cos(sp[0])),
np.dot(np.cos(sp[1]), np.sin(sp[0])),
np.sin(sp[1])]
+ """
+ vector = np.array(spherical_to_cartesian(sp[1], sp[0]))
return vector
@@ -248,8 +254,12 @@ def OFSubroutineYawPitchRoll(vec, ypr, inverse):
# end
# rotations around relative rotatet axis
-
+ M = compose_matrix(scale=None, shear=None, angles=ypr, translate=None,
+ perspective=None, axes='rzyx')[:3, :3]
if (inverse):
+ vec = np.dot(np.transpose(M), vec)
+ """
+ vec=tmpvec
M1 = rotation_matrix(-ypr[2], [1, 0, 0])[:3, :3]
vec = np.transpose(np.dot(M1, np.transpose(vec)))
roty = np.transpose(np.dot(M1, np.transpose([0, 1, 0])))
@@ -259,9 +269,14 @@ def OFSubroutineYawPitchRoll(vec, ypr, inverse):
M4 = rotation_matrix(-ypr[1], [0, 1, 0])[:3, :3]
rotatedax = np.transpose(np.dot(M4, np.transpose(rotz)))
- M3 = rotation_matrix(-ypr[0], rotatedax)[:3, :3]
+ M3 = rotation_matrix(-ypr[0], rotatedax)[:3,:3]
vec = np.transpose(np.dot(M3, np.transpose(vec)))
+ """
else:
+ vec = np.dot(M, vec)
+ """
+ print("new normal vec", vec)
+ vec = tmpvec
M1 = rotation_matrix(ypr[0], [0, 0, 1])[:3, :3]
vec = np.transpose(np.dot(M1, np.transpose(vec)))
roty = np.transpose(np.dot(M1, np.transpose([0, 1, 0])))
@@ -273,72 +288,12 @@ def OFSubroutineYawPitchRoll(vec, ypr, inverse):
M3 = rotation_matrix(ypr[2], rotatedax)[:3, :3]
vec = np.transpose(np.dot(M3, np.transpose(vec)))
+ print("old normal vec", vec)
+ """
return vec
-def Matrix_transform(opticFlow, angles, viewing_direction):
- """Now we need the optic Flow in the spherical coordinates.
- A vector in cartesian coordinates can be transform as one in
- the spherical coordinates following the transformation:
-
- A_rho =+A_x.*cos(epsilon).*cos(phi)
- +A_y.*cos(epsilon).*sin(phi)
- +A_z.*sin(epsilon)
- A_epsilon=-A_x.*sin(epsilon).*cos(phi)
- -A_y.*sin(epsilon).*sin(phi)
- +A_z.*cos(epsilon)
- A_phi =-A_x.*sin(phi)+A_y.*cos(phi)
-
- for epsilon in [-pi/2 +pi/2] and phi in [0 2pi]
- reverse tajectory, needed because the frame x,y,z is expressed in
- the orientation Yaw=pitch=roll=0"""
- if opticFlow is None:
- raise ValueError("opticFlow must not be None")
- if angles is None:
- raise ValueError("angles must not be None")
- if viewing_direction is None:
- raise ValueError("viewing direction must not be None")
- if (not isinstance(opticFlow, np.ndarray)):
- raise TypeError("vector must be of type np.ndarray")
- if opticFlow.shape[0] != 3:
- raise Exception("first dimension of optic flow vector\
- must have size three")
- if not is_numeric_array(opticFlow):
- raise TypeError("opticFlow must be of numerical type")
- if (not isinstance(viewing_direction, list)) and\
- (not isinstance(viewing_direction, np.ndarray)):
- raise TypeError("angles must be list or np.ndarray")
- if not is_numeric_array(viewing_direction):
- raise TypeError("viewing_direction must be of numerical type")
- if len(viewing_direction) != 2:
- raise Exception("first dimension of viewing\
- direction must be of size two")
- opticFlow = OFSubroutineYawPitchRoll(opticFlow, angles, True)
- OFT_x = opticFlow[0]
- OFT_y = opticFlow[1]
- OFT_z = opticFlow[2]
-
- epsilon = viewing_direction[1]
- phi = viewing_direction[0]
-
- # print("OFTs", opticFlow)
- # print("ep,phi", epsilon, phi)
-
- rofterm1 = +OFT_x*np.cos(epsilon)*np.cos(phi)
- rofterm2 = +OFT_y*np.cos(epsilon)*np.sin(phi)
- rofterm3 = +OFT_z*np.sin(epsilon)
- rof = rofterm1+rofterm2+rofterm3
-
- vofterm1 = -OFT_x*np.sin(epsilon)*np.cos(phi)
- vofterm2 = -OFT_y*np.sin(epsilon)*np.sin(phi)
- vofterm3 = OFT_z*np.cos(epsilon)
- vof = vofterm1+vofterm2+vofterm3
- hof = -OFT_x*np.sin(phi) + OFT_y*np.cos(phi)
-
- return [rof, hof, vof]
-
-
def optic_flow(scene, viewing_directions, velocity, distance_channel=3):
""" optic flow
:param scene: ibpc
@@ -427,6 +382,7 @@ def optic_flow(scene, viewing_directions, velocity, distance_channel=3):
rollNow = OFSubroutineYawPitchRoll([0, 0, 1], [0, 0, roll], False)
rollNext = OFSubroutineYawPitchRoll([0, 0, 1], [0, 0, roll+droll], False)
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]))
@@ -478,9 +434,10 @@ def optic_flow(scene, viewing_directions, velocity, distance_channel=3):
# Transform OF from Cartesian coordinates to Spherical coordinates
# according to method
- (OF_rho, OF_phi, OF_epsilon) = Matrix_transform(opticFlow,
- [yaw, pitch, roll],
- [a, e])
+ (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
diff --git a/navipy/processing/test_OpticFlow.py b/navipy/processing/test_OpticFlow.py
index fec681043034d66499c3f58ccc2e20b2e1aaae4e..2e9db4a333b864e567b6112ec29c3b0d398492e2 100644
--- a/navipy/processing/test_OpticFlow.py
+++ b/navipy/processing/test_OpticFlow.py
@@ -3,6 +3,8 @@ import navipy.processing.mcode as mcode
import pandas as pd
# import matplotlib.pyplot as plt
import numpy as np
+# from navipy.maths.homogeneous_transformations
+# import rotation_matrix, compose_matrix, decompose_matrix
def Scale(data, oldmin, oldmax, mini, maxi, ran):
@@ -103,7 +105,7 @@ class TestCase(unittest.TestCase):
self.viewing_directions,
velocity)
for convention in ['ryxz', 'ryzx', 'rxzy', 'rzyx',
- 'rzxy', 'alsjf', '233', 9, None, -1]:
+ 'rzxy', 'alsjf', '233', 9, -1]:
tuples = [('location', 'x'), ('location', 'y'),
('location', 'z'), ('location', 'dx'),
('location', 'dy'), ('location', 'dz'),
@@ -428,8 +430,6 @@ class TestCase(unittest.TestCase):
velocity['rxyz']['dalpha_1'] = dpitch
velocity['rxyz']['dalpha_2'] = droll
- print(velocity)
-
rof, hof, vof = mcode.optic_flow(scene, viewing_directions,
velocity)
cosel = np.cos(elevation)
@@ -691,7 +691,7 @@ class TestCase(unittest.TestCase):
self.velocity[self.convention]['dalpha_2'] = droll
rof, hof, vof = mcode.optic_flow(self.scene, self.viewing_directions,
self.velocity)
-
+ """
testrof = [[-4.88735544e-05, -4.88735544e-05, -4.88735544e-05],
[-4.91907871e-05, -4.94889779e-05, -4.97869621e-05],
[-4.94479358e-05, -5.00455194e-05, -5.06426694e-05]]
@@ -701,10 +701,88 @@ class TestCase(unittest.TestCase):
testvof = [[-0.09950539, -0.09948998, -0.09944426],
[-0.09950368, -0.0994883, -0.09944262],
[-0.09950195, -0.09948661, -0.09944095]]
+ """
+ testrof = [[-8.88472903e-19, -8.87544964e-19, -8.84761429e-19],
+ [1.30104261e-18, -2.16840434e-19, -8.67361738e-19],
+ [-4.33680869e-19, 0.00000000e+00, 4.33680869e-19]]
+ testhof = [[3.18909128e-10, 1.73652203e-03, 3.47251478e-03],
+ [-1.74233017e-04, 1.56202421e-03, 3.29775256e-03],
+ [-3.48413280e-04, 1.38705059e-03, 3.12198582e-03]]
+ testvof = [[-0.09950042, -0.09948526, -0.0994398],
+ [-0.09950042, -0.09948526, -0.0994398],
+ [-0.09950042, -0.09948526, -0.0994398]]
assert np.all(np.isclose(rof, testrof))
assert np.all(np.isclose(hof, testhof))
assert np.all(np.isclose(vof, testvof))
+ """
+ def test_findconv(self):
+ ypr=[1.57079633,0.1,0.1]
+ vec=[0, -0.09900333, 0.00993347]
+ tmpvec = vec
+
+
+ M1 = rotation_matrix(-ypr[2], [1, 0, 0])[:3, :3]
+ vec = np.transpose(np.dot(M1, np.transpose(vec)))
+ roty = np.transpose(np.dot(M1, np.transpose([0, 1, 0])))
+ M2 = rotation_matrix(-ypr[1], roty)[:3, :3]
+ vec = np.transpose(np.dot(M2, np.transpose(vec)))
+ rotz = np.transpose(np.dot(M1, np.transpose([0, 0, 1])))
+ #rotz = np.transpose(np.dot(M2, np.transpose(rotz)))
+ M4 = rotation_matrix(-ypr[1], [0, 1, 0])[:3, :3]
+ rotatedax = np.transpose(np.dot(M4, np.transpose(rotz)))
+
+ M3 = rotation_matrix(-ypr[0], rotatedax)
+ scale, shear, angles, translate, perspective =
+ decompose_matrix(M3,'rxyz')
+ print("angels", angles)
+ vec = np.transpose(np.dot(M3[:3,:3], np.transpose(vec)))
+ print("old vec", vec)
+ oldvec=vec
+
+ angles=[ypr[0], ypr[1],ypr[2], -ypr[0], -ypr[1], -ypr[2]]
+
+ #for c in ['sxyz','sxyx','sxzy','sxzx','syzx','syzy','syxz','syxy',
+ # 'szxy','szxz','szyx','szyz','rzyx','rxyx','ryzx','rxzx',
+ # 'rxzy','ryzy','rzxy','ryxy','ryxz','rzxz','rxyz','rzyz']:
+ for al in angles:
+ for bl in angles:
+ for cl in angles:
+ M = compose_matrix(scale=None, shear=None,
+ angles=[al, bl,cl],
+ translate=None,
+ perspective=None,
+ axes='rzyx')[:3,:3]
+ #M= np.transpose(M)
+ vec = np.dot(np.transpose(M), vec)
+ if (np.isclose(vec[0], oldvec[0]) or
+ np.isclose(vec[0], oldvec[1]) or
+ np.isclose(vec[0], oldvec[2]) or\
+ np.isclose(vec[0], -oldvec[0]) or
+ np.isclose(vec[0], -oldvec[1]) or
+ np.isclose(vec[0], -oldvec[2])) and\
+ (np.isclose(vec[1], oldvec[0]) or
+ np.isclose(vec[1], oldvec[1]) or
+ np.isclose(vec[1], oldvec[2]) or\
+ np.isclose(vec[1], -oldvec[0]) or
+ np.isclose(vec[1], -oldvec[1]) or
+ np.isclose(vec[1], -oldvec[2])) and\
+ (np.isclose(vec[2], oldvec[0]) or
+ np.isclose(vec[2], oldvec[1]) or
+ np.isclose(vec[2], oldvec[2]) or\
+ np.isclose(vec[2], -oldvec[0]) or
+ np.isclose(vec[2], -oldvec[1]) or
+ np.isclose(vec[2], -oldvec[2])):
+ print("found")
+ print("conve", al, bl, cl)
+ print("new vec", vec)
+ #scale, shear, angles, translate,
+ perspective =decompose_matrix(M3,c)
+ #print("angels", angles)
+ #print("old vec", oldvec)
+ print("new vec", vec)
+ vec=tmpvec
+ """
if __name__ == '__main__':