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

navipy/maths/euler.py

@@ -223,7 +223,7 @@ def angle_rate_matrix(ai, aj, ak, axes='xyz'):
     #    raise ValueError("quaternions must not be nan or none")
     if axes not in list(_AXES2TUPLE.keys()):
         raise ValueError("the chosen convention is not supported")
-    s, i, j, k = _AXES2TUPLE[axes]
+    _, i, j, k = _AXES2TUPLE[axes]
     ei = np.zeros(3)
     ej = np.zeros(3)
     ek = np.zeros(3)
@@ -232,19 +232,12 @@ def angle_rate_matrix(ai, aj, ak, axes='xyz'):
     ek[k] = 1
     matrixes = [R1, R2, R3]
     Rj = matrixes[j](aj)
-    rotM = None
-    if not s:
-        Rj = np.transpose(Rj)
-        Rk = matrixes[k](ak)
-        Rk = np.transpose(Rk)
-        p1 = np.dot(Rk, np.dot(Rj, ei))
-        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])
+    Rj = np.transpose(Rj)
+    Rk = matrixes[k](ak)
+    Rk = np.transpose(Rk)
+    p1 = np.dot(Rk, np.dot(Rj, ei))
+    p2 = np.dot(Rk, ej)
+    rotM = np.column_stack([p1, p2, ek])
     return rotM
 
 

navipy/processing/mcode.py

@@ -7,7 +7,8 @@ from navipy.scene import is_numeric_array
 from navipy.maths.homogeneous_transformations\
     import compose_matrix
 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 pandas as pd
 from navipy.maths.euler import angular_velocity
@@ -27,9 +28,11 @@ def _check_optic_flow_param(viewing_directions,
                          the convention used')
 
     index = velocity.index
-    convention = index.get_level_values(0)[-1]
+    convention = sorted(index.get_level_values(0))[-1]
     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):
         raise ValueError('missing index x')
     if 'y' not in velocity.index.get_level_values(1):
@@ -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)):
         raise ValueError('rotation exceeds 90°, computation aborted')
     # 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)
     vof = np.zeros_like(rof)
     # Calculate the angular velocities
-    opticFlowR = angular_velocity(yaw, pitch, roll,
-                                  dyaw, dpitch, droll, convention)
+    angvel = angular_velocity(yaw, pitch, roll,
+                              dyaw, dpitch, droll, convention)
+    M = compose_matrix(angles=[yaw, pitch, roll], translate=None,
+                       perspective=None, axes=convention)[:3, :3]
     # project it on the eye
     for i, a in enumerate(azimuth):
         for j, e in enumerate(elevation):
-            # Transform OF from Cartesian coordinates to Spherical coordinates
-            # according to method
+            # in the bee coordinate system
+            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) = \
                 cartesian_to_spherical_vectors(opticFlowR, [a, e])
             rof[j, i] = OF_rho
@@ -128,6 +139,7 @@ def optic_flow_translational(scene, viewing_directions,
     distance = scene[..., distance_channel, 0]
     distance[distance == 0] = np.nan  # Contact with object
     # and translational velocity
+    # Express in the global coordinate system
     u = [velocity['location']['dx'],
          velocity['location']['dy'],
          velocity['location']['dz']]
@@ -140,17 +152,24 @@ def optic_flow_translational(scene, viewing_directions,
     rof = np.zeros_like(viewing_directions[..., 0])
     hof = 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 j, e in enumerate(elevation):
+            # The spline express in the bee coordinate system
             spline = np.array(spherical_to_cartesian(e, a))
-            M = compose_matrix(angles=[yaw, pitch, roll], translate=None,
-                               perspective=None, axes=convention)[:3, :3]
-            spline = np.dot(M, spline)
+            # in the global coordinate system
+            spline = np.dot(M.transpose(), spline)
             # the Translation-part of the Optic Flow:
-            dotvu = np.dot(u, v)
+            dotvu = v*u
+            #opticFlowT = -np.cross(spline, dotvu)
             splinetrans = np.transpose(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) = \
                 cartesian_to_spherical_vectors(opticFlowT, [a, e])