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 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 @@
"""
import numpy as np
from navipy.scene import is_numeric_array
from navipy.maths.homogeneous_transformations\
import compose_matrix
def cartesian_to_spherical(x, y, z):
......@@ -23,8 +21,7 @@ def spherical_to_cartesian(elevation, azimuth, radius=1):
return x, y, z
def cartesian_to_spherical_vectors(cart_vec, angles,
viewing_direction, axes='zyx'):
def cartesian_to_spherical_vectors(cart_vec, viewing_direction):
"""Now we need the cartesian vector as a spherical vecotr.
A vector in cartesian coordinates can be transform as one in
the spherical coordinates following the transformation:
......@@ -42,8 +39,6 @@ def cartesian_to_spherical_vectors(cart_vec, angles,
the orientation Yaw=pitch=roll=0"""
if cart_vec is 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:
raise ValueError("viewing direction must not be None")
if (not isinstance(cart_vec, np.ndarray)):
......@@ -62,10 +57,6 @@ def cartesian_to_spherical_vectors(cart_vec, angles,
raise Exception("first dimension of viewing\
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_y = cart_vec[1]
SPH_z = cart_vec[2]
......@@ -73,9 +64,6 @@ def cartesian_to_spherical_vectors(cart_vec, angles,
epsilon = viewing_direction[1]
phi = viewing_direction[0]
# print("OFTs", opticFlow)
# print("ep,phi", epsilon, phi)
rofterm1 = +SPH_x*np.cos(epsilon)*np.cos(phi)
rofterm2 = +SPH_y*np.cos(epsilon)*np.sin(phi)
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
import navipy.processing.mcode as mcode
import pandas as pd
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):
......@@ -256,108 +259,60 @@ class TestCase(unittest.TestCase):
the bee moves only in x-direction keeping the other
parameters (y,z,yaw,pitch,roll) constant
"""
positions = np.array([[-20, -20, 2.6, 1.57079633, 0, 0],
[-19.8, -20, 2.6, 1.57079633, 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 = [[2.88404299e-34, 8.22008280e-20, 1.64376617e-19],
[2.34252646e-05, 4.64310635e-05, 6.94159777e-05],
[9.36297157e-05, 1.38760866e-04, 1.83822856e-04]]
testhof = [[0.07692013, 0.07690842, 0.07687327],
[0.0768967, 0.07686158, 0.07680307],
[0.07682644, 0.07676796, 0.07668619]]
testvof = [[2.46536984e-10, 1.34244164e-03, 2.68447412e-03],
[1.34203275e-03, 2.68345936e-03, 4.02366094e-03],
[2.68120450e-03, 4.02043495e-03, 5.35762781e-03]]
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_new_vs_old(self):
"""
this test checks for the correct response if for example
the bee rotates only around the yaw axis keeping the other
parameters (x,y,z,pitch,roll) constant
"""
# 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))
convention = 'zyx'
tuples_posvel = posorient_columns(convention)
tuples_posvel.extend(velocities_columns(convention))
index_posvel = pd.MultiIndex.from_tuples(tuples_posvel,
names=['position',
'orientation'])
velocity = pd.Series(index=index_posvel, data=0)
velocity.loc[('location', 'dx')] = np.random.randn()
elevation = np.linspace(-np.pi/2, np.pi/2, 5)
azimuth = np.linspace(-np.pi, np.pi, 11)
[ma, me] = np.meshgrid(azimuth, elevation)
imshape = me.shape
vdir = np.zeros((ma.shape[0], ma.shape[1], 2))
vdir[..., __spherical_indeces__['elevation']] = me
vdir[..., __spherical_indeces__['azimuth']] = ma
scene = np.random.rand(imshape[0], imshape[1])
scene = np.tile(scene[..., np.newaxis], 4)
scene = scene[..., np.newaxis]
rof, hof, vof =\
mcode.optic_flow_translational(scene, vdir,
velocity, distance_channel=3)
hnorm = np.sqrt(hof**2 + vof ** 2)
# Add abs tol because we compare to zero
np.testing.assert_allclose(rof, np.zeros_like(rof), atol=1e-7)
# The motion is in the plane x,y.
# so no vof at null elevation
vof_el = vof[elevation == 0, :]
np.testing.assert_allclose(vof_el, np.zeros_like(vof_el), atol=1e-7)
# The translational optic flow norm should be small
# for further away objects
# except for the focus of contraction
# i.e. aximuth np.pi and el = 0
# and focus of expension
# i.e. aximuth 0 and el = 0
valid = (vdir[..., __spherical_indeces__['elevation']] == 0) \
& (vdir[..., __spherical_indeces__['azimuth']] == 0)
valid = valid | (
(vdir[..., __spherical_indeces__['elevation']] == 0)
& (vdir[..., __spherical_indeces__['azimuth']] == np.pi))
valid = valid | (
(vdir[..., __spherical_indeces__['elevation']] == 0)
& (vdir[..., __spherical_indeces__['azimuth']] == -np.pi))
valid = not valid
rof_further, hof_further, vof_further =\
mcode.optic_flow_translational(scene+1, vdir,
velocity, distance_channel=3)
hnorm_further = np.sqrt(hof_further**2 + vof_further**2)
np.testing.assert_array_less(hnorm_further[valid], hnorm[valid])
def test_only_yaw(self):
"""
......@@ -410,9 +365,9 @@ class TestCase(unittest.TestCase):
[-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))
np.testing.assert_allclose(rof, testrof)
np.testing.assert_allclose(hof, testhof)
np.testing.assert_allclose(vof, testvof)
def test_only_yaw_big(self):
"""
......@@ -481,7 +436,7 @@ class TestCase(unittest.TestCase):
if not has_zeros:
factor = np.array(hof[:, 0]/cosel)
assert np.all(factor == factor[0])
self.assertAlmostEquals(factor, factor[0])
def test_only_pitch(self):
"""
......@@ -531,9 +486,9 @@ class TestCase(unittest.TestCase):
[-1.74524096e-03, -1.74524096e-03, -1.74524096e-03],
[-3.48994999e-03, -3.48994999e-03, -3.48994999e-03]]
assert np.all(np.isclose(rof, testrof))
assert np.all(np.isclose(hof, testhof))
assert np.all(np.isclose(vof, testvof))
np.testing.assert_allclose(rof, testrof)
np.testing.assert_allclose(hof, testhof)
np.testing.assert_allclose(vof, testvof)
def test_only_pitch_big(self):
"""
......@@ -654,10 +609,10 @@ class TestCase(unittest.TestCase):
factorsin[has_zerossin] = factorsin[0]
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:
continue
assert np.isclose(factorsin[0], factorsin[i])
self.assertAlmostEqual(factorsin[0], factorsin[i])
# print(i)
# assert np.all(np.isclose(hof, testhof))
# assert np.all(np.isclose(vof, testvof))
......@@ -709,77 +664,9 @@ class TestCase(unittest.TestCase):
[0.09879836, 0.09893931, 0.09905007],
[0.09856329, 0.09870419, 0.09881489]]
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,'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
"""
np.testing.assert_allclose(rof, testrof)
np.testing.assert_allclose(hof, testhof)
np.testing.assert_allclose(vof, testvof)
if __name__ == '__main__':
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment