Merge branch 'Luise' into develop

navipy/maths/coordinates.py

@@ -2,6 +2,10 @@
  Conversion between coordinates systems
 """
 import numpy as np
+from navipy.maths.homogeneous_transformations import rotation_matrix
+from navipy.scene import is_numeric_array
+from navipy.maths.homogeneous_transformations\
+    import compose_matrix
 
 
 def cartesian_to_spherical(x, y, z):
@@ -21,3 +25,72 @@ def spherical_to_cartesian(elevation, azimuth, radius=1):
     z = np.sin(elevation)
     cartesian = radius * cartesian
     return x, y, z
+
+
+def cartesian_to_spherical_vectors(opticFlow, angles, viewing_direction):
+    """Now we need the optic Flow in the spherical coordinates.
+    A vector in cartesian coordinates can be transform as one in
+    the spherical coordinates following the transformation:
+
+    A_rho    =+A_x.*cos(epsilon).*cos(phi)
+              +A_y.*cos(epsilon).*sin(phi)
+              +A_z.*sin(epsilon)
+    A_epsilon=-A_x.*sin(epsilon).*cos(phi)
+              -A_y.*sin(epsilon).*sin(phi)
+              +A_z.*cos(epsilon)
+    A_phi    =-A_x.*sin(phi)+A_y.*cos(phi)
+
+    for epsilon in [-pi/2 +pi/2] and phi in [0 2pi]
+    reverse tajectory, needed because the frame x,y,z is expressed in
+    the orientation Yaw=pitch=roll=0"""
+    if opticFlow is None:
+        raise ValueError("opticFlow 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(opticFlow, np.ndarray)):
+        raise TypeError("vector must be of type np.ndarray")
+    if opticFlow.shape[0] != 3:
+        raise Exception("first dimension of optic flow vector\
+                         must have size three")
+    if not is_numeric_array(opticFlow):
+        raise TypeError("opticFlow must be of numerical type")
+    if (not isinstance(viewing_direction, list)) and\
+       (not isinstance(viewing_direction, np.ndarray)):
+        raise TypeError("angles must be list or np.ndarray")
+    if not is_numeric_array(viewing_direction):
+        raise TypeError("viewing_direction must be of numerical type")
+    if len(viewing_direction) != 2:
+        raise Exception("first dimension of viewing\
+                         direction must be of size two")
+
+    vec = opticFlow
+    ypr=angles
+    M = compose_matrix(scale=None, shear=None, angles=ypr, translate=None,
+                       perspective=None, axes='rzyx')[:3, :3]
+    vec = np.dot(np.transpose(M), vec)
+    opticFlow = vec
+
+    OFT_x = opticFlow[0]
+    OFT_y = opticFlow[1]
+    OFT_z = opticFlow[2]
+
+    epsilon = viewing_direction[1]
+    phi = viewing_direction[0]
+
+    # print("OFTs", opticFlow)
+    # print("ep,phi", epsilon, phi)
+
+    rofterm1 = +OFT_x*np.cos(epsilon)*np.cos(phi)
+    rofterm2 = +OFT_y*np.cos(epsilon)*np.sin(phi)
+    rofterm3 = +OFT_z*np.sin(epsilon)
+    rof = rofterm1+rofterm2+rofterm3
+
+    vofterm1 = -OFT_x*np.sin(epsilon)*np.cos(phi)
+    vofterm2 = -OFT_y*np.sin(epsilon)*np.sin(phi)
+    vofterm3 = OFT_z*np.cos(epsilon)
+    vof = vofterm1+vofterm2+vofterm3
+    hof = -OFT_x*np.sin(phi) + OFT_y*np.cos(phi)
+
+    return [rof, hof, vof]

navipy/processing/mcode.py

@@ -4,7 +4,10 @@ Motion code
 from navipy.scene import check_scene
 from navipy.scene import __spherical_indeces__
 from navipy.scene import is_numeric_array
-from navipy.maths.homogeneous_transformations import rotation_matrix
+from navipy.maths.homogeneous_transformations\
+    import compose_matrix
+from navipy.maths.coordinates\
+    import spherical_to_cartesian, cartesian_to_spherical_vectors
 import numpy as np
 import pandas as pd
 
@@ -82,10 +85,13 @@ NOTE: this is NOT the normal spheic coordinate system!
         raise ValueError("azimuth must be float or integer")
     if (not isinstance(sp[1], float)) and (not isinstance(sp[1], int)):
         raise ValueError("elevation must be float or integer")
+    """
     vector = np.zeros((3,))
     vector = [np.dot(np.cos(sp[1]), np.cos(sp[0])),
               np.dot(np.cos(sp[1]), np.sin(sp[0])),
               np.sin(sp[1])]
