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

Optic flow rho is not always 0. and vector field in a random orientation ang vel look strange

parent 634c0c26
No related branches found
No related tags found
No related merge requests found
...@@ -223,7 +223,7 @@ def angle_rate_matrix(ai, aj, ak, axes='xyz'): ...@@ -223,7 +223,7 @@ def angle_rate_matrix(ai, aj, ak, axes='xyz'):
# raise ValueError("quaternions must not be nan or none") # raise ValueError("quaternions must not be nan or none")
if axes not in list(_AXES2TUPLE.keys()): if axes not in list(_AXES2TUPLE.keys()):
raise ValueError("the chosen convention is not supported") raise ValueError("the chosen convention is not supported")
s, i, j, k = _AXES2TUPLE[axes] _, i, j, k = _AXES2TUPLE[axes]
ei = np.zeros(3) ei = np.zeros(3)
ej = np.zeros(3) ej = np.zeros(3)
ek = np.zeros(3) ek = np.zeros(3)
...@@ -232,19 +232,12 @@ def angle_rate_matrix(ai, aj, ak, axes='xyz'): ...@@ -232,19 +232,12 @@ def angle_rate_matrix(ai, aj, ak, axes='xyz'):
ek[k] = 1 ek[k] = 1
matrixes = [R1, R2, R3] matrixes = [R1, R2, R3]
Rj = matrixes[j](aj) Rj = matrixes[j](aj)
rotM = None Rj = np.transpose(Rj)
if not s: Rk = matrixes[k](ak)
Rj = np.transpose(Rj) Rk = np.transpose(Rk)
Rk = matrixes[k](ak) p1 = np.dot(Rk, np.dot(Rj, ei))
Rk = np.transpose(Rk) p2 = np.dot(Rk, ej)
p1 = np.dot(Rk, np.dot(Rj, ei)) rotM = np.column_stack([p1, p2, ek])
p2 = np.dot(Rk, ej)
rotM = np.column_stack([p1, p2, ek])
else:
Ri = matrixes[i](ai)
p3 = np.dot(Ri, np.dot(Rj, ek))
p2 = np.dot(Ri, ej)
rotM = np.column_stack([ei, p2, p3])
return rotM return rotM
......
...@@ -7,7 +7,8 @@ from navipy.scene import is_numeric_array ...@@ -7,7 +7,8 @@ from navipy.scene import is_numeric_array
from navipy.maths.homogeneous_transformations\ from navipy.maths.homogeneous_transformations\
import compose_matrix import compose_matrix
from navipy.maths.coordinates\ from navipy.maths.coordinates\
import spherical_to_cartesian, cartesian_to_spherical_vectors import spherical_to_cartesian, cartesian_to_spherical_vectors, \
cartesian_to_spherical
import numpy as np import numpy as np
import pandas as pd import pandas as pd
from navipy.maths.euler import angular_velocity from navipy.maths.euler import angular_velocity
...@@ -27,9 +28,11 @@ def _check_optic_flow_param(viewing_directions, ...@@ -27,9 +28,11 @@ def _check_optic_flow_param(viewing_directions,
the convention used') the convention used')
index = velocity.index index = velocity.index
convention = index.get_level_values(0)[-1] convention = sorted(index.get_level_values(0))[-1]
if convention not in _AXES2TUPLE.keys(): if convention not in _AXES2TUPLE.keys():
raise ValueError("the chosen convention is not supported") msg = "the chosen convention {} is not supported"
msg = msg.format(convention)
raise ValueError(msg)
if 'x' not in velocity.index.get_level_values(1): if 'x' not in velocity.index.get_level_values(1):
raise ValueError('missing index x') raise ValueError('missing index x')
if 'y' not in velocity.index.get_level_values(1): if 'y' not in velocity.index.get_level_values(1):
...@@ -83,17 +86,25 @@ def optic_flow_rotationonal(viewing_directions, velocity): ...@@ -83,17 +86,25 @@ def optic_flow_rotationonal(viewing_directions, velocity):
(np.abs(droll) > np.pi/2 and 2*np.pi - np.abs(droll) > np.pi/2)): (np.abs(droll) > np.pi/2 and 2*np.pi - np.abs(droll) > np.pi/2)):
raise ValueError('rotation exceeds 90°, computation aborted') raise ValueError('rotation exceeds 90°, computation aborted')
# we init a matrix for rot # we init a matrix for rot
rof = np.zeros_like(velocity.shape[..., 0]) rof = np.zeros_like(viewing_directions[..., 0])
hof = np.zeros_like(rof) hof = np.zeros_like(rof)
vof = np.zeros_like(rof) vof = np.zeros_like(rof)
# Calculate the angular velocities # Calculate the angular velocities
opticFlowR = angular_velocity(yaw, pitch, roll, angvel = angular_velocity(yaw, pitch, roll,
dyaw, dpitch, droll, convention) dyaw, dpitch, droll, convention)
M = compose_matrix(angles=[yaw, pitch, roll], translate=None,
perspective=None, axes=convention)[:3, :3]
# project it on the eye # project it on the eye
for i, a in enumerate(azimuth): for i, a in enumerate(azimuth):
for j, e in enumerate(elevation): for j, e in enumerate(elevation):
# Transform OF from Cartesian coordinates to Spherical coordinates # in the bee coordinate system
# according to method spline = np.array(spherical_to_cartesian(e, a))
# in the global coordinate system
spline = np.dot(M.transpose(), spline)
opticFlowR = -np.cross(angvel, spline)
# in the bee coordinate system
opticFlowR = np.dot(M, opticFlowR)
# Decompose into el, az
(OF_rho, OF_phi, OF_epsilon) = \ (OF_rho, OF_phi, OF_epsilon) = \
cartesian_to_spherical_vectors(opticFlowR, [a, e]) cartesian_to_spherical_vectors(opticFlowR, [a, e])
rof[j, i] = OF_rho rof[j, i] = OF_rho
...@@ -128,6 +139,7 @@ def optic_flow_translational(scene, viewing_directions, ...@@ -128,6 +139,7 @@ def optic_flow_translational(scene, viewing_directions,
distance = scene[..., distance_channel, 0] distance = scene[..., distance_channel, 0]
distance[distance == 0] = np.nan # Contact with object distance[distance == 0] = np.nan # Contact with object
# and translational velocity # and translational velocity
# Express in the global coordinate system
u = [velocity['location']['dx'], u = [velocity['location']['dx'],
velocity['location']['dy'], velocity['location']['dy'],
velocity['location']['dz']] velocity['location']['dz']]
...@@ -140,17 +152,24 @@ def optic_flow_translational(scene, viewing_directions, ...@@ -140,17 +152,24 @@ def optic_flow_translational(scene, viewing_directions,
rof = np.zeros_like(viewing_directions[..., 0]) rof = np.zeros_like(viewing_directions[..., 0])
hof = np.zeros_like(rof) hof = np.zeros_like(rof)
vof = np.zeros_like(rof) vof = np.zeros_like(rof)
# transformation
M = compose_matrix(angles=[yaw, pitch, roll], translate=None,
perspective=None, axes=convention)[:3, :3]
for i, a in enumerate(azimuth): for i, a in enumerate(azimuth):
for j, e in enumerate(elevation): for j, e in enumerate(elevation):
# The spline express in the bee coordinate system
spline = np.array(spherical_to_cartesian(e, a)) spline = np.array(spherical_to_cartesian(e, a))
M = compose_matrix(angles=[yaw, pitch, roll], translate=None, # in the global coordinate system
perspective=None, axes=convention)[:3, :3] spline = np.dot(M.transpose(), spline)
spline = np.dot(M, spline)
# the Translation-part of the Optic Flow: # the Translation-part of the Optic Flow:
dotvu = np.dot(u, v) dotvu = v*u
#opticFlowT = -np.cross(spline, dotvu)
splinetrans = np.transpose(spline) splinetrans = np.transpose(spline)
opticFlowT = -(dotvu-(np.dot(dotvu, splinetrans))*spline) opticFlowT = -(dotvu-(np.dot(dotvu, splinetrans))*spline)
# in the bee coordinate system
opticFlowT = np.dot(M, opticFlowT)
# Decompose into el, az
(OF_rho, OF_phi, OF_epsilon) = \ (OF_rho, OF_phi, OF_epsilon) = \
cartesian_to_spherical_vectors(opticFlowT, [a, e]) cartesian_to_spherical_vectors(opticFlowT, [a, e])
......
This diff is collapsed.
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment