Speed up OF calc

For loops on viewing directions have been removed.
It leads to 200x faster computation :)

navipy/processing/mcode.py

@@ -80,9 +80,9 @@ def optic_flow_rotationonal(viewing_directions, velocity):
     # Check if rotation are not too large
     # because we assume small rotation
     # according to Koenderink van Dorn
-    if ((np.abs(dyaw) > np.pi/2 and 2*np.pi - np.abs(dyaw) > np.pi/2) or
-        (np.abs(dpitch) > np.pi/2 and 2*np.pi - np.abs(dpitch) > np.pi/2) or
-            (np.abs(droll) > np.pi/2 and 2*np.pi - np.abs(droll) > np.pi/2)):
+    if ((np.abs(dyaw) > np.pi / 2 and 2 * np.pi - np.abs(dyaw) > np.pi / 2) or
+        (np.abs(dpitch) > np.pi / 2 and 2 * np.pi - np.abs(dpitch) > np.pi / 2) or
+            (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(elevation)
@@ -94,17 +94,22 @@ def optic_flow_rotationonal(viewing_directions, velocity):
     M = compose_matrix(angles=[yaw, pitch, roll], translate=None,
                        perspective=None, axes=convention)[:3, :3]
     angvel_bee = np.dot(M, angvel)
-    # project it on the eye
-    for i, (a, e) in enumerate(zip(azimuth, elevation)):
-        # in the bee coordinate system
-        spline = np.array(spherical_to_cartesian(e, a))
-        opticFlowR = -np.cross(angvel_bee, spline)
-        # Decompose into el, az
-        (OF_rho, OF_phi, OF_epsilon) = \
-            cartesian_to_spherical_vectors(opticFlowR, [a, e])
-        rof[i] = OF_rho
-        hof[i] = OF_phi
-        vof[i] = OF_epsilon
+    # The spline express in the bee coordinate system
+    spline = np.array(spherical_to_cartesian(elevation, azimuth))
+    # the Rotation-part of the Optic Flow in cartesian coord:
+    # Cross product of angvel_bee and spline
+    opticFlowR = np.zeros_like(spline)
+    opticFlowR[2, :] = angvel_bee[0] * \
+        spline[1, :] - angvel_bee[1] * spline[0, :]
+    opticFlowR[1, :] = angvel_bee[2] * \
+        spline[0, :] - angvel_bee[0] * spline[2, :]
+    opticFlowR[0, :] = angvel_bee[1] * \
+        spline[2, :] - angvel_bee[2] * spline[1, :]
+    opticFlowR = -opticFlowR
+    # opticFlow in spherical coordinate system
+    (rof, hof, vof) = cartesian_to_spherical_vectors(
+        opticFlowR, [azimuth, elevation])
+    # Reshape according to eye
     rof = np.reshape(rof, final_shape)
     hof = np.reshape(hof, final_shape)
     vof = np.reshape(vof, final_shape)
@@ -148,7 +153,8 @@ def optic_flow_translational(distance, viewing_directions,
     if(v == 0):
         u = [0, 0, 0]
     else:
-        u = u/np.linalg.norm(u)
+        u = u / np.linalg.norm(u)
+    # we init a matrix for rot
     # we init a matrix for rot
     rof = np.zeros_like(elevation)
     hof = np.zeros_like(rof)
@@ -157,22 +163,32 @@ def optic_flow_translational(distance, viewing_directions,
     M = compose_matrix(angles=[yaw, pitch, roll], translate=None,
                        perspective=None, axes=convention)[:3, :3]
     u_bee = M.dot(u)
-    for i, (a, e) in enumerate(zip(azimuth, elevation)):
-        # The spline express in the bee coordinate system
-        spline = np.array(spherical_to_cartesian(e, a))
-        # the Translation-part of the Optic Flow:
-        dotvu = v*u_bee
-        opticFlowT = -np.cross(np.cross(spline, dotvu), spline)
-        # Decompose into el, az
-        (OF_rho, OF_phi, OF_epsilon) = \
-            cartesian_to_spherical_vectors(opticFlowT, [a, e])
-
-        rof[i] = OF_rho
-        hof[i] = OF_phi
-        vof[i] = OF_epsilon
+    # The spline express in the bee coordinate system
+    spline = np.array(spherical_to_cartesian(elevation, azimuth))
+    # the Translation-part of the Optic Flow:
+    dotvu = v * u_bee
+    # cross product
+    crossprod = np.zeros_like(spline)
+    crossprod[2, :] = spline[0, :] * dotvu[1] - spline[1, :] * dotvu[0]
+    crossprod[1, :] = spline[2, :] * dotvu[0] - spline[0, :] * dotvu[2]
+    crossprod[0, :] = spline[1, :] * dotvu[2] - spline[2, :] * dotvu[1]
+    # Calc opticFlow in cartesian coordinate system
+    opticFlowT = np.zeros_like(spline)
+    opticFlowT[2, :] = crossprod[0, :] * \
+        spline[1, :] - crossprod[1, :] * spline[0, :]
+    opticFlowT[1, :] = crossprod[2, :] * \
+        spline[0, :] - crossprod[0, :] * spline[2, :]
+    opticFlowT[0, :] = crossprod[1, :] * \
+        spline[2, :] - crossprod[2, :] * spline[1, :]
+    opticFlowT = -opticFlowT
+    # opticFlow in spherical coordinate system
+    (rof, hof, vof) = cartesian_to_spherical_vectors(
+        opticFlowT, [azimuth, elevation])
+    # Divide  by distance
     rof /= distance
     hof /= distance
     vof /= distance
+    # Reshpae according to eye
     rof = np.reshape(rof, final_shape)
     hof = np.reshape(hof, final_shape)
     vof = np.reshape(vof, final_shape)
@@ -191,7 +207,7 @@ def optic_flow(distance, viewing_directions, velocity):
     rofr, hofr, vofr = optic_flow_rotationonal(viewing_directions, velocity)
     roft, hoft, voft = optic_flow_translational(distance, viewing_directions,
                                                 velocity)
-    return rofr+roft, hofr+hoft, vofr+voft
+    return rofr + roft, hofr + hoft, vofr + voft
 
 
 class Module():
@@ -345,7 +361,7 @@ class lp(Module):
         In = self.inM.Input
         for i in range(self.size[0]):
             for j in range(self.size[1]):
-                self.Input[i, j] += self.itau*(In[i, j]-self.Input[i, j])
+                self.Input[i, j] += self.itau * (In[i, j] - self.Input[i, j])
 
 
 class hp(Module):  # for second order just take hp for inM
@@ -403,7 +419,7 @@ class hp(Module):  # for second order just take hp for inM
         In = self.inM.Input
         for i in range(self.size[0]):
             for j in range(self.size[1]):
-                self.Input[i, j] = (In[i, j]-lpOut[i, j])
+                self.Input[i, j] = (In[i, j] - lpOut[i, j])
 
 
 class mul(Module):
@@ -490,7 +506,7 @@ class mul(Module):
             for j in range(self.size[1]):
                 sig_left1 = self.inM1.Input[i, j]
                 sig_left2 = shiftedInput[i, j]
-                self.Input[i, j] = sig_left1*sig_left2
+                self.Input[i, j] = sig_left1 * sig_left2
 
 
 class div(Module):
@@ -578,6 +594,6 @@ class div(Module):
                 sig_left1 = self.inM1.Input[i, j]
                 sig_left2 = shiftedInput[i, j]
                 if sig_left2 != 0:
-                    self.Input[i, j] = sig_left1/sig_left2
+                    self.Input[i, j] = sig_left1 / sig_left2
                 else:
                     self.Input[i, j] = 0