+    """
+    vector = np.array(spherical_to_cartesian(sp[1], sp[0]))
 
     return vector
 
@@ -248,8 +254,12 @@ def OFSubroutineYawPitchRoll(vec, ypr, inverse):
     # end
 
     # rotations around relative rotatet axis
-
+    M = compose_matrix(scale=None, shear=None, angles=ypr, translate=None,
+                       perspective=None, axes='rzyx')[:3, :3]
     if (inverse):
+        vec = np.dot(np.transpose(M), vec)
+        """
+        vec=tmpvec
         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])))
@@ -259,9 +269,14 @@ def OFSubroutineYawPitchRoll(vec, ypr, inverse):
         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)[:3, :3]
+        M3 = rotation_matrix(-ypr[0], rotatedax)[:3,:3]
         vec = np.transpose(np.dot(M3, np.transpose(vec)))
+        """
     else:
+        vec = np.dot(M, vec)
+        """
+        print("new normal vec", vec)
+        vec = tmpvec
         M1 = rotation_matrix(ypr[0], [0, 0, 1])[:3, :3]
         vec = np.transpose(np.dot(M1, np.transpose(vec)))
         roty = np.transpose(np.dot(M1, np.transpose([0, 1, 0])))
@@ -273,72 +288,12 @@ def OFSubroutineYawPitchRoll(vec, ypr, inverse):
 
         M3 = rotation_matrix(ypr[2], rotatedax)[:3, :3]
         vec = np.transpose(np.dot(M3, np.transpose(vec)))
+        print("old normal vec", vec)
+        """
 
     return vec
 
 
-def Matrix_transform(opticFlow, angles, viewing_direction):
-    """Now we need the optic Flow in the spherical coordinates.
-    A vector in cartesian coordinates can be transform as one in
-    the spherical coordinates following the transformation:
-
-    A_rho    =+A_x.*cos(epsilon).*cos(phi)
-              +A_y.*cos(epsilon).*sin(phi)
-              +A_z.*sin(epsilon)
-    A_epsilon=-A_x.*sin(epsilon).*cos(phi)
-              -A_y.*sin(epsilon).*sin(phi)
-              +A_z.*cos(epsilon)
-    A_phi    =-A_x.*sin(phi)+A_y.*cos(phi)
-
-    for epsilon in [-pi/2 +pi/2] and phi in [0 2pi]
-    reverse tajectory, needed because the frame x,y,z is expressed in
-    the orientation Yaw=pitch=roll=0"""
-    if opticFlow is None:
-        raise ValueError("opticFlow 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(opticFlow, np.ndarray)):
-        raise TypeError("vector must be of type np.ndarray")
-    if opticFlow.shape[0] != 3:
-        raise Exception("first dimension of optic flow vector\
-                         must have size three")
-    if not is_numeric_array(opticFlow):
-        raise TypeError("opticFlow must be of numerical type")
-    if (not isinstance(viewing_direction, list)) and\
-       (not isinstance(viewing_direction, np.ndarray)):
-        raise TypeError("angles must be list or np.ndarray")
-    if not is_numeric_array(viewing_direction):
-        raise TypeError("viewing_direction must be of numerical type")
-    if len(viewing_direction) != 2:
-        raise Exception("first dimension of viewing\
-                         direction must be of size two")
-    opticFlow = OFSubroutineYawPitchRoll(opticFlow, angles, True)
-    OFT_x = opticFlow[0]
-    OFT_y = opticFlow[1]
-    OFT_z = opticFlow[2]
-
-    epsilon = viewing_direction[1]
-    phi = viewing_direction[0]
-
-    # print("OFTs", opticFlow)
-    # print("ep,phi", epsilon, phi)
-
-    rofterm1 = +OFT_x*np.cos(epsilon)*np.cos(phi)
-    rofterm2 = +OFT_y*np.cos(epsilon)*np.sin(phi)
-    rofterm3 = +OFT_z*np.sin(epsilon)
-    rof = rofterm1+rofterm2+rofterm3
-
-    vofterm1 = -OFT_x*np.sin(epsilon)*np.cos(phi)
-    vofterm2 = -OFT_y*np.sin(epsilon)*np.sin(phi)
-    vofterm3 = OFT_z*np.cos(epsilon)
-    vof = vofterm1+vofterm2+vofterm3
-    hof = -OFT_x*np.sin(phi) + OFT_y*np.cos(phi)
-
-    return [rof, hof, vof]
-
-
 def optic_flow(scene, viewing_directions, velocity, distance_channel=3):
     """ optic flow
     :param scene: ibpc
