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

split OF functions

parent 5ad952a7
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
navipy/maths/coordinates.py
+ 1
− 13
View file @ 634c0c26
...@@ -3,8 +3,6 @@ ...@@ -3,8 +3,6 @@
""" """
import numpy as np import numpy as np
from navipy.scene import is_numeric_array from navipy.scene import is_numeric_array
from navipy.maths.homogeneous_transformations\
import compose_matrix
def cartesian_to_spherical(x, y, z): def cartesian_to_spherical(x, y, z):
...@@ -23,8 +21,7 @@ def spherical_to_cartesian(elevation, azimuth, radius=1): ...@@ -23,8 +21,7 @@ def spherical_to_cartesian(elevation, azimuth, radius=1):
return x, y, z return x, y, z
def cartesian_to_spherical_vectors(cart_vec, angles, def cartesian_to_spherical_vectors(cart_vec, viewing_direction):
viewing_direction, axes='zyx'):
"""Now we need the cartesian vector as a spherical vecotr. """Now we need the cartesian vector as a spherical vecotr.
A vector in cartesian coordinates can be transform as one in A vector in cartesian coordinates can be transform as one in
the spherical coordinates following the transformation: the spherical coordinates following the transformation:
...@@ -42,8 +39,6 @@ def cartesian_to_spherical_vectors(cart_vec, angles, ...@@ -42,8 +39,6 @@ def cartesian_to_spherical_vectors(cart_vec, angles,
the orientation Yaw=pitch=roll=0""" the orientation Yaw=pitch=roll=0"""
if cart_vec is None: if cart_vec is None:
raise ValueError("cartesian vector must not be None") raise ValueError("cartesian vector must not be None")
if angles is None:
raise ValueError("angles must not be None")
if viewing_direction is None: if viewing_direction is None:
raise ValueError("viewing direction must not be None") raise ValueError("viewing direction must not be None")
if (not isinstance(cart_vec, np.ndarray)): if (not isinstance(cart_vec, np.ndarray)):
...@@ -62,10 +57,6 @@ def cartesian_to_spherical_vectors(cart_vec, angles, ...@@ -62,10 +57,6 @@ def cartesian_to_spherical_vectors(cart_vec, angles,
raise Exception("first dimension of viewing\ raise Exception("first dimension of viewing\
direction must be of size two") direction must be of size two")
M = compose_matrix(scale=None, shear=None, angles=angles, translate=None,
perspective=None, axes=axes)[:3, :3]
cart_vec = np.dot(np.transpose(M), cart_vec)
SPH_x = cart_vec[0] SPH_x = cart_vec[0]
SPH_y = cart_vec[1] SPH_y = cart_vec[1]
SPH_z = cart_vec[2] SPH_z = cart_vec[2]
...@@ -73,9 +64,6 @@ def cartesian_to_spherical_vectors(cart_vec, angles, ...@@ -73,9 +64,6 @@ def cartesian_to_spherical_vectors(cart_vec, angles,
epsilon = viewing_direction[1] epsilon = viewing_direction[1]
phi = viewing_direction[0] phi = viewing_direction[0]
# print("OFTs", opticFlow)
# print("ep,phi", epsilon, phi)
rofterm1 = +SPH_x*np.cos(epsilon)*np.cos(phi) rofterm1 = +SPH_x*np.cos(epsilon)*np.cos(phi)
rofterm2 = +SPH_y*np.cos(epsilon)*np.sin(phi) rofterm2 = +SPH_y*np.cos(epsilon)*np.sin(phi)
rofterm3 = +SPH_z*np.sin(epsilon) rofterm3 = +SPH_z*np.sin(epsilon)
......
navipy/processing/mcode.py
+ 152
− 265
View file @ 634c0c26
This diff is collapsed.
navipy/processing/test_OpticFlow.py
+ 69
− 182
View file @ 634c0c26
...@@ -2,6 +2,9 @@ import unittest ...@@ -2,6 +2,9 @@ import unittest
import navipy.processing.mcode as mcode import navipy.processing.mcode as mcode
import pandas as pd import pandas as pd
import numpy as np import numpy as np
from navipy.moving.agent import posorient_columns
from navipy.moving.agent import velocities_columns
from navipy.scene import __spherical_indeces__
def Scale(data, oldmin, oldmax, mini, maxi, ran): def Scale(data, oldmin, oldmax, mini, maxi, ran):
...@@ -256,108 +259,60 @@ class TestCase(unittest.TestCase): ...@@ -256,108 +259,60 @@ class TestCase(unittest.TestCase):
the bee moves only in x-direction keeping the other the bee moves only in x-direction keeping the other
parameters (y,z,yaw,pitch,roll) constant parameters (y,z,yaw,pitch,roll) constant
""" """
positions = np.array([[-20, -20, 2.6, 1.57079633, 0, 0], convention = 'zyx'
[-19.8, -20, 2.6, 1.57079633, 0, 0]]) tuples_posvel = posorient_columns(convention)
tuples_posvel.extend(velocities_columns(convention))
x = positions[0, 0] index_posvel = pd.MultiIndex.from_tuples(tuples_posvel,
y = positions[0, 1] names=['position',
z = positions[0, 2] 'orientation'])
yaw = positions[0, 3] velocity = pd.Series(index=index_posvel, data=0)
pitch = positions[0, 4] velocity.loc[('location', 'dx')] = np.random.randn()
roll = positions[0, 5]
elevation = np.linspace(-np.pi/2, np.pi/2, 5)
dx = positions[1, 0] - positions[0, 0] azimuth = np.linspace(-np.pi, np.pi, 11)
dy = positions[1, 1] - positions[0, 1] [ma, me] = np.meshgrid(azimuth, elevation)
dz = positions[1, 2] - positions[0, 2] imshape = me.shape
dyaw = positions[1, 3] - positions[0, 3] vdir = np.zeros((ma.shape[0], ma.shape[1], 2))
dpitch = positions[1, 4] - positions[0, 4] vdir[..., __spherical_indeces__['elevation']] = me
droll = positions[1, 5] - positions[0, 5] vdir[..., __spherical_indeces__['azimuth']] = ma
self.velocity['location']['x'] = x scene = np.random.rand(imshape[0], imshape[1])
self.velocity['location']['y'] = y scene = np.tile(scene[..., np.newaxis], 4)
self.velocity['location']['z'] = z scene = scene[..., np.newaxis]
self.velocity[self.convention]['alpha_0'] = yaw
self.velocity[self.convention]['alpha_1'] = pitch rof, hof, vof =\
self.velocity[self.convention]['alpha_2'] = roll mcode.optic_flow_translational(scene, vdir,
self.velocity['location']['dx'] = dx velocity, distance_channel=3)
self.velocity['location']['dy'] = dy hnorm = np.sqrt(hof**2 + vof ** 2)
self.velocity['location']['dz'] = dz # Add abs tol because we compare to zero
self.velocity[self.convention]['dalpha_0'] = dyaw np.testing.assert_allclose(rof, np.zeros_like(rof), atol=1e-7)
self.velocity[self.convention]['dalpha_1'] = dpitch # The motion is in the plane x,y.
self.velocity[self.convention]['dalpha_2'] = droll # so no vof at null elevation
vof_el = vof[elevation == 0, :]
rof, hof, vof = mcode.optic_flow(self.scene, np.testing.assert_allclose(vof_el, np.zeros_like(vof_el), atol=1e-7)
self.viewing_directions,
self.velocity) # The translational optic flow norm should be small
# for further away objects
testrof = [[2.88404299e-34, 8.22008280e-20, 1.64376617e-19], # except for the focus of contraction
[2.34252646e-05, 4.64310635e-05, 6.94159777e-05], # i.e. aximuth np.pi and el = 0
[9.36297157e-05, 1.38760866e-04, 1.83822856e-04]] # and focus of expension
testhof = [[0.07692013, 0.07690842, 0.07687327], # i.e. aximuth 0 and el = 0
[0.0768967, 0.07686158, 0.07680307], valid = (vdir[..., __spherical_indeces__['elevation']] == 0) \
[0.07682644, 0.07676796, 0.07668619]] & (vdir[..., __spherical_indeces__['azimuth']] == 0)
testvof = [[2.46536984e-10, 1.34244164e-03, 2.68447412e-03], valid = valid | (
[1.34203275e-03, 2.68345936e-03, 4.02366094e-03], (vdir[..., __spherical_indeces__['elevation']] == 0)
[2.68120450e-03, 4.02043495e-03, 5.35762781e-03]] & (vdir[..., __spherical_indeces__['azimuth']] == np.pi))
valid = valid | (
assert np.all(np.isclose(rof, testrof)) (vdir[..., __spherical_indeces__['elevation']] == 0)
assert np.all(np.isclose(hof, testhof)) & (vdir[..., __spherical_indeces__['azimuth']] == -np.pi))
assert np.all(np.isclose(vof, testvof)) valid = not valid
def test_only_yaw_new_vs_old(self): rof_further, hof_further, vof_further =\
""" mcode.optic_flow_translational(scene+1, vdir,
this test checks for the correct response if for example velocity, distance_channel=3)
the bee rotates only around the yaw axis keeping the other hnorm_further = np.sqrt(hof_further**2 + vof_further**2)
parameters (x,y,z,pitch,roll) constant
""" np.testing.assert_array_less(hnorm_further[valid], hnorm[valid])
# yaw only everything zero
# only vertical should change, horizontal stays same
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]
self.velocity['location']['x'] = x
self.velocity['location']['y'] = y
self.velocity['location']['z'] = z
self.velocity[self.convention]['alpha_0'] = yaw
self.velocity[self.convention]['alpha_1'] = pitch
self.velocity[self.convention]['alpha_2'] = roll
self.velocity['location']['dx'] = dx
self.velocity['location']['dy'] = dy
self.velocity['location']['dz'] = dz
self.velocity[self.convention]['dalpha_0'] = dyaw
self.velocity[self.convention]['dalpha_1'] = dpitch
self.velocity[self.convention]['dalpha_2'] = droll
rof, hof, vof = mcode.optic_flow(self.scene, self.viewing_directions,
self.velocity)
testrof = [[-6.47644748e-26, -3.52655120e-19, -7.05202754e-19],
[-1.84591335e-11, -1.00513560e-04, -2.00996485e-04],
[-3.69126442e-11, -2.00996503e-04, -4.01931744e-04]]
testhof = [[-0.33, -0.32994974, -0.32979897],
[-0.33, -0.32994974, -0.32979897],
[-0.33, -0.32994974, -0.32979897]]
testvof = [[-1.05768414e-09, -5.75929518e-03, -1.15168350e-02],
[-1.05752305e-09, -5.75841801e-03, -1.15150809e-02],
[-1.05703983e-09, -5.75578677e-03, -1.15098192e-02]]
assert np.all(np.isclose(rof, testrof))
assert np.all(np.isclose(hof, testhof))
assert np.all(np.isclose(vof, testvof))
def test_only_yaw(self): def test_only_yaw(self):
""" """
...@@ -410,9 +365,9 @@ class TestCase(unittest.TestCase): ...@@ -410,9 +365,9 @@ class TestCase(unittest.TestCase):
[-1.05752305e-09, -5.75841801e-03, -1.15150809e-02], [-1.05752305e-09, -5.75841801e-03, -1.15150809e-02],
[-1.05703983e-09, -5.75578677e-03, -1.15098192e-02]] [-1.05703983e-09, -5.75578677e-03, -1.15098192e-02]]
assert np.all(np.isclose(rof, testrof)) np.testing.assert_allclose(rof, testrof)
assert np.all(np.isclose(hof, testhof)) np.testing.assert_allclose(hof, testhof)
assert np.all(np.isclose(vof, testvof)) np.testing.assert_allclose(vof, testvof)
def test_only_yaw_big(self): def test_only_yaw_big(self):
""" """
...@@ -481,7 +436,7 @@ class TestCase(unittest.TestCase): ...@@ -481,7 +436,7 @@ class TestCase(unittest.TestCase):
if not has_zeros: if not has_zeros:
factor = np.array(hof[:, 0]/cosel) factor = np.array(hof[:, 0]/cosel)
assert np.all(factor == factor[0]) self.assertAlmostEquals(factor, factor[0])
def test_only_pitch(self): def test_only_pitch(self):
""" """
...@@ -531,9 +486,9 @@ class TestCase(unittest.TestCase): ...@@ -531,9 +486,9 @@ class TestCase(unittest.TestCase):
[-1.74524096e-03, -1.74524096e-03, -1.74524096e-03], [-1.74524096e-03, -1.74524096e-03, -1.74524096e-03],
[-3.48994999e-03, -3.48994999e-03, -3.48994999e-03]] [-3.48994999e-03, -3.48994999e-03, -3.48994999e-03]]
assert np.all(np.isclose(rof, testrof)) np.testing.assert_allclose(rof, testrof)
assert np.all(np.isclose(hof, testhof)) np.testing.assert_allclose(hof, testhof)
assert np.all(np.isclose(vof, testvof)) np.testing.assert_allclose(vof, testvof)
def test_only_pitch_big(self): def test_only_pitch_big(self):
""" """
...@@ -654,10 +609,10 @@ class TestCase(unittest.TestCase): ...@@ -654,10 +609,10 @@ class TestCase(unittest.TestCase):
factorsin[has_zerossin] = factorsin[0] factorsin[has_zerossin] = factorsin[0]
for i in range(1, len(factor)): for i in range(1, len(factor)):
assert np.isclose(factor[1], factor[i]) self.assertAlmostEqual(np.isclose(factor[1], factor[i]))
if i == 90: if i == 90:
continue continue
assert np.isclose(factorsin[0], factorsin[i]) self.assertAlmostEqual(factorsin[0], factorsin[i])
# print(i) # print(i)
# assert np.all(np.isclose(hof, testhof)) # assert np.all(np.isclose(hof, testhof))
# assert np.all(np.isclose(vof, testvof)) # assert np.all(np.isclose(vof, testvof))
...@@ -709,77 +664,9 @@ class TestCase(unittest.TestCase): ...@@ -709,77 +664,9 @@ class TestCase(unittest.TestCase):
[0.09879836, 0.09893931, 0.09905007], [0.09879836, 0.09893931, 0.09905007],
[0.09856329, 0.09870419, 0.09881489]] [0.09856329, 0.09870419, 0.09881489]]
assert np.all(np.isclose(rof, testrof)) np.testing.assert_allclose(rof, testrof)
assert np.all(np.isclose(hof, testhof)) np.testing.assert_allclose(hof, testhof)
assert np.all(np.isclose(vof, testvof)) np.testing.assert_allclose(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,'xyz')
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 ['xyz','xyx','xzy','xzx','yzx','yzy','yxz','yxy',
# 'zxy','zxz','zyx','zyz','zyx','xyx','yzx','xzx',
# 'xzy','yzy','zxy','yxy','yxz','zxz','xyz','zyz']:
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='zyx')[: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__': if __name__ == '__main__':
......
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