@@ -427,6 +382,7 @@ def optic_flow(scene, viewing_directions, velocity, distance_channel=3):
     rollNow = OFSubroutineYawPitchRoll([0, 0, 1], [0, 0, roll], False)
     rollNext = OFSubroutineYawPitchRoll([0, 0, 1], [0, 0, roll+droll], False)
     rRoll = np.cross(rollNow, rollNext)
+
     rof = np.zeros((azimuth.shape[0], elevation.shape[0]))
     hof = np.zeros((azimuth.shape[0], elevation.shape[0]))
     vof = np.zeros((azimuth.shape[0], elevation.shape[0]))
@@ -478,9 +434,10 @@ def optic_flow(scene, viewing_directions, velocity, distance_channel=3):
 
             # Transform OF from Cartesian coordinates to Spherical coordinates
             # according to method
-            (OF_rho, OF_phi, OF_epsilon) = Matrix_transform(opticFlow,
-                                                            [yaw, pitch, roll],
-                                                            [a, e])
+            (OF_rho, OF_phi, OF_epsilon) =\
+                cartesian_to_spherical_vectors(opticFlow,
+                                               [yaw, pitch, roll],
+                                               [a, e])
 
             rof[j, i] = OF_rho
             hof[j, i] = OF_phi

navipy/processing/test_OpticFlow.py

@@ -3,6 +3,8 @@ import navipy.processing.mcode as mcode
 import pandas as pd
 # import matplotlib.pyplot as plt
 import numpy as np
+# from navipy.maths.homogeneous_transformations
+# import rotation_matrix, compose_matrix, decompose_matrix
 
 
 def Scale(data, oldmin, oldmax, mini, maxi, ran):
@@ -103,7 +105,7 @@ class TestCase(unittest.TestCase):
                                              self.viewing_directions,
                                              velocity)
         for convention in ['ryxz', 'ryzx', 'rxzy', 'rzyx',
-                           'rzxy', 'alsjf', '233', 9, None, -1]:
+                           'rzxy', 'alsjf', '233', 9, -1]:
             tuples = [('location', 'x'), ('location', 'y'),
                       ('location', 'z'), ('location', 'dx'),
                       ('location', 'dy'), ('location', 'dz'),
@@ -428,8 +430,6 @@ class TestCase(unittest.TestCase):
         velocity['rxyz']['dalpha_1'] = dpitch
         velocity['rxyz']['dalpha_2'] = droll
 
-        print(velocity)
-
         rof, hof, vof = mcode.optic_flow(scene, viewing_directions,
                                          velocity)
         cosel = np.cos(elevation)
@@ -691,7 +691,7 @@ class TestCase(unittest.TestCase):
         self.velocity[self.convention]['dalpha_2'] = droll
         rof, hof, vof = mcode.optic_flow(self.scene, self.viewing_directions,
                                          self.velocity)
-
+        """
         testrof = [[-4.88735544e-05, -4.88735544e-05, -4.88735544e-05],
                    [-4.91907871e-05, -4.94889779e-05, -4.97869621e-05],
                    [-4.94479358e-05, -5.00455194e-05, -5.06426694e-05]]
@@ -701,10 +701,88 @@ class TestCase(unittest.TestCase):
         testvof = [[-0.09950539, -0.09948998, -0.09944426],
                    [-0.09950368, -0.0994883, -0.09944262],
                    [-0.09950195, -0.09948661, -0.09944095]]
+        """
+        testrof = [[-8.88472903e-19, -8.87544964e-19, -8.84761429e-19],
+                   [1.30104261e-18, -2.16840434e-19, -8.67361738e-19],
+                   [-4.33680869e-19, 0.00000000e+00, 4.33680869e-19]]
+        testhof = [[3.18909128e-10, 1.73652203e-03, 3.47251478e-03],
+                   [-1.74233017e-04, 1.56202421e-03, 3.29775256e-03],
+                   [-3.48413280e-04, 1.38705059e-03, 3.12198582e-03]]
+        testvof = [[-0.09950042, -0.09948526, -0.0994398],
+                   [-0.09950042, -0.09948526, -0.0994398],
+                   [-0.09950042, -0.09948526, -0.0994398]]
 
         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,'rxyz')
+        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 ['sxyz','sxyx','sxzy','sxzx','syzx','syzy','syxz','syxy',
+        #          'szxy','szxz','szyx','szyz','rzyx','rxyx','ryzx','rxzx',
+        #          'rxzy','ryzy','rzxy','ryxy','ryxz','rzxz','rxyz','rzyz']:
+        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='rzyx')[: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__':