diff --git a/navipy/comparing/__init__.py b/navipy/comparing/__init__.py
index 7d32eaadc2e720b744212fd09d1fd2402671808d..a918ca387d40c6c41f68e1dffcf351a2c8e328c6 100644
--- a/navipy/comparing/__init__.py
+++ b/navipy/comparing/__init__.py
@@ -92,12 +92,17 @@ def diff_optic_flow(current, memory):
to memory by using the optic flow under the
constrain that the brightness is constant, (small movement),
using a taylor expansion and solving the equation:
-0=I_t+\delta I*<u,v> or I_x+I_y+I_t=0
+.. math::
+
+ 0=I_t+\delta I*<u,v> or I_x+I_y+I_t=0
afterwards the aperture problem is solved by a
Matrix equation Ax=b, where x=(u,v) and
-A=(I_x,I_y) and b = I_t
+.. math::
+
+ A=(I_x,I_y) and b = I_t
+The intput parameters are the following:
:param current: current place code
:param memory: memorised place code
:returns: a directional vectors
diff --git a/navipy/comparing/test.py b/navipy/comparing/test.py
index 03df852834d84984f6fbd04d7d898d4f0c32cd60..18b6a8e0ca6a9188e165b37380309df14680f7a7 100644
--- a/navipy/comparing/test.py
+++ b/navipy/comparing/test.py
@@ -8,11 +8,24 @@ import pkg_resources
class TestCase(unittest.TestCase):
def setUp(self):
+ """ loads the database """
self.mydb_filename = pkg_resources.resource_filename(
'navipy', 'resources/database.db')
self.mydb = database.DataBaseLoad(self.mydb_filename)
def test_imagediff_curr(self):
+ """
+ this test checks the function imdiff works
+ correctly.
+ it checks if correct errors are raised for:
+ - frame containing nans
+ - frame has wrong dimension
+
+ and checks if the returned frame is correct:
+ - has correct shape
+ - does not contain nans
+ - contains only numeric values
+ """
curr = self.mydb.scene(rowid=1)
mem = self.mydb.scene(rowid=2)
curr2 = curr.copy()
@@ -39,6 +52,18 @@ class TestCase(unittest.TestCase):
self.assertTrue(is_numeric_array(diff))
def test_imagediff_memory(self):
+ """
+ this test checks the function imagediff works
+ correctly for the parameter memory.
+ it checks if correct errors are raised for:
+ - memory containing nans
+ - memory has wrong dimension
+
+ and checks if the returned frame is correct:
+ - has correct shape
+ - does not contain nans
+ - contains only numeric values
+ """
curr = self.mydb.scene(rowid=1)
mem = self.mydb.scene(rowid=2)
mem2 = curr.copy()
@@ -64,6 +89,18 @@ class TestCase(unittest.TestCase):
self.assertTrue(is_numeric_array(diff))
def test_rot_imagediff_curr(self):
+ """
+ this test checks the function imagediff works
+ correctly for the parameter current.
+ it checks if correct errors are raised for:
+ - memory containing nans
+ - memory has wrong dimension
+
+ and checks if the returned frame is correct:
+ - has correct shape
+ - does not contain nans
+ - contains only numeric values
+ """
curr = self.mydb.scene(rowid=1)
mem = self.mydb.scene(rowid=2)
curr2 = curr.copy()
@@ -89,6 +126,18 @@ class TestCase(unittest.TestCase):
self.assertTrue(is_numeric_array(diff))
def test_rotimagediff_memory(self):
+ """
+ this test checks the function rot_imagediff works
+ correctly for the parameter memory.
+ it checks if correct errors are raised for:
+ - memory containing nans
+ - memory has wrong dimension
+
+ and checks if the returned frame is correct:
+ - has correct shape
+ - does not contain nans
+ - contains only numeric values
+ """
curr = self.mydb.scene(rowid=1)
mem = self.mydb.scene(rowid=2)
mem2 = curr.copy()
@@ -114,6 +163,18 @@ class TestCase(unittest.TestCase):
self.assertTrue(is_numeric_array(diff))
def test_simple_imagediff_curr(self):
+ """
+ this test checks the function simple_imagediff works
+ correctly for the parameter current.
+ it checks if correct errors are raised for:
+ - memory containing nans
+ - memory has wrong dimension
+
+ and checks if the returned frame is correct:
+ - has correct shape
+ - does not contain nans
+ - contains only numeric values
+ """
curr = self.mydb.scene(rowid=1)
mem = self.mydb.scene(rowid=2)
curr2 = curr.copy()
@@ -141,6 +202,18 @@ class TestCase(unittest.TestCase):
# self.assertTrue(diff.shape[3] == 1)
def test_simple_imagediff_mem(self):
+ """
+ this test checks the function imagediff works
+ correctly for the parameter memory.
+ it checks if correct errors are raised for:
+ - memory containing nans
+ - memory has wrong dimension
+
+ and checks if the returned frame is correct:
+ - has correct shape
+ - does not contain nans
+ - contains only numeric values
+ """
curr = self.mydb.scene(rowid=1)
mem = self.mydb.scene(rowid=2)
mem2 = curr.copy()
@@ -168,6 +241,18 @@ class TestCase(unittest.TestCase):
# self.assertTrue(diff.shape[3] == 1)
def test_diff_optic_flow_memory(self):
+ """
+ this test checks the function diff_optic_flow works
+ correctly for the parameter memory.
+ it checks if correct errors are raised for:
+ - memory containing nans
+ - memory has wrong dimension
+
+ and checks if the returned frame is correct:
+ - has correct shape
+ - does not contain nans
+ - contains only numeric values
+ """
curr = self.mydb.scene(rowid=1)
mem = self.mydb.scene(rowid=2)
mem2 = curr.copy()
@@ -192,6 +277,18 @@ class TestCase(unittest.TestCase):
self.assertTrue(is_numeric_array(vec))
def test_diff_optic_flow_curr(self):
+ """
+ this test checks the function diff_optic_flow works
+ correctly for the parameter current.
+ it checks if correct errors are raised for:
+ - memory containing nans
+ - memory has wrong dimension
+
+ and checks if the returned frame is correct:
+ - has correct shape
+ - does not contain nans
+ - contains only numeric values
+ """
curr = self.mydb.scene(rowid=1)
mem = self.mydb.scene(rowid=2)
curr2 = curr.copy()
diff --git a/navipy/database/__init__.py b/navipy/database/__init__.py
index 0a08881fb959bb0d8661b4624ca5fcb30e32939c..185d8016b3a874e455f182d00f8f7ff9300414b9 100644
--- a/navipy/database/__init__.py
+++ b/navipy/database/__init__.py
@@ -39,12 +39,23 @@ sqlite3.register_converter("array", convert_array)
class DataBase():
"""DataBase is the parent class of DataBaseLoad and DataBaseSave.
-It creates three sql table on initialisation.
+ It creates three sql table on initialisation.
"""
__float_tolerance = 1e-14
def __init__(self, filename, channels=['R', 'G', 'B', 'D']):
- """Initialisation of the database """
+ """Initialisation of the database
+ the first database is the image database to store the images
+ the second database is the position_orientation database to
+ store the position and orientations of the corresponding image
+ the third database is the normalisation database that stores the
+ ranges of the images.
+ :param filename: filename of the database to be loaded, stored
+ created
+ channels: channels for the images (Red,Green,Blue,Distance)
+ :type filename: string
+ channels: list
+ """
if not isinstance(filename, str):
raise TypeError('filename should be a string')
if not isinstance(channels, list):
@@ -73,9 +84,11 @@ It creates three sql table on initialisation.
self.tablecolumns['position_orientation']['x'] = 'real'
self.tablecolumns['position_orientation']['y'] = 'real'
self.tablecolumns['position_orientation']['z'] = 'real'
- self.tablecolumns['position_orientation']['alpha_0'] = 'real'
- self.tablecolumns['position_orientation']['alpha_1'] = 'real'
- self.tablecolumns['position_orientation']['alpha_2'] = 'real'
+ self.tablecolumns['position_orientation']['q_0'] = 'real'
+ self.tablecolumns['position_orientation']['q_1'] = 'real'
+ self.tablecolumns['position_orientation']['q_2'] = 'real'
+ self.tablecolumns['position_orientation']['q_3'] = 'real'
+ self.tablecolumns['position_orientation']['rotconv_id'] = 'string'
self.tablecolumns['image'] = dict()
self.tablecolumns['image']['data'] = 'array'
# self.tablecolumns['viewing_directions'] = dict()
@@ -120,6 +133,13 @@ It creates three sql table on initialisation.
self.viewing_directions[..., 1] = ma
def table_exist(self, tablename):
+ """
+ checks wether a table with name tablename exists in the database
+ :param tablename: name of the table
+ :type tablename: string
+ :returns: validity
+ :rtype: boolean
+ """
if not isinstance(tablename, str):
raise TypeError('tablename should be a string')
self.db_cursor.execute(
@@ -131,6 +151,15 @@ It creates three sql table on initialisation.
return bool(self.db_cursor.fetchone())
def check_data_validity(self, rowid):
+ """
+ checks wether all three tables in the database
+ (images,position_orientation, normalisation) contain
+ an entry with the given id.
+ :param rowid: id to be checked
+ :type rowid: int
+ :returns: validity
+ :rtype: boolean
+ """
if not isinstance(rowid, int):
raise TypeError('rowid must be an integer')
if rowid <= 0:
@@ -161,6 +190,34 @@ It creates three sql table on initialisation.
return valid
def get_posid(self, posorient):
+ """
+ returns the id of a given position and orientation
+ in the database
+ :param posorient: position and orientation
+ is a 1x6 vector containing:
+ *in case of euler angeles the index should be
+ ['location']['x']
+ ['location']['y']
+ ['location']['z']
+ [convention][alpha_0]
+ [convention][alpha_1]
+ [convention][alpha_2]
+ **where convention can be:
+ rxyz, rxzy, ryxz, ryzx, rzyx, rzxy
+ *in case of quaternions the index should be
+ ['location']['x']
+ ['location']['y']
+ ['location']['z']
+ [convention]['q_0']
+ [convention]['q_1']
+ [convention]['q_2']
+ [convention]['q_3']
+ **where convention can be:
+ quaternion
+ :type rowid: pd.Series
+ :returns: id
+ :rtype: int
+ """
if not isinstance(posorient, pd.Series):
raise TypeError('posorient should be a pandas Series')
if posorient is not None:
@@ -168,39 +225,130 @@ It creates three sql table on initialisation.
raise TypeError('posorient should be a pandas Series')
if posorient.empty:
raise Exception('position must not be empty')
- if 'x' not in posorient.index:
- raise ValueError('missing index x')
- if 'y' not in posorient.index:
- raise ValueError('missing index y')
- if 'z' not in posorient.index:
- raise ValueError('missing index z')
- if 'alpha_0' not in posorient.index:
- raise ValueError('missing index alpha_0')
- if 'alpha_1' not in posorient.index:
- raise ValueError('missing index alpha_1')
- if 'alpha_2' not in posorient.index:
- raise ValueError('missing index alpha_2')
+ found_convention = False
+ index = posorient.index
+ convention = index.get_level_values(0)[-1]
+ if convention == 'rxyz':
+ found_convention = True
+ elif convention == 'ryzx':
+ found_convention = True
+ elif convention == 'rxzy':
+ found_convention = True
+ elif convention == 'ryxz':
+ found_convention = True
+ elif convention == 'rzxy':
+ found_convention = True
+ elif convention == 'rzyx':
+ found_convention = True
+ elif convention == 'quaternion':
+ found_convention = True
+ if not found_convention:
+ raise ValueError("your convention is not supported")
+ if convention != 'quaternion':
+ if 'x' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index x')
+ if 'y' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index y')
+ if 'z' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index z')
+ if 'alpha_0' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_0')
+ if 'alpha_1' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_1')
+ if 'alpha_2' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_2')
+ elif convention == 'quaternion':
+ if 'x' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index x')
+ if 'y' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index y')
+ if 'z' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index z')
+ if 'q_0' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_0')
+ if 'q_1' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_1')
+ if 'q_2' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_2')
+ if 'q_3' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_2')
if np.any(pd.isnull(posorient)):
raise ValueError('posorient must not contain nan')
- where = """x>=? and x<=?"""
- where += """and y>=? and y<=?"""
- where += """and z>=? and z<=?"""
- where += """and alpha_0>=? and alpha_0<=?"""
- where += """and alpha_1>=? and alpha_1<=?"""
- where += """and alpha_2>=? and alpha_2<=?"""
- params = (
- posorient['x'] - self.__float_tolerance,
- posorient['x'] + self.__float_tolerance,
- posorient['y'] - self.__float_tolerance,
- posorient['y'] + self.__float_tolerance,
- posorient['z'] - self.__float_tolerance,
- posorient['z'] + self.__float_tolerance,
- posorient['alpha_0'] - self.__float_tolerance,
- posorient['alpha_0'] + self.__float_tolerance,
- posorient['alpha_1'] - self.__float_tolerance,
- posorient['alpha_1'] + self.__float_tolerance,
- posorient['alpha_2'] - self.__float_tolerance,
- posorient['alpha_2'] + self.__float_tolerance)
+ index = posorient.index
+ convention = index.get_level_values(0)[-1]
+ cursor = self.db_cursor.execute('select * from position_orientation')
+ names = list(map(lambda x: x[0], cursor.description))
+ where = ""
+ params = None
+ if 'alpha_0' in names:
+ warnings.warn("you are loading a database with an\
+ old convention")
+ where += """x>=? and x<=?"""
+ where += """and y>=? and y<=?"""
+ where += """and z>=? and z<=?"""
+ where += """and alpha_0>=? and alpha_0<=?"""
+ where += """and alpha_1>=? and alpha_1<=?"""
+ where += """and alpha_2>=? and alpha_2<=?"""
+ params = (
+ posorient['location']['x'] - self.__float_tolerance,
+ posorient['location']['x'] + self.__float_tolerance,
+ posorient['location']['y'] - self.__float_tolerance,
+ posorient['location']['y'] + self.__float_tolerance,
+ posorient['location']['z'] - self.__float_tolerance,
+ posorient['location']['z'] + self.__float_tolerance,
+ posorient[convention]['alpha_0'] - self.__float_tolerance,
+ posorient[convention]['alpha_0'] + self.__float_tolerance,
+ posorient[convention]['alpha_1'] - self.__float_tolerance,
+ posorient[convention]['alpha_1'] + self.__float_tolerance,
+ posorient[convention]['alpha_2'] - self.__float_tolerance,
+ posorient[convention]['alpha_2'] + self.__float_tolerance)
+ elif convention != 'quaternion':
+ where += """x>=? and x<=?"""
+ where += """and y>=? and y<=?"""
+ where += """and z>=? and z<=?"""
+ where += """and q_0>=? and q_0<=?"""
+ where += """and q_1>=? and q_1<=?"""
+ where += """and q_2>=? and q_2<=?"""
+ where += """and rotconv_id =?"""
+ params = (
+ posorient['location']['x'] - self.__float_tolerance,
+ posorient['location']['x'] + self.__float_tolerance,
+ posorient['location']['y'] - self.__float_tolerance,
+ posorient['location']['y'] + self.__float_tolerance,
+ posorient['location']['z'] - self.__float_tolerance,
+ posorient['location']['z'] + self.__float_tolerance,
+ posorient[convention]['alpha_0'] - self.__float_tolerance,
+ posorient[convention]['alpha_0'] + self.__float_tolerance,
+ posorient[convention]['alpha_1'] - self.__float_tolerance,
+ posorient[convention]['alpha_1'] + self.__float_tolerance,
+ posorient[convention]['alpha_2'] - self.__float_tolerance,
+ posorient[convention]['alpha_2'] + self.__float_tolerance,
+ convention)
+ else:
+ where += """x>=? and x<=?"""
+ where += """and y>=? and y<=?"""
+ where += """and z>=? and z<=?"""
+ where += """and q_0>=? and q_0<=?"""
+ where += """and q_1>=? and q_1<=?"""
+ where += """and q_2>=? and q_2<=?"""
+ where += """and q_3>=? and q_3<=?"""
+ where += """and rotconv_id =?"""
+ params = (
+ posorient['location']['x'] - self.__float_tolerance,
+ posorient['location']['x'] + self.__float_tolerance,
+ posorient['location']['y'] - self.__float_tolerance,
+ posorient['location']['y'] + self.__float_tolerance,
+ posorient['location']['z'] - self.__float_tolerance,
+ posorient['location']['z'] + self.__float_tolerance,
+ posorient[convention]['q_0'] - self.__float_tolerance,
+ posorient[convention]['q_0'] + self.__float_tolerance,
+ posorient[convention]['q_1'] - self.__float_tolerance,
+ posorient[convention]['q_1'] + self.__float_tolerance,
+ posorient[convention]['q_2'] - self.__float_tolerance,
+ posorient[convention]['q_2'] + self.__float_tolerance,
+ posorient[convention]['q_3'] - self.__float_tolerance,
+ posorient[convention]['q_3'] + self.__float_tolerance,
+ convention)
self.db_cursor.execute(
"""
SELECT count(*)
@@ -215,24 +363,43 @@ It creates three sql table on initialisation.
WHERE {};
""".format(where), params)
return self.db_cursor.fetchone()[0]
+ elif self.create & (convention != 'quaternion'):
+ self.db_cursor.execute(
+ """
+ INSERT
+ INTO position_orientation(x,y,z,q_0,q_1,q_2,q_3,rotconv_id)
+ VALUES (?,?,?,?,?,?,?,?)
+ """, (
+ posorient['location']['x'],
+ posorient['location']['y'],
+ posorient['location']['z'],
+ posorient[convention]['alpha_0'],
+ posorient[convention]['alpha_1'],
+ posorient[convention]['alpha_2'],
+ np.nan,
+ convention))
+ rowid = self.db_cursor.lastrowid
+ self.db.commit()
+ return rowid
elif self.create:
self.db_cursor.execute(
"""
INSERT
- INTO position_orientation(x,y,z,alpha_0,alpha_1,alpha_2)
- VALUES (?,?,?,?,?,?)
+ INTO position_orientation(x,y,z,q_0,q_1,q_2,rotconv_id)
+ VALUES (?,?,?,?,?,?,?,?)
""", (
- posorient['x'],
- posorient['y'],
- posorient['z'],
- posorient['alpha_0'],
- posorient['alpha_1'],
- posorient['alpha_2']))
+ posorient['location']['x'],
+ posorient['location']['y'],
+ posorient['location']['z'],
+ posorient[convention]['q_0'],
+ posorient[convention]['q_1'],
+ posorient[convention]['q_2'],
+ posorient[convention]['q_3'],
+ convention))
rowid = self.db_cursor.lastrowid
self.db.commit()
return rowid
else:
- print(posorient)
raise ValueError('posorient not found')
@property
@@ -284,17 +451,38 @@ class DataBaseLoad(DataBase):
@property
def posorients(self):
"""Return the position orientations of all points in the \
-database
+ database
+ :returns: all position orientations
+ :rtype: list of pd.Series
"""
posorient = pd.read_sql_query(
"select * from position_orientation;", self.db)
posorient.set_index('id', inplace=True)
+ if not isinstance(posorient.index, pd.core.index.MultiIndex):
+ warnings.warn("you are loading a database with old\
+ conventions, it will be transformed\
+ automatically into the new one")
+ convention = 'rxyz'
+ tuples = []
+ for n in posorient.columns:
+ if n in ['x', 'y', 'z']:
+ tuples.append(('location', n))
+ else:
+ tuples.append((convention, n))
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
+ posorient.columns = index
+ return posorient
+
return posorient
@property
def normalisations(self):
- """Return the position orientations of all points in the \
-database
+ """Returns the normalised scenes of all points in the \
+ database
+ :returns: all position orientations
+ :rtype: list of pd.Series
"""
posorient = pd.read_sql_query(
"select * from normalisation;", self.db)
@@ -307,20 +495,55 @@ database
raise TypeError('posorient should be a pandas Series')
if posorient.empty:
raise Exception('position must not be empty')
- if 'x' not in posorient.index:
- raise ValueError('missing index x')
- if 'y' not in posorient.index:
- raise ValueError('missing index y')
- if 'z' not in posorient.index:
- raise ValueError('missing index z')
- if 'alpha_0' not in posorient.index:
- raise ValueError('missing index alpha_0')
- if 'alpha_1' not in posorient.index:
- raise ValueError('missing index alpha_1')
- if 'alpha_2' not in posorient.index:
- raise ValueError('missing index alpha_2')
+ found_convention = False
+ index = posorient.index
+ convention = index.get_level_values(0)[-1]
+ if convention == 'rxyz':
+ found_convention = True
+ elif convention == 'ryzx':
+ found_convention = True
+ elif convention == 'rxzy':
+ found_convention = True
+ elif convention == 'ryxz':
+ found_convention = True
+ elif convention == 'rzxy':
+ found_convention = True
+ elif convention == 'rzyx':
+ found_convention = True
+ elif convention == 'quaternion':
+ found_convention = True
+ if not found_convention:
+ raise ValueError("your convention is not supported")
+ if convention != 'quaternion':
+ if 'x' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index x')
+ if 'y' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index y')
+ if 'z' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index z')
+ if 'alpha_0' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_0')
+ if 'alpha_1' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_1')
+ if 'alpha_2' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_2')
+ elif convention == 'quaternion':
+ if 'x' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index x')
+ if 'y' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index y')
+ if 'z' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index z')
+ if 'q_0' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_0')
+ if 'q_1' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_1')
+ if 'q_2' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_2')
+ if 'q_3' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_2')
if np.any(pd.isnull(posorient)):
- raise ValueError('posorient must not contain nan')
+ raise ValueError('posorient must not contain nan')
if rowid is not None:
if not isinstance(rowid, int):
raise TypeError('rowid must be an integer')
@@ -343,15 +566,65 @@ database
toreturn = toreturn.loc[0, :]
toreturn.name = toreturn.id
toreturn.drop('id')
- toreturn = toreturn.astype(float)
- return toreturn
+ # toreturn = toreturn.astype(float)
+ posorient = None
+ convention = toreturn.rotconv_id
+ if convention != 'quaternion':
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('rxyz', 'alpha_0'),
+ ('rxyz', 'alpha_1'), ('rxyz', 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
+ posorient = pd.Series(index=index)
+ posorient['location']['x'] = toreturn.loc['x']
+ posorient['location']['y'] = toreturn.loc['y']
+ posorient['location']['z'] = toreturn.loc['z']
+ posorient[convention]['alpha_0'] = toreturn.loc['q_0']
+ posorient[convention]['alpha_1'] = toreturn.loc['q_1']
+ posorient[convention]['alpha_2'] = toreturn.loc['q_2']
+ else:
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention, 'q_0'),
+ (convention, 'q_1'), (convention, 'q_2'),
+ (convention, 'q_3')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
+ posorient = pd.Series(index=index)
+ posorient['location']['x'] = toreturn.loc['x']
+ posorient['location']['y'] = toreturn.loc['y']
+ posorient['location']['z'] = toreturn.loc['z']
+ posorient[convention]['q_0'] = toreturn.loc['q_0']
+ posorient[convention]['q_1'] = toreturn.loc['q_1']
+ posorient[convention]['q_2'] = toreturn.loc['q_2']
+ posorient[convention]['q_3'] = toreturn.loc['q_3']
+
+ return posorient
def scene(self, posorient=None, rowid=None):
"""Read an image at a given position-orientation or given id of row in the \
database.
-
- :param posorient: a pandas Series with index \
- ['x','y','z','alpha_0','alpha_1','alpha_2']
+ :param posorient: is a 1x6 vector containing:
+ *in case of euler angeles the index should be
+ ['location']['x']
+ ['location']['y']
+ ['location']['z']
+ [convention][alpha_0]
+ [convention][alpha_1]
+ [convention][alpha_2]
+ **where convention can be:
+ rxyz, rxzy, ryxz, ryzx, rzyx, rzxy
+ *in case of quaternions the index should be
+ ['location']['x']
+ ['location']['y']
+ ['location']['z']
+ [convention]['q_0']
+ [convention]['q_1']
+ [convention]['q_2']
+ [convention]['q_3']
+ **where convention can be:
+ quaternion
:param rowid: an integer
:returns: an image
:rtype: numpy.ndarray
@@ -362,18 +635,53 @@ database
raise TypeError('posorient should be a pandas Series')
if posorient.empty:
raise Exception('position must not be empty')
- if 'x' not in posorient.index:
- raise ValueError('missing index x')
- if 'y' not in posorient.index:
- raise ValueError('missing index y')
- if 'z' not in posorient.index:
- raise ValueError('missing index z')
- if 'alpha_0' not in posorient.index:
- raise ValueError('missing index alpha_0')
- if 'alpha_1' not in posorient.index:
- raise ValueError('missing index alpha_1')
- if 'alpha_2' not in posorient.index:
- raise ValueError('missing index alpha_2')
+ found_convention = False
+ index = posorient.index
+ convention = index.get_level_values(0)[-1]
+ if convention == 'rxyz':
+ found_convention = True
+ elif convention == 'ryzx':
+ found_convention = True
+ elif convention == 'rxzy':
+ found_convention = True
+ elif convention == 'ryxz':
+ found_convention = True
+ elif convention == 'rzxy':
+ found_convention = True
+ elif convention == 'rzyx':
+ found_convention = True
+ elif convention == 'quaternion':
+ found_convention = True
+ if not found_convention:
+ raise ValueError("your convention is not supported")
+ if convention != 'quaternion':
+ if 'x' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index x')
+ if 'y' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index y')
+ if 'z' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index z')
+ if 'alpha_0' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_0')
+ if 'alpha_1' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_1')
+ if 'alpha_2' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_2')
+ elif convention == 'quaternion':
+ if 'x' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index x')
+ if 'y' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index y')
+ if 'z' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index z')
+ if 'q_0' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_0')
+ if 'q_1' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_1')
+ if 'q_2' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_2')
+ if 'q_3' not in posorient.index.get_level_values(1):
+ raise ValueError('missing index alpha_2')
if np.any(pd.isnull(posorient)):
raise ValueError('posorient must not contain nan')
if rowid is not None:
@@ -420,6 +728,14 @@ database
return toreturn
def denormalise_image(self, image, cmaxminrange):
+ """denomalises an image
+ :param image: the image to be denormalised
+ :param cmaxminrange: new range of the denormalised image
+ :type image: np.ndarray
+ :type cmaxminrange: pd.Series
+ :returns: denormalised image
+ :rtype: numpy.ndarray
+ """
if not isinstance(image, np.ndarray):
raise TypeError('image must be np.array')
if len(image.shape) != 3:
@@ -484,6 +800,33 @@ class DataBaseSave(DataBase):
return True
def write_image(self, posorient, image):
+ """stores an image in the database. Automatically
+ calculates the cminmax range from the image and
+ channels.
+ :param posorient: is a 1x6 vector containing:
+ *in case of euler angeles the index should be
+ ['location']['x']
+ ['location']['y']
+ ['location']['z']
+ [convention][alpha_0]
+ [convention][alpha_1]
+ [convention][alpha_2]
+ **where convention can be:
+ rxyz, rxzy, ryxz, ryzx, rzyx, rzxy
+ *in case of quaternions the index should be
+ ['location']['x']
+ ['location']['y']
+ ['location']['z']
+ [convention]['q_0']
+ [convention]['q_1']
+ [convention]['q_2']
+ [convention]['q_3']
+ **where convention can be:
+ quaternion
+ :param image: image to be stored
+ :type image: np.ndarray
+ :type posorient: pd.Series
+ """
normed_im, cmaxminrange = self.normalise_image(image, self.arr_dtype)
rowid = self.get_posid(posorient)
# Write image
@@ -530,6 +873,13 @@ class DataBaseSave(DataBase):
self.db.commit()
def normalise_image(self, image, dtype=np.uint8):
+ """normalises an image to a range between 0 and 1.
+ :param image: image to be normalised
+ :param dtype: type of the image (default: np.uint8)
+ :type image: np.ndarray
+ :returns: normalised image
+ :rtype: np.ndarray
+ """
if not isinstance(image, np.ndarray):
raise TypeError('image must be np.array')
if np.any(np.isnan(image)):
diff --git a/navipy/database/test.py b/navipy/database/test.py
index 44dca8b0e2168b09004afdfef5309884acf3440c..372faea96de60441c9a94152817af4c9bd1b67f4 100644
--- a/navipy/database/test.py
+++ b/navipy/database/test.py
@@ -12,10 +12,18 @@ from navipy.database import DataBaseLoad, DataBaseSave, DataBase
class TestCase(unittest.TestCase):
def setUp(self):
self.mydb_filename = pkg_resources.resource_filename(
- 'navipy', 'resources/database.db')
+ 'navipy', 'resources/database2.db')
self.mydb = database.DataBaseLoad(self.mydb_filename)
def test_DataBase_init_(self):
+ """
+ this test checks the initialization of a DataBase works
+ correctly.
+ it checks if correct errors are raised for:
+ - a database file with out .db ending is used for initialization
+ - wrong types are passed for the database name
+ i.e. integers, floats, none, nan
+ """
# filename must end with .db
with self.assertRaises(NameError):
DataBase('test')
@@ -25,11 +33,18 @@ class TestCase(unittest.TestCase):
DataBase(n)
# only works if testdb was created before e.g. with DataBaseSave
- with self.assertRaises(NameError):
- DataBase('test')
+ # with self.assertRaises(NameError):
+ # DataBase('test')
def test_DataBase_init_channel(self):
- # filename must end with .db
+ """
+ this test checks the initialization of a DataBase works
+ correctly.
+ it checks if correct errors are raised for:
+ - channels names, which are no strings or chars
+ - channel name is None value or nan
+ """
+ # channels must be strings or char
for n in [3, 8.7, None, np.nan]:
with self.assertRaises(TypeError):
DataBase(self.mydb_filename, n)
@@ -37,18 +52,19 @@ class TestCase(unittest.TestCase):
# db=DataBase(self.mydb_filename,[3,4])
with self.assertRaises(ValueError):
DataBase(self.mydb_filename, [None, 2])
- # with self.assertRaises(ValueError):
- # db=DataBase(self.mydb_filename,[4.5,'f'])
- # filename must be string
- for n in [2, 5.0, None, np.nan]:
- with self.assertRaises(TypeError):
- DataBase(n)
-
# not working, because DataBase does not create a new database
with self.assertRaises(NameError):
DataBase('test', ['R', 'B'])
def test_table_exist(self):
+ """
+ this test checks if the function table_exists works correctly
+ it checks if correct errors are raised for:
+ - a database name that are not of type string or char
+ i.e. integer, float, none, nan
+ - Attention: in this test the check for a table that existed
+ did not work correctly (False was returned)
+ """
# self.assertFalse(self.mydb.table_exist('testblubb'))
for n in [2, 7.0, None, np.nan]:
with self.assertRaises(TypeError):
@@ -56,6 +72,16 @@ class TestCase(unittest.TestCase):
# self.assertFalse(self.mydb.table_exist(self.mydb_filename))
def test_check_data_validity(self):
+ """
+ this test checks the function data validity works
+ correctly. It should return true if the database
+ contains data in the row, that is checked for
+ it checks if correct errors are raised for:
+ - row numbers that are not integers.
+ i.e. float non, nan (must be integer)
+ - row is out of range; smaller or equal to 0
+ - checks if true is returned for an exiting entry (row=1)
+ """
for n in [7.0, None, np.nan]:
with self.assertRaises(TypeError):
self.mydb.check_data_validity(n)
@@ -65,76 +91,120 @@ class TestCase(unittest.TestCase):
assert self.mydb.check_data_validity(1)
def get_posid_test(self):
+ """
+ this test checks the function get_posid works
+ correctly.
+ it checks if correct errors are raised for:
+ - posorient is missing an entry (no 'x' column)
+ - posorient contains nan or none values
+ - posorient is of wrong type (dict instead of pd.series)
+ """
conn = sqlite3.connect(self.mydb_filename)
c = conn.cursor()
c.execute(""" SELECT * FROM position_orientation WHERE (rowid=1) """)
rows = c.fetchall()[0]
- # working case
- posorient = pd.Series(index=['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient.x = rows[5]
- posorient.y = rows[6]
- posorient.z = rows[1]
- posorient.alpha_0 = rows[3]
- posorient.alpha_1 = rows[2]
- posorient.alpha_2 = rows[4]
+ # convention = rows[1]
+
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('rxyz', 'alpha_0'),
+ ('rxyz', 'alpha_1'), ('rxyz', 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position', 'orientation'])
+
+ posorient = pd.Series(index=index)
+ posorient['location']['x'] = rows[6]
+ posorient['location']['y'] = rows[7]
+ posorient['location']['z'] = rows[8]
+ posorient['rxyz']['alpha_0'] = rows[3]
+ posorient['rxyz']['alpha_1'] = rows[5]
+ posorient['rxyz']['alpha_2'] = rows[4]
+
posid = self.mydb.get_posid(posorient)
assert posid == 1
# incorrect case missing column
- posorient2 = pd.Series(index=['y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient2.y = posorient.y
- posorient2.z = posorient.z
- posorient2.alpha_0 = posorient.alpha_0
- posorient2.alpha_1 = posorient.alpha_1
- posorient2.alpha_2 = posorient.alpha_2
+ tuples = [('location', 'y'),
+ ('location', 'z'), ('rxyz', 'alpha_0'),
+ ('rxyz', 'alpha_1'), ('rxyz', 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position', 'orientation'])
+
+ posorient2 = pd.Series(index=index)
+ posorient2['location']['y'] = rows[7]
+ posorient2['location']['z'] = rows[8]
+ posorient2['rxyz']['alpha_0'] = rows[3]
+ posorient2['rxyz']['alpha_1'] = rows[5]
+ posorient2['rxyz']['alpha_2'] = rows[4]
+
with self.assertRaises(Exception):
posid = self.mydb.get_posid(posorient2)
# incorrect case None
- posorient2 = pd.Series(index=['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient2.x = None
- posorient2.y = posorient.y
- posorient2.z = posorient.z
- posorient2.alpha_0 = posorient.alpha_0
- posorient2.alpha_1 = posorient.alpha_1
- posorient2.alpha_2 = posorient.alpha_2
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('rxyz', 'alpha_0'),
+ ('rxyz', 'alpha_1'), ('rxyz', 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(self.tuples,
+ names=['position', 'orientation'])
+
+ posorient3 = pd.Series(index=index)
+ posorient3['location']['x'] = None
+ posorient3['location']['y'] = rows[7]
+ posorient3['location']['z'] = rows[8]
+ posorient3['rxyz']['alpha_0'] = rows[3]
+ posorient3['rxyz']['alpha_1'] = rows[5]
+ posorient3['rxyz']['alpha_2'] = rows[4]
with self.assertRaises(ValueError):
posid = self.mydb.get_posid(posorient2)
# incorrect case nan
- posorient2 = pd.Series(index=['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient2.x = np.nan
- posorient2.y = posorient.y
- posorient2.z = posorient.z
- posorient2.alpha_0 = posorient.alpha_0
- posorient2.alpha_1 = posorient.alpha_1
- posorient2.alpha_2 = posorient.alpha_2
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('rxyz', 'alpha_0'),
+ ('rxyz', 'alpha_1'), ('rxyz', 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(self.tuples,
+ names=['position', 'orientation'])
+
+ posorient2 = pd.Series(index=index)
+ posorient2['location']['x'] = np.nan
+ posorient2['location']['y'] = rows[7]
+ posorient2['location']['z'] = rows[8]
+ posorient2['rxyz']['alpha_0'] = rows[3]
+ posorient2['rxyz']['alpha_1'] = rows[5]
+ posorient2['rxyz']['alpha_2'] = rows[4]
with self.assertRaises(ValueError):
posid = self.mydb.get_posid(posorient2)
# incorrect case no pandas series but dict
posorient2 = {}
- posorient2['x'] = posorient.x
- posorient2['y'] = posorient.y
- posorient2['z'] = posorient.z
- posorient2['alpha_0'] = posorient.alpha_0
- posorient2['alpha_1'] = posorient.alpha_1
- posorient2['alpha_2'] = posorient.alpha_2
+ posorient2['location']['x'] = rows[6]
+ posorient2['location']['y'] = rows[7]
+ posorient2['location']['z'] = rows[8]
+ posorient2['rxyz']['alpha_0'] = rows[3]
+ posorient2['rxyz']['alpha_1'] = rows[5]
+ posorient2['rxyz']['alpha_2'] = rows[4]
with self.assertRaises(TypeError):
self.mydb.get_posid(posorient2)
# not working case empty
- posorient2 = pd.Series(index=['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('rxyz', 'alpha_0'),
+ ('rxyz', 'alpha_1'), ('rxyz', 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(self.tuples,
+ names=['position', 'orientation'])
+
+ posorient2 = pd.Series(index=index)
with self.assertRaises(Exception):
self.mydb.get_posid(posorient2)
def test_DataBaseLoad_init_(self):
+ """
+ this test checks the function DataBaseLoad works
+ correctly.
+ it checks if correct errors are raised for:
+ - filename does not end with .db
+ - filename is not a string or char
+ i.e. integer, float, none, nan
+ """
# filename must end with .db
with self.assertRaises(NameError):
DataBaseLoad('test')
@@ -149,90 +219,139 @@ class TestCase(unittest.TestCase):
DataBaseLoad('test')
def test_read_posorient(self):
+ """
+ this test checks the function read_posorient works
+ correctly.
+ it checks if correct errors are raised for:
+ - posorient is missing an entry (no 'x' column)
+ - posorient contains nan or none values
+ - posorient is of wrong type (dict instead of pd.series)
+ """
conn = sqlite3.connect(self.mydb_filename)
c = conn.cursor()
c.execute(""" SELECT * FROM position_orientation WHERE (rowid=1) """)
rows = c.fetchall()[0]
# working case
- posorient = pd.Series(index=['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient.x = rows[5]
- posorient.y = rows[6]
- posorient.z = rows[1]
- posorient.alpha_0 = rows[3]
- posorient.alpha_1 = rows[2]
- posorient.alpha_2 = rows[4]
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('rxyz', 'alpha_0'),
+ ('rxyz', 'alpha_1'), ('rxyz', 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position', 'orientation'])
+
+ posorient = pd.Series(index=index)
+ posorient['location']['x'] = rows[6]
+ posorient['location']['y'] = rows[7]
+ posorient['location']['z'] = rows[8]
+ posorient['rxyz']['alpha_0'] = rows[3]
+ posorient['rxyz']['alpha_1'] = rows[5]
+ posorient['rxyz']['alpha_2'] = rows[4]
posid = self.mydb.read_posorient(posorient=posorient)
- assert posid.x == posorient.x
+ # print(posid)
+ assert posid['location']['x'] == posorient['location']['x']
# incorrect case missing column
- posorient2 = pd.Series(index=['y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient2.y = posorient.y
- posorient2.z = posorient.z
- posorient2.alpha_0 = posorient.alpha_0
- posorient2.alpha_1 = posorient.alpha_1
- posorient2.alpha_2 = posorient.alpha_2
+ tuples = [('location', 'y'),
+ ('location', 'z'), ('rxyz', 'alpha_0'),
+ ('rxyz', 'alpha_1'), ('rxyz', 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position', 'orientation'])
+
+ posorient2 = pd.Series(index=index)
+ posorient2['location']['y'] = rows[7]
+ posorient2['location']['z'] = rows[8]
+ posorient2['rxyz']['alpha_0'] = rows[3]
+ posorient2['rxyz']['alpha_1'] = rows[5]
+ posorient2['rxyz']['alpha_2'] = rows[4]
with self.assertRaises(ValueError):
self.mydb.read_posorient(posorient=posorient2)
# incorrect case None
- posorient2 = pd.Series(index=['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient2.x = None
- posorient2.y = posorient.y
- posorient2.z = posorient.z
- posorient2.alpha_0 = posorient.alpha_0
- posorient2.alpha_1 = posorient.alpha_1
- posorient2.alpha_2 = posorient.alpha_2
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('rxyz', 'alpha_0'),
+ ('rxyz', 'alpha_1'), ('rxyz', 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position', 'orientation'])
+
+ posorient2 = pd.Series(index=index)
+ posorient2['location']['x'] = None
+ posorient2['location']['y'] = rows[7]
+ posorient2['location']['z'] = rows[8]
+ posorient2['rxyz']['alpha_0'] = rows[3]
+ posorient2['rxyz']['alpha_1'] = rows[5]
+ posorient2['rxyz']['alpha_2'] = rows[4]
with self.assertRaises(ValueError):
self.mydb.read_posorient(posorient=posorient2)
# incorrect case nan
- posorient2 = pd.Series(index=['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient2.x = np.nan
- posorient2.y = posorient.y
- posorient2.z = posorient.z
- posorient2.alpha_0 = posorient.alpha_0
- posorient2.alpha_1 = posorient.alpha_1
- posorient2.alpha_2 = posorient.alpha_2
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('rxyz', 'alpha_0'),
+ ('rxyz', 'alpha_1'), ('rxyz', 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position', 'orientation'])
+
+ posorient2 = pd.Series(index=index)
+ posorient2['location']['x'] = np.nan
+ posorient2['location']['y'] = rows[7]
+ posorient2['location']['z'] = rows[8]
+ posorient2['rxyz']['alpha_0'] = rows[3]
+ posorient2['rxyz']['alpha_1'] = rows[5]
+ posorient2['rxyz']['alpha_2'] = rows[4]
with self.assertRaises(ValueError):
self.mydb.read_posorient(posorient=posorient2)
# incorrect case no pandas series but dict
posorient2 = {}
- posorient2['x'] = posorient.x
- posorient2['y'] = posorient.y
- posorient2['z'] = posorient.z
- posorient2['alpha_0'] = posorient.alpha_0
- posorient2['alpha_1'] = posorient.alpha_1
- posorient2['alpha_2'] = posorient.alpha_2
+ posorient2['location'] = {}
+ posorient2['rxyz'] = {}
+ posorient2['location']['x'] = rows[6]
+ posorient2['location']['y'] = rows[7]
+ posorient2['location']['z'] = rows[8]
+ posorient2['rxyz']['alpha_0'] = rows[3]
+ posorient2['rxyz']['alpha_1'] = rows[5]
+ posorient2['rxyz']['alpha_2'] = rows[4]
with self.assertRaises(TypeError):
self.mydb.read_posorient(posorient=posorient2)
# not working case empty
- posorient2 = pd.Series(index=['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('rxyz', 'alpha_0'),
+ ('rxyz', 'alpha_1'), ('rxyz', 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position', 'orientation'])
+
+ posorient2 = pd.Series(index=index)
with self.assertRaises(Exception):
self.mydb.read_posorient(posorient=posorient2)
def test_read_posid_id(self):
+ """
+ this test checks the function read_posorient works
+ correctly.
+ it checks if correct errors are raised for:
+ - rowid is out of range (<=0)
+ - rowid is of type char, none, nan, float
+ and checks if the returned entry for rowid 1 is correct
+ - that it all columns and correct values
+ """
conn = sqlite3.connect(self.mydb_filename)
c = conn.cursor()
c.execute(""" SELECT * FROM position_orientation WHERE (rowid=1) """)
rows = c.fetchall()[0]
# working case
- posorient = pd.Series(index=['id', 'x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient.id = rows[0]
- posorient.x = rows[5]
- posorient.y = rows[6]
- posorient.z = rows[1]
- posorient.alpha_0 = rows[3]
- posorient.alpha_1 = rows[2]
- posorient.alpha_2 = rows[4]
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('rxyz', 'alpha_0'),
+ ('rxyz', 'alpha_1'), ('rxyz', 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position', 'orientation'])
+
+ posorient = pd.Series(index=index)
+ posorient['location']['x'] = rows[6]
+ posorient['location']['y'] = rows[7]
+ posorient['location']['z'] = rows[8]
+ posorient['rxyz']['alpha_0'] = rows[3]
+ posorient['rxyz']['alpha_1'] = rows[5]
+ posorient['rxyz']['alpha_2'] = rows[4]
for rowid in [0, -2]:
with self.assertRaises(ValueError):
# print("rowid",rowid)
@@ -248,15 +367,29 @@ class TestCase(unittest.TestCase):
for rowid in [1]:
posoriend2 = self.mydb.read_posorient(rowid=rowid)
- assert posoriend2.id == posorient.id
- assert posoriend2.x == posorient.x
- assert posoriend2.y == posorient.y
- assert posoriend2.z == posorient.z
- assert posoriend2.alpha_0 == posorient.alpha_0
- assert posoriend2.alpha_1 == posorient.alpha_1
- assert posoriend2.alpha_2 == posorient.alpha_2
+ assert posoriend2['location']['x'] == posorient['location']['x']
+ assert posoriend2['location']['y'] == posorient['location']['y']
+ assert posoriend2['location']['z'] == posorient['location']['z']
+ assert (posoriend2['rxyz']['alpha_0'] ==
+ posorient['rxyz']['alpha_0'])
+ assert (posoriend2['rxyz']['alpha_1'] ==
+ posorient['rxyz']['alpha_1'])
+ assert (posoriend2['rxyz']['alpha_2'] ==
+ posorient['rxyz']['alpha_2'])
def test_scene_id(self):
+ """
+ this test checks the function scene works
+ correctly.
+ it checks if correct errors are raised for:
+ - rowid is out of range (<=0)
+ - rowid is of type char, none, nan, float
+
+ and checks if the returned entry for different
+ rows is correct
+ - has correct shape
+ - does not contain nans
+ """
for rowid in [0, -2]:
with self.assertRaises(ValueError):
# print("rowid",rowid)
@@ -283,19 +416,31 @@ class TestCase(unittest.TestCase):
self.assertTrue(image.shape[1] > 0)
def test_scene_posorient(self):
+ """
+ this test checks the function scene works
+ correctly.
+ it checks if correct errors are raised for:
+ - posorient is missing an entry (no 'x' column)
+ - posorient contains nan or none values
+ - posorient is of wrong type (dict instead of pd.series)
+ """
conn = sqlite3.connect(self.mydb_filename)
c = conn.cursor()
c.execute(""" SELECT * FROM position_orientation WHERE (rowid=1) """)
rows = c.fetchall()[0]
# working case
- posorient = pd.Series(index=['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient.x = rows[5]
- posorient.y = rows[6]
- posorient.z = rows[1]
- posorient.alpha_0 = rows[3]
- posorient.alpha_1 = rows[2]
- posorient.alpha_2 = rows[4]
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('rxyz', 'alpha_0'),
+ ('rxyz', 'alpha_1'), ('rxyz', 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position', 'orientation'])
+ posorient = pd.Series(index=index)
+ posorient['location']['x'] = rows[6]
+ posorient['location']['y'] = rows[7]
+ posorient['location']['z'] = rows[8]
+ posorient['rxyz']['alpha_0'] = rows[3]
+ posorient['rxyz']['alpha_1'] = rows[5]
+ posorient['rxyz']['alpha_2'] = rows[4]
image = self.mydb.scene(posorient=posorient)
self.assertIsNotNone(image)
self.assertFalse(sum(image.shape) == 0)
@@ -304,59 +449,91 @@ class TestCase(unittest.TestCase):
self.assertTrue(image.shape[3] == 1)
# incorrect case missing column
- posorient2 = pd.Series(index=['y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient2.y = posorient.y
- posorient2.z = posorient.z
- posorient2.alpha_0 = posorient.alpha_0
- posorient2.alpha_1 = posorient.alpha_1
- posorient2.alpha_2 = posorient.alpha_2
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('rxyz', 'alpha_0'),
+ ('rxyz', 'alpha_1'), ('rxyz', 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position', 'orientation'])
+
+ posorient2 = pd.Series(index=index)
+ posorient2['location']['y'] = rows[7]
+ posorient2['location']['z'] = rows[8]
+ posorient2['rxyz']['alpha_0'] = rows[3]
+ posorient2['rxyz']['alpha_1'] = rows[5]
+ posorient2['rxyz']['alpha_2'] = rows[4]
with self.assertRaises(Exception):
image = self.mydb.scene(posorient=posorient2)
# incorrect case None
- posorient2 = pd.Series(index=['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient2.x = None
- posorient2.y = posorient.y
- posorient2.z = posorient.z
- posorient2.alpha_0 = posorient.alpha_0
- posorient2.alpha_1 = posorient.alpha_1
- posorient2.alpha_2 = posorient.alpha_2
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('rxyz', 'alpha_0'),
+ ('rxyz', 'alpha_1'), ('rxyz', 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position', 'orientation'])
+
+ posorient2 = pd.Series(index=index)
+ posorient2['location']['x'] = None
+ posorient2['location']['y'] = rows[7]
+ posorient2['location']['z'] = rows[8]
+ posorient2['rxyz']['alpha_0'] = rows[3]
+ posorient2['rxyz']['alpha_1'] = rows[5]
+ posorient2['rxyz']['alpha_2'] = rows[4]
with self.assertRaises(ValueError):
image = self.mydb.scene(posorient=posorient2)
# incorrect case nan
- posorient2 = pd.Series(index=['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient2.x = np.nan
- posorient2.y = posorient.y
- posorient2.z = posorient.z
- posorient2.alpha_0 = posorient.alpha_0
- posorient2.alpha_1 = posorient.alpha_1
- posorient2.alpha_2 = posorient.alpha_2
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('rxyz', 'alpha_0'),
+ ('rxyz', 'alpha_1'), ('rxyz', 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position', 'orientation'])
+
+ posorient2 = pd.Series(index=index)
+ posorient2['location']['x'] = np.nan
+ posorient2['location']['y'] = rows[7]
+ posorient2['location']['z'] = rows[8]
+ posorient2['rxyz']['alpha_0'] = rows[3]
+ posorient2['rxyz']['alpha_1'] = rows[5]
+ posorient2['rxyz']['alpha_2'] = rows[4]
with self.assertRaises(ValueError):
image = self.mydb.scene(posorient=posorient2)
# incorrect case no pandas series but dict
posorient2 = {}
- posorient2['x'] = posorient.x
- posorient2['y'] = posorient.y
- posorient2['z'] = posorient.z
- posorient2['alpha_0'] = posorient.alpha_0
- posorient2['alpha_1'] = posorient.alpha_1
- posorient2['alpha_2'] = posorient.alpha_2
+ posorient2['location'] = {}
+ posorient2['rxyz'] = {}
+ posorient2['location']['x'] = rows[6]
+ posorient2['location']['y'] = rows[7]
+ posorient2['location']['z'] = rows[8]
+ posorient2['rxyz']['alpha_0'] = rows[3]
+ posorient2['rxyz']['alpha_1'] = rows[5]
+ posorient2['rxyz']['alpha_2'] = rows[4]
with self.assertRaises(TypeError):
image = self.mydb.scene(posorient=posorient2)
# not working case empty
- posorient2 = pd.Series(index=['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position', 'orientation'])
+
+ posorient2 = pd.Series(index=index)
with self.assertRaises(Exception):
image = self.mydb.scene(posorient=posorient2)
def test_denormalise_image(self):
+ """
+ this test checks the function denormalise_image works
+ correctly.
+ it checks if correct errors are raised for:
+ - image has wrong type (list instead of np.ndarray)
+ - image does not have enough dimensions
+ - image contains nan values
+ - image has to many dimensions
+ - cminmax range is missing one channel
+ - cminmax range is empty pd.series
+ - cminmax range is dictionary
+ - cminmax contains nans
+ """
image = self.mydb.scene(rowid=1)
image = np.squeeze(image)
cminmaxrange = pd.Series(index=['R_min', 'R_max', 'R_range',
@@ -458,6 +635,15 @@ class TestCase(unittest.TestCase):
self.mydb.denormalise_image(imagecorrect, cminmaxrange)
def test_DataBaseSave(self):
+ """
+ this test checks the function DataBaseSaver works
+ correctly.
+ it checks if correct errors are raised for:
+ - the filename is of type integer
+
+ checks for correct result if a new DataBaseSaver
+ is created (no error is thrown)
+ """
# should work and creat new database
with tempfile.TemporaryDirectory() as folder:
testdb_filename = folder + '/testdatabase.db'
@@ -467,6 +653,14 @@ class TestCase(unittest.TestCase):
database.DataBaseSave(filename=3)
def test_normalise_image(self):
+ """
+ this test checks the function normalise_image works
+ correctly.
+ it checks if correct errors are raised for:
+ - image is of wrong type (list)
+ - image has wrong dimensionality (too big, too small)
+ - image contains nan values
+ """
image = self.mydb.scene(rowid=1)
image = np.squeeze(image)
with tempfile.TemporaryDirectory() as folder:
@@ -498,6 +692,13 @@ class TestCase(unittest.TestCase):
loadDB.normalise_image(image2)
def test_insert_replace(self):
+ """
+ this test checks the function insert_replace works
+ correctly.
+ it checks if correct errors are raised for:
+ - filename is of type integer, float, nan or none
+ - filename does not exist in database/params are wrong
+ """
params = {}
params['hight'] = 1.7
params['age'] = 20
@@ -509,9 +710,6 @@ class TestCase(unittest.TestCase):
with self.assertRaises(TypeError):
tmpmydb.insert_replace(name, params)
- for name in [3, 7.5, np.nan, None]:
- with self.assertRaises(TypeError):
- tmpmydb.insert_replace(name, params)
with self.assertRaises(sqlite3.OperationalError):
tmpmydb.insert_replace('test', params)
diff --git a/navipy/maths/euler.py b/navipy/maths/euler.py
index 0b9ca987c9b6ff527a4d89eef2b8a4f5fba33db5..4ffb875a000d30b6ecf7d480c80909c58059de6d 100644
--- a/navipy/maths/euler.py
+++ b/navipy/maths/euler.py
@@ -108,3 +108,44 @@ def from_quaternion(quaternion, axes='sxyz'):
"""Return Euler angles from quaternion for specified axis sequence.
"""
return from_matrix(quat.matrix(quaternion), axes)
+
+
+def angle_rate_matrix(ai, aj, ak, axes='sxyz'):
+ """
+ Return the Euler Angle Rates Matrix
+
+ from Diebels Representing Attitude: Euler Angles,
+ Unit Quaternions, and Rotation, 2006
+ """
+ try:
+ firstaxis, parity, repetition, frame = constants._AXES2TUPLE[axes]
+ except (AttributeError, KeyError):
+ constants._TUPLE2AXES[axes] # validation
+ firstaxis, parity, repetition, frame = axes
+ i = firstaxis
+ j = constants._NEXT_AXIS[i + parity]
+ k = constants._NEXT_AXIS[i - parity + 1]
+ veci = np.zeros(3)
+ veci[i] = 1
+ vecj = np.zeros(3)
+ vecj[j] = 1
+ veck = np.zeros(3)
+ veck[k] = 1
+ if frame == 0: # static
+ roti = matrix(ai, 0, 0, axes=axes)
+ rotj = matrix(0, aj, 0, axes=axes)
+ mat = np.hstack([veci, roti.dot(vecj), roti.dot(rotj).dot(veck)])
+ else:
+ rotk = matrix(0, 0, ak, axes=axes).transpose()
+ rotj = matrix(0, aj, 0, axes=axes).transpose()
+ mat = np.hstack([rotk.dot(rotj.dot(veci)), rotk.dot(vecj), veck])
+ return mat
+
+
+def angular_velocity(ai, aj, ak, dai, daj, dak, axes='sxyz'):
+ """
+ Return the angular velocity
+ """
+ return angle_rate_matrix(ai, aj, ak, axes=axes).dot(np.vstack([dai,
+ daj,
+ dak]))
diff --git a/navipy/maths/homogeneous_transformations.py b/navipy/maths/homogeneous_transformations.py
index 759ad993dda3e7a8de89186529eaee81543811a6..0b93b9b32272aca6beee781b3b5a4f7128edd2cc 100644
--- a/navipy/maths/homogeneous_transformations.py
+++ b/navipy/maths/homogeneous_transformations.py
@@ -3,6 +3,7 @@
import numpy as np
import navipy.maths.constants as constants
import navipy.maths.euler as euler
+import navipy.maths.quaternion as quaternion
from navipy.maths.tools import vector_norm
from navipy.maths.tools import unit_vector
@@ -396,7 +397,10 @@ def compose_matrix(scale=None, shear=None, angles=None, translate=None,
T[:3, 3] = translate[:3]
M = np.dot(M, T)
if angles is not None:
- R = euler.matrix(angles[0], angles[1], angles[2], axes)
+ if axes == 'quaternion':
+ R = quaternion.matrix(angles)
+ else:
+ R = euler.matrix(angles[0], angles[1], angles[2], axes)
M = np.dot(M, R)
if shear is not None:
Z = np.identity(4)
diff --git a/navipy/maths/test_euler.py b/navipy/maths/test_euler.py
index 61d4641f95d42949ac67e934cd1a07305a711f12..b4f5c5da6ae33cfcef69c73791880f62b7f25cc4 100644
--- a/navipy/maths/test_euler.py
+++ b/navipy/maths/test_euler.py
@@ -17,6 +17,22 @@ class TestEuler(unittest.TestCase):
print('axes', key, 'failed')
self.assertTrue(np.all(np.array(condition.values())))
+ def test_betweenconvention(self):
+ """
+ Test orientation from one convention to another
+ """
+ condition = dict()
+ refkey = 'rxyz'
+ for key, _ in constants._AXES2TUPLE.items():
+ rotation_0 = euler.matrix(1, 2, 3, refkey)
+ ai, aj, ak = euler.from_matrix(rotation_0, key)
+ rotation_1 = euler.matrix(ai, aj, ak, key)
+ condition[key] = np.allclose(rotation_0,
+ rotation_1)
+ if condition[key] is False:
+ print('axes', key, 'failed')
+ self.assertTrue(np.all(np.array(condition.values())))
+
def test_from_quaternion(self):
angles = euler.from_quaternion([0.99810947, 0.06146124, 0, 0])
self.assertTrue(np.allclose(angles, [0.123, 0, 0]))
diff --git a/navipy/moving/agent.py b/navipy/moving/agent.py
index 9036c1e68afc3546b1d37d4ea4eb108258e49ea2..7b30f1721f9bc6a4ebddcbb43949704884ccb7e8 100644
--- a/navipy/moving/agent.py
+++ b/navipy/moving/agent.py
@@ -49,14 +49,28 @@ class DefaultBrain():
class AbstractAgent():
- def __init__(self):
+ def __init__(self, convention='rxyz'):
self._brain = DefaultBrain()
self._alter_posorientvel = defaultcallback
- self._posorient_col = ['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2']
- self._velocity_col = ['d' + col for col in self._posorient_col]
- self._posorient_vel_col = self._posorient_col.copy()
- self._posorient_vel_col.extend(self._velocity_col)
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention, 'alpha_0'),
+ (convention, 'alpha_1'), (convention, 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
+ self._posorient_col = index
+
+ tuples_vel = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention, 'alpha_0'),
+ (convention, 'alpha_1'), (convention, 'alpha_2'),
+ ('location', 'dx'), ('location', 'dy'),
+ ('location', 'dz'), (convention, 'dalpha_0'),
+ (convention, 'dalpha_1'), (convention, 'dalpha_2')]
+ index_vel = pd.MultiIndex.from_tuples(tuples_vel,
+ names=['position',
+ 'orientation'])
+ self._velocity_col = index_vel
+ self._posorient_vel_col = self._velocity_col.copy()
self._posorient_vel = pd.Series(
index=self._posorient_vel_col,
data=np.nan)
@@ -135,8 +149,10 @@ CyberBeeAgent is a close loop agent and need to be run within blender \
"""
- def __init__(self, brain):
- AbstractAgent.__init__(self)
+ def __init__(self, brain, convention):
+ if convention is None:
+ raise Exception("a convention must be specified")
+ AbstractAgent.__init__(self, convention)
AbstractAgent._alter_posorientvel = \
lambda motion_vec: navimomath.next_pos(motion_vec,
move_mode='free_run')
@@ -160,12 +176,14 @@ GridAgent is a close loop agent here its position is snap to a grid.
"""
- def __init__(self, brain,
+ def __init__(self, brain, convention,
posorients_queue=None,
results_queue=None):
+ if convention is None:
+ raise Exception("convention must be set")
if (posorients_queue is not None) and (results_queue is not None):
multiprocessing.Process.__init__(self)
- AbstractAgent.__init__(self)
+ AbstractAgent.__init__(self, convention)
self._alter_posorientvel = self.snap_to_grid
self.brain = brain
self._posorients_queue = posorients_queue
@@ -207,8 +225,9 @@ GridAgent is a close loop agent here its position is snap to a grid.
posorient_vel,
move_mode=self._mode_move['mode'],
move_param=self._mode_move['param'])
+ tmppos = self._brain.posorients
tmp = navimomath.closest_pos(
- posorient_vel, self._brain.posorients)
+ posorient_vel, tmppos) # self._brain.posorients)
posorient_vel.loc[self._posorient_col] = \
tmp.loc[self._posorient_col]
posorient_vel.name = tmp.name
@@ -247,7 +266,6 @@ GridAgent is a close loop agent here its position is snap to a grid.
self._posorient_vel.loc[common_id] = start_posorient.loc[common_id]
self.move()
next_posorient = self._posorient_vel
-
self._posorients_queue.task_done()
self._results_queue.put((start_posorient, next_posorient))
self._posorients_queue.task_done()
diff --git a/navipy/moving/maths.py b/navipy/moving/maths.py
index 630225562d11688a7b17814237fcd3e77a4940fd..55389c762ea1d37fa746d49f06d5f03437e77f9e 100644
--- a/navipy/moving/maths.py
+++ b/navipy/moving/maths.py
@@ -35,44 +35,68 @@ def next_pos(motion_vec, move_mode, move_param=None):
if move_mode not in mode_moves_supported().keys():
raise KeyError(
'move mode must is not supported {}'.format(move_mode))
-
- speed = motion_vec.loc[['dx', 'dy', 'dz']]
+ tuples = [('location', 'dx'), ('location', 'dy'),
+ ('location', 'dz')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
+ speed = pd.Series(index=index)
+ speed['location']['dx'] = motion_vec['location']['dx']
+ speed['location']['dy'] = motion_vec['location']['dy']
+ speed['location']['dz'] = motion_vec['location']['dz']
+ # speed = motion_vec.loc[['dx', 'dy', 'dz']]
if move_mode == 'on_cubic_grid':
# speed in spherical coord
- epsilon = np.arctan2(speed.dz, np.sqrt(speed.dx**2 + speed.dy**2))
- phi = np.arctan2(speed.dy, speed.dx)
+ epsilon = np.arctan2(speed['location']['dz'],
+ np.sqrt(speed['location']['dx']**2 +
+ speed['location']['dy']**2))
+ phi = np.arctan2(speed['location']['dy'], speed['location']['dx'])
radius = np.sqrt(np.sum(speed**2))
if np.isclose(radius, 0):
- scaling = 0
+ # scaling = 0
speed = 0 * speed
else:
- deltas = pd.Series(index=['dx', 'dy', 'dz'])
- deltas.dz = epsilon > (np.pi / 8) - epsilon < (np.pi / 8)
+ tuples = [('location', 'dx'), ('location', 'dy'),
+ ('location', 'dz')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
+ deltas = pd.Series(index=index)
+ deltas['location']['dz'] = float(epsilon > (np.pi / 8) -
+ epsilon < (np.pi / 8))
edgecases = np.linspace(-np.pi, np.pi, 9)
case_i = np.argmin(np.abs(phi - edgecases))
if case_i == 8 or case_i == 0 or case_i == 1 or case_i == 7:
- deltas.dx = -1
+ deltas['location']['dx'] = -1
elif case_i == 3 or case_i == 4 or case_i == 5:
- deltas.dx = 1
+ deltas['location']['dx'] = 1
else:
- deltas.dx = 0
+ deltas['location']['dx'] = 0
if case_i == 1 or case_i == 2 or case_i == 3:
- deltas.dy = -1
+ deltas['location']['dy'] = -1
elif case_i == 5 or case_i == 6 or case_i == 7:
- deltas.dy = 1
+ deltas['location']['dy'] = 1
+
else:
- deltas.dy = 0
- scaling = 1
+ deltas['location']['dy'] = 0
+ # scaling = 1
speed = move_param['grid_spacing'] * deltas
elif move_mode is 'free_run':
- scaling = 1 # <=> dt = 1, user need to scale speed in dt units
+ pass
+ # scaling = 1 # <=> dt = 1, user need to scale speed in dt units
else:
raise ValueError('grid_mode is not supported')
toreturn = motion_vec
- toreturn.loc[['x', 'y', 'z']] += speed.rename({'dx': 'x',
- 'dy': 'y',
- 'dz': 'z'}) * scaling
+ toreturn['location']['x'] = (toreturn['location']['x'] +
+ speed['location']['dx'])
+ toreturn['location']['y'] = (toreturn['location']['y'] +
+ speed['location']['dy'])
+ toreturn['location']['z'] = (toreturn['location']['z'] +
+ speed['location']['dz'])
+ # toreturn.loc[['x', 'y', 'z']] += speed.rename({'dx': 'x',
+ # 'dy': 'y',
+ # 'dz': 'z'}) * scaling
return toreturn
@@ -81,13 +105,15 @@ def closest_pos(pos, positions):
:param pos: the position to find (a pandas Series with ['x','y','z']
:param positions: the possible closest positions
- (a pandas dataframe with ['x','y','z'])
+ (a pandas dataframe with
+ [['location','x'],['location','y'],['location','z']])
"""
+
euclidian_dist = np.sqrt(
- (pos.x - positions.x)**2
- + (pos.y - positions.y)**2
- + (pos.z - positions.z)**2)
- return positions.loc[euclidian_dist.idxmin(), :]
+ (pos['location']['x'] - positions['location']['x'])**2
+ + (pos['location']['y'] - positions['location']['y'])**2
+ + (pos['location']['z'] - positions['location']['z'])**2)
+ return positions.loc[euclidian_dist.idxmin()]
def closest_pos_memory_friendly(pos, database):
diff --git a/navipy/moving/test_agent.py b/navipy/moving/test_agent.py
index 67a33ea1c909502f71ae935d7a4adf8a55fb7976..3319808148c5b7baca40fbf7fae78345c6810a05 100644
--- a/navipy/moving/test_agent.py
+++ b/navipy/moving/test_agent.py
@@ -8,6 +8,7 @@ import navipy.moving.agent as naviagent
import navipy.database as navidb
from navipy import Brain
import pkg_resources
+import warnings
import unittest
@@ -17,11 +18,26 @@ version = float(nx.__version__)
class BrainTest(Brain):
def __init__(self, renderer=None):
Brain.__init__(self, renderer=renderer)
- self.__posorient_col = ['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2']
- self.__velocity_col = ['d' + col for col in self.__posorient_col]
- self.__posorient_vel_col = self.__posorient_col
- self.__posorient_vel_col.extend(self.__velocity_col)
+ convention = 'rxyz'
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention, 'alpha_0'),
+ (convention, 'alpha_1'), (convention, 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
+ self.__posorient_col = index
+ tuples_vel = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention, 'alpha_0'),
+ (convention, 'alpha_1'), (convention, 'alpha_2'),
+ ('location', 'dx'), ('location', 'dy'),
+ ('location', 'dz'), (convention, 'dalpha_0'),
+ (convention, 'dalpha_1'), (convention, 'dalpha_2')]
+ index_vel = pd.MultiIndex.from_tuples(tuples_vel,
+ names=['position',
+ 'orientation'])
+ self.__velocity_col = index_vel
+ self.__posorient_vel_col = self.__velocity_col.copy()
+ # self.__posorient_vel_col.extend(self.__velocity_col)
def velocity(self):
return pd.Series(data=0, index=self.__posorient_vel_col)
@@ -33,11 +49,26 @@ class TestNavipyMovingAgent(unittest.TestCase):
'navipy', 'resources/database.db')
self.mydb = navidb.DataBaseLoad(self.mydb_filename)
self.brain = BrainTest(self.mydb)
- self.__posorient_col = ['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2']
- self.__velocity_col = ['d' + col for col in self.__posorient_col]
+ convention = 'rxyz'
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention, 'alpha_0'),
+ (convention, 'alpha_1'), (convention, 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
+ self.__posorient_col = index
+ tuples_vel = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention, 'alpha_0'),
+ (convention, 'alpha_1'), (convention, 'alpha_2'),
+ ('location', 'dx'), ('location', 'dy'),
+ ('location', 'dz'), (convention, 'dalpha_0'),
+ (convention, 'dalpha_1'), (convention, 'dalpha_2')]
+ index_vel = pd.MultiIndex.from_tuples(tuples_vel,
+ names=['position',
+ 'orientation'])
+ self.__velocity_col = index_vel
self.__posorient_vel_col = self.__posorient_col
- self.__posorient_vel_col.extend(self.__velocity_col)
+ # self.__posorient_vel_col.extend(self.__velocity_col)
#
# AbstractAgent
#
@@ -56,37 +87,52 @@ class TestNavipyMovingAgent(unittest.TestCase):
# GridAgent
#
def test_move_gridagent(self):
- agent = naviagent.GridAgent(self.brain)
- initposorient = self.brain.posorients.loc[13, :]
+ agent = naviagent.GridAgent(self.brain, 'rxyz')
+ initposorient = None
+ with warnings.catch_warnings(record=True):
+ initposorient = self.brain.posorients.loc[13, :]
initposovel = pd.Series(data=0,
index=self.__posorient_vel_col)
initposovel.loc[initposorient.index] = initposorient
agent.posorient = initposovel
with self.assertRaises(AttributeError):
agent.move()
+ tuples = [('location', 'dx'), ('location', 'dy'),
+ ('location', 'dz')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
mode_move = {'mode': 'on_cubic_grid',
'param': {'grid_spacing':
pd.Series(data=1,
- index=['dx', 'dy', 'dz'])}}
+ index=index)}}
agent.mode_of_motion = mode_move
- agent.move()
+ with warnings.catch_warnings(record=True):
+ agent.move()
obtained = agent.posorient
self.assertTrue(np.allclose(
obtained, initposorient.loc[obtained.index]))
def test_fly_gridagent(self):
- agent = naviagent.GridAgent(self.brain)
- initposorient = self.brain.posorients.loc[13, :]
+ agent = naviagent.GridAgent(self.brain, 'rxyz')
+ initposorient = None
+ with warnings.catch_warnings(record=True):
+ initposorient = self.brain.posorients.loc[13, :]
initposovel = pd.Series(data=0,
index=self.__posorient_vel_col)
initposovel.loc[initposorient.index] = initposorient
agent.posorient = initposovel
with self.assertRaises(AttributeError):
agent.fly(max_nstep=10)
+ tuples = [('location', 'dx'), ('location', 'dy'),
+ ('location', 'dz')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
mode_move = {'mode': 'on_cubic_grid',
'param': {'grid_spacing':
pd.Series(data=1,
- index=['dx', 'dy', 'dz'])}}
+ index=index)}}
agent.mode_of_motion = mode_move
agent.fly(max_nstep=10)
obtained = agent.posorient
@@ -98,7 +144,9 @@ class TestNavipyMovingAgent(unittest.TestCase):
#
def test_init_graphagent(self):
- agent = naviagent.GraphAgent(self.brain)
+ agent = None
+ with warnings.catch_warnings(record=True):
+ agent = naviagent.GraphAgent(self.brain)
if version < 2:
graph_nodes = list(agent.graph.nodes())
else:
@@ -108,7 +156,9 @@ class TestNavipyMovingAgent(unittest.TestCase):
'Init of graph failed. Node missmatch')
def test_graph_setter(self):
- agent = naviagent.GraphAgent(self.brain)
+ agent = None
+ with warnings.catch_warnings(record=True):
+ agent = naviagent.GraphAgent(self.brain)
if version < 2:
graph_nodes = list(agent.graph.nodes())
else:
@@ -136,7 +186,9 @@ class TestNavipyMovingAgent(unittest.TestCase):
3 Two loops attractors
"""
# Test all node to first
- agent = naviagent.GraphAgent(self.brain)
+ agent = None
+ with warnings.catch_warnings(record=True):
+ agent = naviagent.GraphAgent(self.brain)
if version < 2:
graph_nodes = list(agent.graph.nodes())
@@ -201,7 +253,9 @@ class TestNavipyMovingAgent(unittest.TestCase):
2. Saddle points
3. Local minima
"""
- agent = naviagent.GraphAgent(self.brain)
+ agent = None
+ with warnings.catch_warnings(record=True):
+ agent = naviagent.GraphAgent(self.brain)
# Local maxima
if version < 2:
graph_nodes = list(agent.graph.nodes())
diff --git a/navipy/moving/test_maths.py b/navipy/moving/test_maths.py
index 0917850568097bd3d22ee8d154a20dfc188cd7ac..75fbc8eef6917163133f4e0045fc4fd44a689f96 100644
--- a/navipy/moving/test_maths.py
+++ b/navipy/moving/test_maths.py
@@ -26,8 +26,14 @@ class TestNavipyMovingMaths(unittest.TestCase):
def test_notsupported_mofm(self):
""" Test that a TypeError is correctly raised
"""
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('location', 'dx'),
+ ('location', 'dy'), ('location', 'dz')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
motion_vec = pd.Series(data=0,
- index=['x', 'y', 'z', 'dx', 'dy', 'dz'])
+ index=index)
move_mode = 'NotSupportedMode'
mode_param = dict()
mode_param['grid_spacing'] = 1
@@ -44,8 +50,14 @@ class TestNavipyMovingMaths(unittest.TestCase):
test this by series equality.
"""
# Test if stay at same position.
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('location', 'dx'),
+ ('location', 'dy'), ('location', 'dz')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
motion_vec = pd.Series(data=0,
- index=['x', 'y', 'z', 'dx', 'dy', 'dz'])
+ index=index)
move_mode = 'on_cubic_grid'
mode_param = dict()
mode_param['grid_spacing'] = 1
@@ -61,14 +73,30 @@ class TestNavipyMovingMaths(unittest.TestCase):
22.5 degrees modulo 45 degrees. We therefore test the functions
close to each instability.
"""
- positions = pd.DataFrame({'x': [0, 0, 0, 1, 1, 1, 2, 2, 2],
- 'y': [0, 1, 2, 0, 1, 2, 0, 1, 2],
- 'z': [0, 0, 0, 0, 0, 0, 0, 0, 0]},
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
+ positions = pd.DataFrame(data=[[0, 0, 0],
+ [0, 1, 0],
+ [0, 2, 0],
+ [1, 0, 0],
+ [1, 1, 0],
+ [1, 2, 0],
+ [2, 0, 0],
+ [2, 1, 0],
+ [2, 2, 0]],
+ columns=index,
dtype=np.float)
move_mode = 'on_cubic_grid'
move_param = dict()
+ tuples = [('location', 'dx'), ('location', 'dy'),
+ ('location', 'dz')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position', 'orientation'])
move_param['grid_spacing'] = pd.Series(data=1,
- index=['dx', 'dy', 'dz'])
+ index=index)
expected_dict = dict()
expected_dict[-22] = 7 # [2,1]
expected_dict[22] = 7
@@ -87,14 +115,18 @@ class TestNavipyMovingMaths(unittest.TestCase):
expected_dict[293] = 6 # [2, 0]
expected_dict[337] = 6
expected_dict[338] = 7 # equivalent to -22
+ tuples2 = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('location', 'dx'),
+ ('location', 'dy'), ('location', 'dz')]
+ index2 = pd.MultiIndex.from_tuples(tuples2,
+ names=['position', 'orientation'])
for angle, exp_i in expected_dict.items():
alpha = np.deg2rad(angle)
motion_vec = pd.Series(
data=[1, 1, 0,
np.cos(alpha), np.sin(alpha), 0],
- index=['x', 'y', 'z',
- 'dx', 'dy', 'dz'],
+ index=index2,
dtype=np.float)
newpos = navimaths.next_pos(motion_vec,
move_mode,
diff --git a/navipy/processing/mcode.py b/navipy/processing/mcode.py
index 7eb14619b1754d4ea70ffe5c934e16c034252329..f162474fb93027014972d6ea0f25447e4183a64e 100644
--- a/navipy/processing/mcode.py
+++ b/navipy/processing/mcode.py
@@ -1,15 +1,873 @@
"""
Motion code
"""
-from .tools import check_scene
+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
+import numpy as np
+import pandas as pd
-def optic_flow(scene, viewing_directions,
- velocity, distance_channel=3):
- """ optic flow """
+def spherical_to_Vector(sp):
+ """ translate 2D azimuth and elevation vector to 3D vector
+ :param sp: np array to be translated from spherical to euclidean
+ coordinates
+ :returns vector
+ :rtype np.ndarray """
+ if sp is None:
+ raise ValueError("sp must not be None")
+ if not isinstance(sp, np.ndarray):
+ raise TypeError("vector must be list or array")
+ if len(sp) != 2:
+ raise ValueError("vector must contain two elements")
+ if (not isinstance(sp[0], float)) and (not isinstance(sp[0], int)):
+ 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.array([np.dot(np.cos(sp[1]), np.cos(sp[0])),
+ np.dot(np.cos(sp[1]), np.sin(sp[0])),
+ np.sin(sp[1])])
+
+ return vector
+
+
+def OFSubroutineSpToVector(sphericCoord):
+ """
+OFSUBROUTINESPTOVECTOR recieves a pair of spheric coordinates and
+converts them into a 3dim vector.
+
+ :param sphericCoord - a Sx2 matrix with spheric coordinates
+
+ :return vector a Sx3 matrix with the transformed coordinates
+ :rtype np.ndarray
+
+NOTE: this is NOT the normal spheic coordinate system!
+ spheric coordinates given as azimuth (Longitude) and
+ zenith (Latitude) given in geographic coordinate system) in radian
+ [pi/2 0]= [0 1 0] left, [3*pi/2 0]= [0 -1 0] right,
+ [0 pi/2]= [0 0 1] top, [0 -pi/2]= [0 0 -1] bottom
+
+
+ #################################################################
+ Copyright (C) 2009-2014 J.P. Lindemann, C. Strub
+
+ This file is part of the ivtools.
+ https://opensource.cit-ec.de/projects/ivtools
+
+ the Optic-Flow toolbox is free software: you can redistribute it
+ and/or modify it under the terms of the GNU General Public License
+ as published by the Free Software Foundation, either version 3 of
+ the License, or (at your option) any later version.
+
+ the Optic-Flow toolbox is distributed in the hope that it will be
+ useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Foobar. If not, see <http://www.gnu.org/licenses/>.
+ ##################################################################
+"""
+ sp = sphericCoord
+ if sp is None:
+ raise ValueError("sp must not be None")
+ if not isinstance(sp, list):
+ raise TypeError("vector must be list or array")
+ if not isinstance(sp, np.ndarray) and not isinstance(sp, list):
+ raise TypeError("vector must be list or array")
+ if len(sp) != 2:
+ raise ValueError("vector must contain two elements")
+ if (not isinstance(sp[0], float)) and (not isinstance(sp[0], int)):
+ 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])]
+
+ return vector
+
+
+def OFSubroutineRotate(vec, rotAx, alpha):
+ """
+ OFSUBROUTINEROTATE rotates the given vector through the rotAxis as
+ UNIT-vector
+
+ :param vec - Vx3 matrix with vectors to rotate
+ :param rotAx - the axis to rotate around
+ :param alpha - the angle to rotate in radian
+
+ :return rotVec - the rotated vectors in the input matrix
+ :rtype np.ndarray
+
+
+
+ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ % Copyright (C) 2009-2014 J.P. Lindemann, C. Strub
+ %
+ % This file is part of the ivtools.
+ % https://opensource.cit-ec.de/projects/ivtools
+ %
+ % the Optic-Flow toolbox is free software: you can redistribute it
+ % and/or modify it under the terms of the GNU General Public License
+ % as published by the Free Software Foundation, either version 3 of
+ % the License, or (at your option) any later version.
+ %
+ % the Optic-Flow toolbox is distributed in the hope that it will be
+ % useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ % of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ % GNU General Public License for more details.
+ %
+ % You should have received a copy of the GNU General Public License
+ % along with Foobar. If not, see <http://www.gnu.org/licenses/>.
+ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ """
+ if vec is None:
+ raise ValueError("vector must not be None")
+ if rotAx is None:
+ raise ValueError("rotation axis must not be None")
+ if not isinstance(vec, list) and (not isinstance(vec, np.ndarray)):
+ raise TypeError("vector must be list or array")
+ if not isinstance(rotAx, np.ndarray) and (not isinstance(rotAx, list)):
+ raise TypeError("rotation Axis must be list or array")
+ if len(vec) != 3:
+ raise ValueError("vector must contain two elements")
+ if (not isinstance(rotAx[0], float)) and (not isinstance(rotAx[0], int)):
+ raise ValueError("rotation Axis must be float or integer")
+ if (not isinstance(rotAx[1], float)) and (not isinstance(rotAx[1], int)):
+ raise ValueError("rotation Axis must be float or integer")
+ if (not isinstance(rotAx[2], float)) and (not isinstance(rotAx[2], int)):
+ raise ValueError("rotation Axis must be float or integer")
+ if (not isinstance(alpha, float)) and (not isinstance(alpha, int)):
+ raise ValueError("rotation vector must be float or integer")
+ if (not isinstance(vec, np.ndarray)) and (not isinstance(vec, list)):
+ raise TypeError("vector must be of type np.ndarray or list")
+ if isinstance(vec, np.ndarray):
+ if vec.shape[0] != 3:
+ raise Exception("second dimension of vector must have size three")
+ if not is_numeric_array(vec):
+ raise TypeError("vec must be of numerical type")
+ # scaling the axis to unit vector
+ rotAx = rotAx / np.linalg.norm(rotAx)
+
+ # this is an rotation matrix:
+ Rot = [[np.cos(alpha) + np.power(rotAx[0], 2) * (1 - np.cos(alpha)),
+ np.dot(rotAx[0], rotAx[1]) *
+ (1 - np.cos(alpha)) - rotAx[2] * np.sin(alpha),
+ np.dot(rotAx[0], rotAx[2]) *
+ (1-np.cos(alpha)) + rotAx[1] * np.sin(alpha)],
+ [np.dot(rotAx[1], rotAx[0]) *
+ (1-np.cos(alpha)) + rotAx[2] * np.sin(alpha),
+ np.cos(alpha)+np.power(rotAx[1], 2) *
+ (1-np.cos(alpha)), np.dot(rotAx[1],
+ rotAx[2]) * (1-np.cos(alpha))-rotAx[0] * np.sin(alpha)],
+ [np.dot(rotAx[2], rotAx[0]) *
+ (1-np.cos(alpha))-rotAx[1] * np.sin(alpha),
+ np.dot(rotAx[2], rotAx[1]) *
+ (1-np.cos(alpha)) + rotAx[0] * np.sin(alpha),
+ np.cos(alpha) + np.power(rotAx[2], 2) * (1-np.cos(alpha))]]
+
+ rotVec = np.transpose(np.dot(Rot, np.transpose(vec)))
+ return rotVec
+
+
+def OFSubroutineYawPitchRoll(vec, ypr, inverse):
+ """
+ OFSUBROUTINEYAWPITCHROLL applies yaw pitch and roll to a vector
+ inverse = true to inverse the YawPitchRoll
+
+ :param vec - a 3dim vector to rotate
+ :param ypr - [yaw pitch roll]
+ :param inverse - boolean to dertermine whether the inverse rotations or
+ the normal rotation should be computed
+
+ :returns rotated vector
+ :rtype np.ndarray
+
+
+
+ #############################################################################
+ Copyright (C) 2009-2014 J.P. Lindemann, C. Strub
+
+ This file is part of the ivtools.
+ https://opensource.cit-ec.de/projects/ivtools
+
+ the Optic-Flow toolbox is free software: you can redistribute it
+ and/or modify it under the terms of the GNU General Public License
+ as published by the Free Software Foundation, either version 3 of
+ the License, or (at your option) any later version.
+
+ the Optic-Flow toolbox is distributed in the hope that it will
+ be useful,but WITHOUT ANY WARRANTY; without even the implied
+ warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ See the GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Foobar. If not, see <http://www.gnu.org/licenses/>.
+ """
+ if vec is None:
+ raise ValueError("vector must not be None")
+ if ypr is None:
+ raise ValueError("angles (ypr) must not be None")
+ if (not isinstance(vec, np.ndarray)) and (not isinstance(vec, list)):
+ raise TypeError("vector must be of type np.ndarray")
+ if isinstance(vec, list):
+ if len(vec) != 3:
+ raise Exception("second dimension of vector must have size three")
+ else:
+ if vec.shape[0] != 3:
+ raise Exception("second dimension of vector must have size three")
+ if not is_numeric_array(vec):
+ raise TypeError("vec must be of numerical type")
+ if (not isinstance(ypr, np.ndarray)) and (not isinstance(ypr, list)):
+ raise TypeError("angle vector must be of type np.ndarray or list")
+ if isinstance(ypr, list):
+ if len(ypr) != 3:
+ raise Exception("first dimension of angle vector must have\
+ size three")
+ else:
+ if ypr.shape[0] != 3:
+ raise Exception("first dimension of angle vector\
+ must have size three")
+ if not is_numeric_array(ypr):
+ raise TypeError("angle vector must be of numerical type")
+ if not isinstance(inverse, bool):
+ raise TypeError("inverse must be of type bool")
+
+ # rotations around abselute fix-axis
+ # if (inverse==true)
+ # vec=OFSubroutineRotate(vec,[1 0 0],-ypr(3));
+ # vec=OFSubroutineRotate(vec,[0 1 0],-ypr(2));
+ # vec=OFSubroutineRotate(vec,[0 0 1],-ypr(1));
+ # else
+ # vec=OFSubroutineRotate(vec,[0 0 1],ypr(1));
+ # vec=OFSubroutineRotate(vec,[0 1 0],ypr(2));
+ # vec=OFSubroutineRotate(vec,[1 0 0],ypr(3));
+ # end
+
+ # rotations around relative rotatet axis
+
+ if (inverse):
+ 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])))
+ 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]
+ vec = np.transpose(np.dot(M3, np.transpose(vec)))
+ else:
+ 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])))
+ M2 = rotation_matrix(ypr[1], roty)[:3, :3]
+ vec = np.transpose(np.dot(M2, np.transpose(vec)))
+ rotx = np.transpose(np.dot(M1, np.transpose([1, 0, 0])))
+ M4 = rotation_matrix(ypr[1], [0, 1, 0])[:3, :3]
+ rotatedax = np.transpose(np.dot(M4, np.transpose(rotx)))
+
+ M3 = rotation_matrix(ypr[2], rotatedax)[:3, :3]
+ vec = np.transpose(np.dot(M3, np.transpose(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
+ :param viewing_directions: viewing direction of each pixel
+ (azimuth,elevation)
+ :param velocity: pandas series
+ (x,y,z,alpha,beta,gamma,dx,dy,dz,dalpha,dbeta,dgamma)
+ :distance channel: distance"""
check_scene(scene)
if distance_channel not in range(4):
raise ValueError('distance channel out of range')
- distance = scene[..., distance_channel, 0]
- distance += distance
- raise NameError('Not Implemented')
+ if not isinstance(velocity, pd.Series):
+ raise TypeError('velocity should be a pandas Series')
+ if velocity is None:
+ raise ValueError("velocity must not be None")
+ if velocity.empty:
+ raise Exception('velocity must not be empty')
+ if not isinstance(velocity.index, pd.core.index.MultiIndex):
+ raise Exception('velocity must have a multiindex containing \
+ the convention used')
+ index = velocity.index
+ convention = index.get_level_values(0)[-1]
+ if convention != 'rxyz':
+ raise ValueError("to calculate the optic flow,\
+ angles must be in rxyz convention")
+ 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):
+ raise ValueError('missing index y')
+ if 'z' not in velocity.index.get_level_values(1):
+ raise ValueError('missing index z')
+ if 'alpha_0' not in velocity.index.get_level_values(1):
+ raise ValueError('missing index alpha_0')
+ if 'alpha_1' not in velocity.index.get_level_values(1):
+ raise ValueError('missing index alpha_1')
+ if 'alpha_2' not in velocity.index.get_level_values(1):
+ raise ValueError('missing index alpha_2')
+ if np.any(pd.isnull(velocity)):
+ raise ValueError('velocity must not contain nan')
+ if viewing_directions is None:
+ raise ValueError("viewing direction must not be None")
+ if (not isinstance(viewing_directions, list)) and\
+ (not isinstance(viewing_directions, np.ndarray)):
+ raise TypeError("angels must be list or np.ndarray")
+ if not is_numeric_array(viewing_directions):
+ raise TypeError("viewing_direction must be of numerical type")
+ if viewing_directions.shape[1] != 2:
+ raise Exception("second dimension of viewing\
+ direction must be of size two")
+ distance = np.squeeze(scene[:, :, distance_channel, 0])
+ # distance += distance
+ elevation = viewing_directions[..., __spherical_indeces__['elevation']]
+ azimuth = viewing_directions[..., __spherical_indeces__['azimuth']]
+ yaw = velocity[convention]['alpha_0']
+ pitch = velocity[convention]['alpha_1']
+ roll = velocity[convention]['alpha_2']
+ dyaw = velocity[convention]['dalpha_0']
+ dpitch = velocity[convention]['dalpha_1']
+ droll = velocity[convention]['dalpha_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')
+
+ u = [velocity['location']['dx'],
+ velocity['location']['dy'],
+ velocity['location']['dz']]
+ v = np.linalg.norm(u)
+ if(v == 0):
+ u = [0, 0, 0]
+ else:
+ u = u/np.linalg.norm(u)
+
+ yawNow = OFSubroutineYawPitchRoll([1, 0, 0], [yaw, 0, 0], False)
+ yawNext = OFSubroutineYawPitchRoll([1, 0, 0], [yaw+dyaw, 0, 0], False)
+ rYaw = np.cross(yawNow, yawNext)
+
+ # now rotation around y-axis
+ pitchNow = OFSubroutineYawPitchRoll([1, 0, 0], [0, pitch, 0], False)
+ pitchNext = OFSubroutineYawPitchRoll([1, 0, 0],
+ [0, pitch+dpitch, 0], False)
+ rPitch = np.cross(pitchNow, pitchNext)
+
+ # and roatation around x-axis
+ 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]))
+
+ for i, a in enumerate(azimuth):
+ for j, e in enumerate(elevation):
+ spline = OFSubroutineSpToVector([a, e])
+ spline = OFSubroutineYawPitchRoll(spline,
+ [yaw, pitch, roll],
+ False)
+ p = distance[j, i]
+ if(p == 0):
+ # if object touches the enviroment, OpticFlow dosnt need to be
+ # scaled -> distance=1
+ p = 1
+
+ # the Translation-part of the Optic Flow:
+ dotvu = np.dot(u, v)
+ splinetrans = np.transpose(spline)
+ opticFlowT = -(dotvu-(np.dot(dotvu, splinetrans))*spline)/p
+ # check if there actualy is a rotation and if compute
+ # surface-normal and scale it with angle between vectors
+ # negative because flow relative to observer
+ if np.linalg.norm(rYaw) <= 0.000001:
+ opticFlowRyaw = 0
+ else:
+ normrYaw = np.linalg.norm(rYaw)
+ rYawN = np.dot(rYaw/normrYaw, np.arcsin(normrYaw))
+ opticFlowRyaw = -np.cross(rYawN, spline)
+
+ if np.linalg.norm(rPitch) <= 0.000001:
+ opticFlowRpitch = 0
+ else:
+ normrPitch = np.linalg.norm(rPitch)
+ rPitchN = np.dot(rPitch/normrPitch, np.arcsin(normrPitch))
+ opticFlowRpitch = -np.cross(rPitchN, spline)
+ if np.linalg.norm(rRoll) <= 0.000001:
+ opticFlowRroll = 0
+ else:
+ normrRoll = np.linalg.norm(rRoll)
+ rRollN = rRoll/normrRoll*np.arcsin(normrRoll)
+ opticFlowRroll = -np.cross(rRollN, spline)
+
+ # combine the rotations
+ opticFlowR = opticFlowRyaw+opticFlowRpitch+opticFlowRroll
+
+ # and add Translation and Rotation to get the Optic Flow
+ opticFlow = opticFlowT+opticFlowR
+
+ # 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])
+
+ rof[j, i] = OF_rho
+ hof[j, i] = OF_phi
+ vof[j, i] = OF_epsilon
+
+ return rof, hof, vof
+
+
+class Module():
+ """
+ This class represents a Module that functions as a storage
+ """
+ def __init__(self, size=(180, 360)):
+ """
+ initializes the storage as an np.ndarray containing zeros
+ of size size
+ :param size: the tupel containing the size of the
+ storage (Input)
+ """
+ if size is None:
+ raise ValueError("size must not be None")
+ if not isinstance(size, tuple):
+ raise TypeError("size must be a tuple")
+ if len(size) < 2:
+ raise Exception("length of size must at least be two")
+ self.size = size
+ self.Input = np.zeros(size)
+
+ @property
+ def size(self):
+ """
+ getter for the the size field
+ :returns size: size of the Input field
+ :rtype tuple
+ """
+ return self.__size
+
+ @size.setter
+ def size(self, size):
+ """
+ setter for the size of the storage
+ :param size: tuple that contains the size of the storage
+ """
+ if size is None:
+ raise ValueError("size must not be None")
+ if not isinstance(size, tuple):
+ raise TypeError("size must be a tuple")
+ if len(size) < 2:
+ raise Exception("length of size must at least be two")
+ self.__size = size
+
+ @property
+ def Input(self):
+ """
+ getter for the Input field
+ :returns Input
+ :rtype np.ndarray
+ """
+ return self.__Input
+
+ @Input.setter
+ def Input(self, Input):
+ """
+ setter for the Input field, automaticaly sets the
+ the size field to the shape of the Input
+ :param Input
+ """
+ if Input is None:
+ raise ValueError("Input must not be None")
+ if not isinstance(Input, np.ndarray):
+ raise TypeError("Input must be np array")
+ if len(Input.shape) < 2:
+ raise Exception("Input must have at least 2 dimensions")
+ if (Input.shape[0] < 1) and (Input.shape[1] < 1):
+ raise Exception("Each dimension of the Input\
+ must have at least be of size one")
+ self.__Input = Input
+ self.size = Input.shape
+
+ def update(self,):
+ """"
+ update function can be implemented for
+ inheriting classes
+ """
+ pass
+
+
+class lp(Module):
+ """
+ Implementation of a low pass filter
+ """
+ def __init__(self, tau, inM):
+ """
+ Initializes the lowpass filter, the size of the output is
+ set to the size of the input signal (inM)
+ :param tau: time constant of the filter
+ :param freq: cut off frequence of the filter
+ :param inM: Module that stores and represents the input signal
+ """
+ if not isinstance(tau, float) and isinstance(tau, int):
+ raise ValueError("tau must be of type float or integer")
+ if inM is None:
+ raise ValueError("input Module must not be None")
+ if not isinstance(inM, Module):
+ raise ValueError("input Module must be of type Module")
+ self.size = inM.size
+ Module.__init__(self, self.size)
+ self.inM = inM
+ self.itau = tau
+
+ @property
+ def itau(self):
+ """
+ getter of the time constant which is
+ calculated by 1000/(tau*freq)
+ """
+ return self.__itau
+
+ @itau.setter
+ def itau(self, itau):
+ """
+ setter of the time constant
+ :param itau: time constant
+ """
+ if not isinstance(itau, float) and isinstance(itau, int):
+ raise ValueError("itau must be of type float or integer")
+ self.__itau = itau
+
+ @property
+ def inM(self):
+ """
+ setter of the input Module
+ :returns inM
+ :rtype Module
+ """
+ return self.__inM
+
+ @inM.setter
+ def inM(self, inM):
+ """
+ setter of the input Module
+ :param inM
+ """
+ if inM is None:
+ raise ValueError("input Module must not be None")
+ if not isinstance(inM, Module):
+ raise ValueError("input Module must be of type Module")
+ self.__inM = inM
+
+ def update(self,):
+ """
+ update functions, updates the filtered signal for the
+ the current input signal. out_t+1 +=tau*(input-out_t)
+ """
+ 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])
+
+
+class hp(Module): # for second order just take hp for inM
+ """
+ Implements a high pass filter
+ """
+ def __init__(self, tau, inM):
+ """
+ Initializes the high pass filter
+ :param tau: time constant
+ :param freq: cut off frequency
+ :param inM: Module that stores the input signal
+ """
+ if not isinstance(tau, float) and isinstance(tau, int):
+ raise ValueError("tau must be of type float or integer")
+ if inM is None:
+ raise ValueError("input Module must not be None")
+ if not isinstance(inM, Module):
+ raise ValueError("input Module must be of type Module")
+ self.size = inM.size
+ Module.__init__(self, self.size)
+ self.inM = inM
+ self.lowpass = lp(tau, inM)
+
+ @property
+ def inM(self):
+ """
+ getter for the input Module
+ :returns inM
+ :rtype Module
+ """
+ return self.__inM
+
+ @inM.setter
+ def inM(self, inM):
+ """
+ setter for the input Module
+ :param inM: input Module for the input signal
+ """
+ if inM is None:
+ raise ValueError("input Module must not be None")
+ if not isinstance(inM, Module):
+ raise ValueError("input Module must be of type Module")
+ self.__inM = inM
+
+ def update(self,):
+ """
+ updates the output signal with the current input signal
+ out_t+1=Input-lowpass(Input)
+ """
+ self.inM.update()
+ self.lowpass.update()
+ lpOut = self.lowpass.Input
+ 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])
+
+
+class mul(Module):
+ """
+ Implements the multiplication of two Modules
+ """
+ def __init__(self, inM1, inM2):
+ """
+ Initializes the multiplication module
+ :param inM1: first input Module
+ :param inM2: second input Module
+ """
+ if inM1 is None:
+ raise ValueError("input Module must not be None")
+ if not isinstance(inM1, Module):
+ raise ValueError("input Module must be of type Module")
+ if inM2 is None:
+ raise ValueError("input Module must not be None")
+ if not isinstance(inM2, Module):
+ raise ValueError("input Module must be of type Module")
+ Module.__init__(self, inM1.size)
+ self.inM1 = inM1
+ self.inM2 = inM2
+
+ @property
+ def inM1(self):
+ """
+ getter for the first input Module
+ :returns inM1
+ :rtype Module
+ """
+ return self.__inM1
+
+ @inM1.setter
+ def inM(self, inM1):
+ """
+ setter for the first input Module
+ :param inM1
+ """
+ if inM1 is None:
+ raise ValueError("input Module must not be None")
+ if not isinstance(inM1, Module):
+ raise ValueError("input Module must be of type Module")
+ self.__inM1 = inM1
+
+ @property
+ def inM2(self):
+ """
+ getter for the second input Module
+ :returns inM2
+ :rtype Module
+ """
+ return self.__inM2
+
+ @inM2.setter
+ def inM2(self, inM2):
+ """
+ setter for the first input Module
+ :param inM1
+ """
+ if inM2 is None:
+ raise ValueError("input Module must not be None")
+ if not isinstance(inM2, Module):
+ raise ValueError("input Module must be of type Module")
+ self.__inM2 = inM2
+
+ def update(self, shift, axis=None):
+ """
+ updates the output (multiplication of the two input Modules)
+ for the current input (see numpy roll)
+ :param shift: shifts the Input provided by the first module
+ by the given amount
+ :param axis: shifts the Input of the first module along the
+ provided axis
+ """
+ if not isinstance(shift, int):
+ raise TypeError("shift must be an integer")
+ if axis is not None:
+ if not isinstance(axis, int):
+ raise TypeError("axis must be of type integer")
+ shiftedInput = np.roll(self.inM1.Input, shift, axis)
+ for i in range(self.size[0]):
+ 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
+
+
+class div(Module):
+ """
+ Implements the division of two Modules
+ """
+ def __init__(self, inM1, inM2):
+ """
+ Initializes the multiplication module
+ :param inM1: first input Module
+ :param inM2: second input Module
+ """
+ if inM1 is None:
+ raise ValueError("input Module must not be None")
+ if not isinstance(inM1, Module):
+ raise ValueError("input Module must be of type Module")
+ if inM2 is None:
+ raise ValueError("input Module must not be None")
+ if not isinstance(inM2, Module):
+ raise ValueError("input Module must be of type Module")
+ Module.__init__(self, inM1.size)
+ self.inM1 = inM1
+ self.inM2 = inM2
+
+ @property
+ def inM1(self):
+ """
+ getter for the first input Module
+ :returns inM1
+ :rtype Module
+ """
+ return self.__inM1
+
+ @inM1.setter
+ def inM(self, inM1):
+ """
+ setter for the first input Module
+ :param inM1
+ """
+ if inM1 is None:
+ raise ValueError("input Module must not be None")
+ if not isinstance(inM1, Module):
+ raise ValueError("input Module must be of type Module")
+ self.__inM1 = inM1
+
+ @property
+ def inM2(self):
+ """
+ getter for the second input Module
+ :returns inM2
+ :rtype Module
+ """
+ return self.__inM2
+
+ @inM2.setter
+ def inM2(self, inM2):
+ """
+ setter for the first input Module
+ :param inM1
+ """
+ if inM2 is None:
+ raise ValueError("input Module must not be None")
+ if not isinstance(inM2, Module):
+ raise ValueError("input Module must be of type Module")
+ self.__inM2 = inM2
+
+ def update(self, shift, axis=None):
+ """
+ updates the output (division of the two input Modules)
+ for the current input (see numpy roll)
+ :param shift: shifts the Input provided by the first module
+ by the given amount
+ :param axis: shifts the Input of the first module along the
+ provided axis
+ """
+ if not isinstance(shift, int):
+ raise TypeError("shift must be an integer")
+ if axis is not None:
+ if not isinstance(axis, int):
+ raise TypeError("axis must be of type integer")
+ shiftedInput = np.roll(self.inM1.Input, shift, axis)
+ for i in range(self.size[0]):
+ for j in range(self.size[1]):
+ sig_left1 = self.inM1.Input[i, j]
+ sig_left2 = shiftedInput[i, j]
+ if sig_left2 != 0:
+ self.Input[i, j] = sig_left1/sig_left2
+ else:
+ self.Input[i, j] = 0
diff --git a/navipy/processing/pcode.py b/navipy/processing/pcode.py
index 8ed0ead6aae61fcd727dd7a30bf0fd211f42bd55..f542c173ff8fb2a0e84323ce722a7f8dd00210d9 100644
--- a/navipy/processing/pcode.py
+++ b/navipy/processing/pcode.py
@@ -133,6 +133,16 @@ def pcv(place_code, viewing_directions):
should be 1'.format(place_code.shape[component_dim]))
elevation = viewing_directions[..., __spherical_indeces__['elevation']]
azimuth = viewing_directions[..., __spherical_indeces__['azimuth']]
+ if (np.any(elevation < -np.pi/2) or np.any(elevation > np.pi/2)):
+ # if (np.any(elevation < -2*np.pi) or np.any(elevation > 2*np.pi)):
+ raise ValueError(" Elevation must be radians in range [-2*pi;2*pi]")
+ if (np.max(elevation) - np.min(elevation) > 2 * np.pi):
+ raise ValueError(" max difference in elevation must be < 2*pi")
+ if (np.any(azimuth < -np.pi) or np.any(azimuth > np.pi)):
+ # if (np.any(azimuth < -2*np.pi) or np.any(azimuth > np.pi*2)):
+ raise ValueError(" Azimuth must be radians in range [-2*pi;2*pi]")
+ if (np.max(azimuth) - np.min(azimuth) > 2 * np.pi):
+ raise ValueError(" max difference in azimuth must be < 2*pi")
x, y, z = spherical_to_cartesian(elevation, azimuth, radius=1)
unscaled_lv = np.zeros((viewing_directions.shape[0],
viewing_directions.shape[1],
diff --git a/navipy/processing/test.py b/navipy/processing/test.py
index ff13c23084fe5212967bd6bf97cfe752e5bc212a..5b8218a5d556ff9909b5e0973b3422d7c905494a 100644
--- a/navipy/processing/test.py
+++ b/navipy/processing/test.py
@@ -15,19 +15,38 @@ class TestCase(unittest.TestCase):
self.mydb = database.DataBaseLoad(self.mydb_filename)
def test_scene_posorient(self):
+ """
+ this test checks that the correct errors are raised if
+ wrong values for the input parameters are passed to the
+ function scene of the navipy.database module
+ it also contains some test where correct parameter values
+ were passed to the scene function and the output was
+ checked for correctness.
+ test cases:
+ missing entries in the posorient pd.series
+ None, NaN values in the posorient pd.series
+ posorient is of wrong type (dictionary instead of pd.series)
+ empty posorient pd.series
+ """
conn = sqlite3.connect(self.mydb_filename)
c = conn.cursor()
c.execute(""" SELECT * FROM position_orientation WHERE (rowid=1) """)
rows = c.fetchall()[0]
# working case
- posorient = pd.Series(index=['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient.x = rows[5]
- posorient.y = rows[6]
- posorient.z = rows[1]
- posorient.alpha_0 = rows[3]
- posorient.alpha_1 = rows[2]
- posorient.alpha_2 = rows[4]
+ convention = 'rxyz'
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention, 'alpha_0'),
+ (convention, 'alpha_1'), (convention, 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
+ posorient = pd.Series(index=index)
+ posorient['location']['x'] = rows[5]
+ posorient['location']['y'] = rows[6]
+ posorient['location']['z'] = rows[1]
+ posorient[convention]['alpha_0'] = rows[3]
+ posorient[convention]['alpha_1'] = rows[2]
+ posorient[convention]['alpha_2'] = rows[4]
image = self.mydb.scene(posorient=posorient)
self.assertIsNotNone(image)
self.assertFalse(sum(image.shape) == 0)
@@ -36,59 +55,80 @@ class TestCase(unittest.TestCase):
self.assertTrue(image.shape[3] == 1)
# incorrect case missing column
- posorient2 = pd.Series(index=['y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient2.y = posorient.y
- posorient2.z = posorient.z
- posorient2.alpha_0 = posorient.alpha_0
- posorient2.alpha_1 = posorient.alpha_1
- posorient2.alpha_2 = posorient.alpha_2
+ tuples2 = [('location', 'y'),
+ ('location', 'z'), (convention, 'alpha_0'),
+ (convention, 'alpha_1'), (convention, 'alpha_2')]
+ index2 = pd.MultiIndex.from_tuples(tuples2,
+ names=['position',
+ 'orientation'])
+ posorient2 = pd.Series(index=index2)
+ posorient2['location']['y'] = rows[6]
+ posorient2['location']['z'] = rows[1]
+ posorient2[convention]['alpha_0'] = rows[3]
+ posorient2[convention]['alpha_1'] = rows[2]
+ posorient2[convention]['alpha_2'] = rows[4]
with self.assertRaises(Exception):
image = self.mydb.scene(posorient=posorient2)
# incorrect case None
- posorient2 = pd.Series(index=['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient2.x = None
- posorient2.y = posorient.y
- posorient2.z = posorient.z
- posorient2.alpha_0 = posorient.alpha_0
- posorient2.alpha_1 = posorient.alpha_1
- posorient2.alpha_2 = posorient.alpha_2
+ posorient2 = pd.Series(index=index)
+ posorient2['location']['x'] = None
+ posorient2['location']['y'] = rows[6]
+ posorient2['location']['z'] = rows[1]
+ posorient2[convention]['alpha_0'] = rows[3]
+ posorient2[convention]['alpha_1'] = rows[2]
+ posorient2[convention]['alpha_2'] = rows[4]
with self.assertRaises(ValueError):
image = self.mydb.scene(posorient=posorient2)
# incorrect case nan
- posorient2 = pd.Series(index=['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
- posorient2.x = np.nan
- posorient2.y = posorient.y
- posorient2.z = posorient.z
- posorient2.alpha_0 = posorient.alpha_0
- posorient2.alpha_1 = posorient.alpha_1
- posorient2.alpha_2 = posorient.alpha_2
+ posorient2 = pd.Series(index=index)
+ posorient2['location']['x'] = np.nan
+ posorient2['location']['y'] = rows[6]
+ posorient2['location']['z'] = rows[1]
+ posorient2[convention]['alpha_0'] = rows[3]
+ posorient2[convention]['alpha_1'] = rows[2]
+ posorient2[convention]['alpha_2'] = rows[4]
with self.assertRaises(ValueError):
image = self.mydb.scene(posorient=posorient2)
# incorrect case no pandas series but dict
posorient2 = {}
- posorient2['x'] = posorient.x
- posorient2['y'] = posorient.y
- posorient2['z'] = posorient.z
- posorient2['alpha_0'] = posorient.alpha_0
- posorient2['alpha_1'] = posorient.alpha_1
- posorient2['alpha_2'] = posorient.alpha_2
+ posorient2['location'] = {}
+ posorient2[convention] = {}
+ posorient2['location']['x'] = rows[5]
+ posorient2['location']['y'] = rows[6]
+ posorient2['location']['z'] = rows[1]
+ posorient2[convention]['alpha_0'] = rows[3]
+ posorient2[convention]['alpha_1'] = rows[2]
+ posorient2[convention]['alpha_2'] = rows[4]
with self.assertRaises(TypeError):
image = self.mydb.scene(posorient=posorient2)
# not working case empty
- posorient2 = pd.Series(index=['x', 'y', 'z',
- 'alpha_0', 'alpha_1', 'alpha_2'])
-
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention, 'alpha_0'),
+ (convention, 'alpha_1'), (convention, 'alpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
+ posorient2 = pd.Series(index=index)
with self.assertRaises(Exception):
image = self.mydb.scene(posorient=posorient2)
def test_skyline_scene(self):
+ """
+ this test checks that the correct errors are raised if
+ wrong values for the input parameters are passed to the
+ function skyline_scene of the navipy.database module
+ it also contains some test where correct parameter values
+ were passed to the scene function and the output was
+ checked for correctness.
+ test cases:
+ None, NaN values in the the scene
+ scene is of wrong type (np.array)
+ scene is of wrong size
+ """
scene = self.mydb.scene(rowid=1)
scene2 = scene.copy()
scene2[3, 5, 2, 0] = np.nan
@@ -97,11 +137,13 @@ class TestCase(unittest.TestCase):
scene3 = np.array(scene3)
scene4 = np.zeros((3, 4, 5, 0))
- # put useless stuff here
+ # contains nan
with self.assertRaises(ValueError):
pcode.skyline(scene2)
+ # np.array instead of
with self.assertRaises(TypeError):
pcode.skyline(scene3)
+ # wrong size
with self.assertRaises(Exception):
pcode.skyline(scene4)
@@ -118,6 +160,20 @@ class TestCase(unittest.TestCase):
self.assertTrue(skyline.shape[1] > 0)
def test_id(self):
+ """
+ this test checks that the correct errors are raised if
+ wrong values for the input parameters id of the
+ function scene of the navipy.database module
+ it also contains some test where correct parameter values
+ were passed to the scene function and the output was
+ checked for correctness.
+ test cases:
+ zero and negative id
+ char for the id
+ None for the id
+ NaN for the id
+ float for the id
+ """
for rowid in [0, -2]:
with self.assertRaises(ValueError):
# print("rowid",rowid)
@@ -131,6 +187,7 @@ class TestCase(unittest.TestCase):
with self.assertRaises(TypeError):
self.mydb.scene(rowid=4.5)
+ # working cases
for rowid in [1, 2, 3, 4, 5]:
image = self.mydb.scene(rowid=rowid)
# image=np.array(image)
@@ -144,6 +201,17 @@ class TestCase(unittest.TestCase):
self.assertTrue(image.shape[1] > 0)
def test_distance_channel(self):
+ """
+ this test checks that the correct errors are raised if
+ wrong values for the input parameters distance_channel is passed to the
+ function scene of the navipy.database module
+ it also contains some test where correct parameter values
+ were passed to the scene function and the output was
+ checked for correctness.
+ test cases:
+ None, NaN, float, char values in the for the distance channel
+ negative int for the distance channel
+ """
scene = self.mydb.scene(rowid=1)
# should not be working
@@ -166,6 +234,18 @@ class TestCase(unittest.TestCase):
self.assertEqual(weighted_scene.shape, scene.shape)
def test_contr_weight_scene(self):
+ """
+ this test checks that the correct errors are raised if
+ wrong values for the input parameter scene is passed to the
+ function contr_weight_scene of the navipy.database module
+ it also contains some test where correct parameter values
+ were passed to the scene function and the output was
+ checked for correctness.
+ test cases:
+ None, NaN values in the the scene
+ scene is of wrong type (np.array)
+ scene is of wrong size
+ """
scene = self.mydb.scene(rowid=1)
# working cases
@@ -194,6 +274,18 @@ class TestCase(unittest.TestCase):
contrast = pcode.contrast_weighted_nearness(scene4)
def test_contr_weight_contrast(self):
+ """
+ this test checks that the correct errors are raised if
+ wrong values for the input parameter contrast_size are passed to the
+ function skyline_scene of the navipy.database module.
+ correct values are in the range between 2 and 5.
+ it also contains some test where correct parameter values
+ were passed to the scene function and the output was
+ checked for correctness.
+ test cases:
+ None, NaN values, chars, floats for the contrast_size
+ int values that are out of range (<2;>5)
+ """
scene = self.mydb.scene(rowid=1)
for size in [9.4, 'g', None, np.nan]:
with self.assertRaises(TypeError):
@@ -219,10 +311,26 @@ class TestCase(unittest.TestCase):
self.assertEqual(contrast.shape[1], scene.shape[1])
def test_pcv(self):
+ """
+ this test checks that the correct errors are raised if
+ wrong values for the input parameter direction is passed to the
+ function pcv of the navipy.database module.
+ correct values are in the range between 2 and 5.
+ it also contains some test where correct parameter values
+ were passed to the scene function and the output was
+ checked for correctness.
+ test cases:
+ wrong shape (must match the scenes shape)
+ last dimension shape does not match (must 2, azimuth, elevation)
+ direction has too many dimensions
+ is empty
+ contains wrong values (None, nan)
+ """
# working case
rowid = 1
my_scene = self.mydb.scene(rowid=rowid)
- directions = self.mydb.viewing_directions
+ directions = self.mydb.viewing_directions.copy()
+ directions = np.radians(directions)
my_pcv = pcode.pcv(my_scene, directions)
self.assertIsNotNone(my_pcv)
self.assertFalse(sum(my_pcv.shape) == 0)
@@ -259,12 +367,57 @@ class TestCase(unittest.TestCase):
with self.assertRaises(ValueError):
my_pcv = pcode.pcv(my_scene, testdirection)
+ # test if error is throught for elevation or azimuth out of range
+ # check elevation, should be in [-pi*2;pi*2]
+ testdirections = np.zeros((180, 360, 2))
+ testdirections[10, 15, 0] = -np.pi*2 - 0.001
+ with self.assertRaises(ValueError):
+ my_pcv = pcode.pcv(my_scene, testdirections)
+
+ testdirections = np.zeros((180, 360, 2))
+ testdirections[10, 15, 0] = np.pi*2 + 0.001
+ with self.assertRaises(ValueError):
+ my_pcv = pcode.pcv(my_scene, testdirections)
+
+ # check azimuth, should be in [-2*pi;2*pi]
+ testdirections = np.zeros((180, 360, 2))
+ testdirections[10, 15, 1] = -2 * np.pi - 0.001
+ with self.assertRaises(ValueError):
+ my_pcv = pcode.pcv(my_scene, testdirections)
+
+ testdirections = np.zeros((180, 360, 2))
+ testdirections[10, 15, 1] = 2*np.pi + 0.001
+ with self.assertRaises(ValueError):
+ my_pcv = pcode.pcv(my_scene, testdirections)
+
+ testdirections = np.zeros((180, 360, 2))
+ testdirections[10, 15, 1] = np.pi + 0.001
+ testdirections[10, 16, 1] = - np.pi - 0.001
+ with self.assertRaises(ValueError):
+ my_pcv = pcode.pcv(my_scene, testdirections)
+
def test_apcv(self):
+ """
+ this test checks that the correct errors are raised if
+ wrong values for the input parameter direction is passed to the
+ function apcv of the navipy.database module.
+ correct values are in the range between 2 and 5.
+ it also contains some test where correct parameter values
+ were passed to the scene function and the output was
+ checked for correctness.
+ test cases:
+ wrong shape (must match the scenes shape)
+ last dimension shape does not match (must 2, azimuth, elevation)
+ direction has too many dimensions
+ is empty
+ contains wrong values (None, nan)
+ """
# working case
rowid = 1
my_scene = self.mydb.scene(rowid=rowid)
- # print("scene shape",my_scene.shape)
- directions = self.mydb.viewing_directions
+ directions = self.mydb.viewing_directions.copy()
+ directions = np.radians(directions)
+
my_pcv = pcode.apcv(my_scene, directions)
self.assertIsNotNone(my_pcv)
@@ -303,6 +456,18 @@ class TestCase(unittest.TestCase):
my_pcv = pcode.apcv(my_scene, testdirection)
def test_size(self):
+ """
+ this test checks that the correct errors are raised if
+ wrong values for the input parameter size are passed to the
+ function michelson_contrast of the navipy.database module.
+ correct values are in the range between 2 and 5.
+ it also contains some test where correct parameter values
+ were passed to the scene function and the output was
+ checked for correctness.
+ test cases:
+ None, NaN values, chars, floats for the contrast_size
+ int values that are out of range (<2;>5)
+ """
# not working cases:
scene = self.mydb.scene(rowid=1)
@@ -328,6 +493,18 @@ class TestCase(unittest.TestCase):
self.assertTrue(contrast.shape[1] > 0)
def test_michelsoncontrast_scene(self):
+ """
+ this test checks that the correct errors are raised if
+ wrong values for the input parameter scene is passed to the
+ function michelson_contrast of the navipy.database module
+ it also contains some test where correct parameter values
+ were passed to the scene function and the output was
+ checked for correctness.
+ test cases:
+ None, NaN values in the the scene
+ scene is of wrong type (np.array)
+ scene is of wrong size
+ """
scene = self.mydb.scene(rowid=1)
diff --git a/navipy/processing/test_OpticFlow.py b/navipy/processing/test_OpticFlow.py
new file mode 100644
index 0000000000000000000000000000000000000000..fec681043034d66499c3f58ccc2e20b2e1aaae4e
--- /dev/null
+++ b/navipy/processing/test_OpticFlow.py
@@ -0,0 +1,711 @@
+import unittest
+import navipy.processing.mcode as mcode
+import pandas as pd
+# import matplotlib.pyplot as plt
+import numpy as np
+
+
+def Scale(data, oldmin, oldmax, mini, maxi, ran):
+ """Scales all values in data into the range mini, maxi
+ Input:
+ - data: values to be scaled
+ - mini: minimum of new range
+ - maxi: maximum of new range
+ - ran: range of new values (ran=maxi-mini)
+
+ Output:
+ - data: the scaled data"""
+ data = (data-oldmin)/oldmax
+ dmax = np.max(data)
+ dmin = np.min(data)
+ scaling = ran/(dmax-dmin)
+ data = (data-dmin)*scaling
+ data = data+mini
+ return data
+
+
+class TestCase(unittest.TestCase):
+ def setUp(self):
+ """sets up a distance array that is used in some tests,
+ as well as the velocity and viewing direction frames
+ """
+ distance = np.array([[2.60009956, 2.60009956, 2.60009956],
+ [2.60009956, 2.60009956, 2.60009956],
+ [2.60009956, 2.60009956, 2.60009956]])
+
+ self.scene = np.zeros((3, 3, 4, 1))
+ self.scene[:, :, 0, 0] = np.array(distance).copy()
+ self.scene[:, :, 1, 0] = np.array(distance).copy()
+ self.scene[:, :, 2, 0] = np.array(distance).copy()
+ self.scene[:, :, 3, 0] = np.array(distance).copy()
+ self.convention = 'rxyz'
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('location', 'dx'),
+ ('location', 'dy'), ('location', 'dz'),
+ (self.convention, 'alpha_0'), (self.convention, 'alpha_1'),
+ (self.convention, 'alpha_2'), (self.convention, 'dalpha_0'),
+ (self.convention, 'dalpha_1'), (self.convention, 'dalpha_2')]
+ self.index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
+ self.velocity = pd.Series(index=self.index)
+
+ self.elevation = np.arange(-np.pi/2, 4*(np.pi/360)-np.pi/2,
+ 2*(np.pi/360))
+ self.azimuth = np.arange(0, 4*(np.pi/360)+2*(np.pi/360), 2*(np.pi/360))
+
+ self.viewing_directions = np.zeros((3, 2))
+ self.viewing_directions[:, 0] = self.elevation
+ self.viewing_directions[:, 1] = self.azimuth
+
+ def test_wrong_convention(self):
+ """ this test checks if an error is raised
+ when none or a convention other than 'rxyz'
+ is used
+ """
+ velocity = pd.Series(index=['x', 'y', 'z',
+ 'alpha_0', 'alpha_1', 'alpha_2',
+ 'dx', 'dy', 'dz',
+ 'dalpha_0', 'dalpha_1', 'dalpha_2'])
+
+ 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]
+
+ velocity.x = x
+ velocity.y = y
+ velocity.z = z
+ velocity.alpha_0 = yaw
+ velocity.alpha_1 = pitch
+ velocity.alpha_2 = roll
+ velocity.dx = dx
+ velocity.dy = dy
+ velocity.dz = dz
+ velocity.dalpha_0 = dyaw
+ velocity.dalpha_1 = dpitch
+ velocity.dalpha_2 = droll
+
+ with self.assertRaises(Exception):
+ rof, hof, vof = mcode.optic_flow(self.scene,
+ self.viewing_directions,
+ velocity)
+ for convention in ['ryxz', 'ryzx', 'rxzy', 'rzyx',
+ 'rzxy', 'alsjf', '233', 9, None, -1]:
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('location', 'dx'),
+ ('location', 'dy'), ('location', 'dz'),
+ (convention, 'alpha_0'),
+ (convention, 'alpha_1'),
+ (convention, 'alpha_2'),
+ (convention, 'dalpha_0'),
+ (convention, 'dalpha_1'),
+ (convention, 'dalpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position',
+ 'orientation'])
+ velocity = pd.Series(index=index)
+ velocity['location']['x'] = x
+ velocity['location']['y'] = y
+ velocity['location']['z'] = z
+ velocity[convention]['alpha_0'] = yaw
+ velocity[convention]['alpha_1'] = pitch
+ velocity[convention]['alpha_2'] = roll
+ velocity['location']['dx'] = dx
+ velocity['location']['dy'] = dy
+ velocity['location']['dz'] = dz
+ velocity[convention]['dalpha_0'] = dyaw
+ velocity[convention]['dalpha_1'] = dpitch
+ velocity[convention]['dalpha_2'] = droll
+ with self.assertRaises(ValueError):
+ rof, hof, vof = mcode.optic_flow(self.scene,
+ self.viewing_directions,
+ velocity)
+
+ def test_rotation_not_too_strong(self):
+ """
+ this test checks that a Value Error is throught,
+ if the change in rotation is more than pi/2.
+ Thoughs, if the velocity of yaw, pitch and roll is
+ is smaller than pi/2
+ """
+ # dyaw is too big
+ positions = np.array([[-20, -20, 2.6, 1.57079633, 0, 0],
+ [-19.8, -20, 2.6,
+ 1.57079633 + np.pi/2 + 0.00001, 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
+
+ with self.assertRaises(ValueError):
+ rof, hof, vof = mcode.optic_flow(self.scene,
+ self.viewing_directions,
+ self.velocity)
+
+ # dpitch is too big
+ positions = np.array([[-20, -20, 2.6, 1.57079633, 0, 0],
+ [-19.8, -20, 2.6,
+ 1.57079633, 0 + np.pi/2 + 0.0001, 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
+
+ with self.assertRaises(ValueError):
+ rof, hof, vof = mcode.optic_flow(self.scene,
+ self.viewing_directions,
+ self.velocity)
+
+ # droll is too big
+ positions = np.array([[-20, -20, 2.6, 1.57079633, 0, 0],
+ [-19.8, -20, 2.6, 1.57079633, 0,
+ 0 + np.pi/2 + 0.0001]])
+
+ 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
+
+ with self.assertRaises(ValueError):
+ rof, hof, vof = mcode.optic_flow(self.scene,
+ self.viewing_directions,
+ self.velocity)
+
+ def test_only_x(self):
+ """
+ this test checks for the correct response if for example
+ 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 = [[0.00000000e+00, 0.00000000e+00, 0.00000000e+00],
+ [-8.07793567e-28, 0.00000000e+00, 6.77626358e-21],
+ [-1.61558713e-27, 0.00000000e+00, 0.00000000e+00]]
+
+ testhof = [[0.07692013, 0.07690842, 0.07687327],
+ [0.07692013, 0.07690842, 0.07687327],
+ [0.07692013, 0.07690842, 0.07687327]]
+
+ testvof = [[2.46536979e-10, 1.34244164e-03, 2.68447412e-03],
+ [2.46499430e-10, 1.34223718e-03, 2.68406526e-03],
+ [2.46386795e-10, 1.34162387e-03, 2.68283881e-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(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 = [[-7.02912790e-58, -8.01829413e-51, 1.06910588e-50],
+ [0.00000000e+00, -2.11758237e-22, -4.23516474e-22],
+ [-2.25532711e-28, 1.69406589e-21, -1.69406589e-21]]
+ testhof = [[-2.02066722e-17, -2.02066722e-17, -2.02066722e-17],
+ [-5.75929412e-03, -5.75929412e-03, -5.75929412e-03],
+ [-1.15168339e-02, -1.15168339e-02, -1.15168339e-02]]
+ testvof = [[-1.14794370e-41, -1.44444746e-34, 1.92592994e-34],
+ [0.00000000e+00, -1.35525272e-20, -2.71050543e-20],
+ [-6.45841185e-27, 5.42101086e-20, 0.00000000e+00]]
+
+ # print("sinel",sinel)
+ # print("sin prop:", hof[:,0]/list(reversed(sinel)))
+ # plt.plot(hof[:,0])
+ # plt.plot(sinel)
+ # plt.show()
+
+ 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_big(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.
+ here the azimuthal optic flow should be zero and the
+ elevational optic flow should be proportional with a
+ constant factor to the cos of the elevation
+ """
+ # generate scene aka distance channel
+ scene = np.random.random((180, 360, 4, 1))
+
+ elevation = np.arange(-np.pi/2, np.pi/2, 2*(np.pi/360))
+ azimuth = np.arange(0, 2*np.pi, 2*(np.pi/360))
+
+ viewing_directions = np.zeros((180, 2))
+ viewing_directions[:, 0] = elevation
+ viewing_directions[:, 1] = azimuth[0:180]
+
+ 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]
+
+ tuples = [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), ('location', 'dx'),
+ ('location', 'dy'), ('location', 'dz'),
+ (self.convention, 'alpha_0'), (self.convention, 'alpha_1'),
+ (self.convention, 'alpha_2'), (self.convention, 'dalpha_0'),
+ (self.convention, 'dalpha_1'), (self.convention, 'dalpha_2')]
+ index = pd.MultiIndex.from_tuples(tuples,
+ names=['position', 'orientation'])
+ velocity = pd.Series(index=index)
+
+ velocity['location']['x'] = x
+ velocity['location']['y'] = y
+ velocity['location']['z'] = z
+ velocity['rxyz']['alpha_0'] = yaw
+ velocity['rxyz']['alpha_1'] = pitch
+ velocity['rxyz']['alpha_2'] = roll
+ velocity['location']['dx'] = dx
+ velocity['location']['dy'] = dy
+ velocity['location']['dz'] = dz
+ velocity['rxyz']['dalpha_0'] = dyaw
+ 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)
+ has_zeros = len(np.where(cosel == 0)) > 0
+ factor = np.array([0])
+ if not has_zeros:
+ factor = np.array(hof[:, 0]/cosel)
+ # print("sinel",sinel)
+ # print("cos prop:", hof[:,0]/cosel)
+ # fig, ax = plt.subplots()
+ # ax.plot(elevation,hof[:,0],label='hof')
+ # ax.plot(elevation,cosel,label='cos(el)')
+ # plt.xlabel('elevation')
+ # ax.legend()
+ # plt.show()
+
+ assert np.all(factor == factor[0])
+ # assert np.all(np.isclose(hof, testhof))
+ # assert np.all(np.isclose(vof, testvof))
+
+ def test_only_pitch(self):
+ """
+ this test checks for the correct response if for example
+ the bee rotates only around the pitch axis keeping the other
+ parameters (x,y,z,yaw,roll) constant
+ """
+ positions = np.array([[-20, -20, 2.6, 1.57079633, 0, 0],
+ [-20, -20, 2.6, 1.57079633, 0.1, 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 = [[0.00000000e+00, -1.20370622e-35, -2.40741243e-35],
+ [-8.07793567e-28, 0.00000000e+00, -6.77626358e-21],
+ [-1.61558713e-27, 0.00000000e+00, -2.71050543e-20]]
+ testhof = [[-0.1, -0.09998477, -0.09993908],
+ [-0.09998477, -0.09996954, -0.09992386],
+ [-0.09993908, -0.09992386, -0.0998782]]
+ testvof = [[-3.20510345e-10, -1.74524096e-03, -3.48994999e-03],
+ [-3.20510345e-10, -1.74524096e-03, -3.48994999e-03],
+ [-3.20510345e-10, -1.74524096e-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))
+
+ def test_only_pitch_big(self):
+ """
+ this test checks for the correct response if for example
+ the bee rotates only around the pitch axis keeping the other
+ parameters (x,y,z,yaw,roll) constant.
+ here the azimuthal optic flow should be zero and the
+ elevational optic flow should be proportional with a
+ constant factor to the cos of the elevation
+ """
+ scene = np.random.random((180, 360, 4, 1))
+
+ elevation = np.arange(-np.pi/2, np.pi/2, 2*(np.pi/360))
+
+ azimuth = np.arange(0, 2*np.pi, 2*(np.pi/360))
+ viewing_directions = np.zeros((180, 2))
+ viewing_directions[:, 0] = elevation
+ viewing_directions[:, 1] = azimuth[0:180]
+
+ positions = np.array([[-20, -20, 2.6, 1.57079633, 0, 0],
+ [-20, -20, 2.6, 1.57079633, 0.1, 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(scene, viewing_directions,
+ self.velocity)
+
+ cosel = np.cos(elevation)
+ has_zeros = len(np.where(cosel == 0)) > 0
+ factor = np.array([0])
+ if not has_zeros:
+ factor = np.array(vof[0, :]/cosel)
+ # print("cos prop:", vof[0, :]/cosel)
+ # fig, ax = plt.subplots()
+ # ax.plot(elevation, vof[0, :], label='hof')
+ # ax.plot(elevation, cosel, label='cos(el)')
+ # plt.xlabel('elevation')
+ # ax.legend()
+ # plt.show()
+
+ assert np.all(factor == factor[0])
+ # assert np.all(np.isclose(hof, testhof))
+ # assert np.all(np.isclose(vof, testvof))
+
+ def test_only_roll_big(self):
+ """
+ this test checks for the correct response if for example
+ the bee rotates only around the roll axis keeping the other
+ parameters (x,y,z,yaw,pitch) constant
+ here the azimuthal optic flow should be zero and the
+ elevational optic flow should be proportional with a
+ constant factor to the cos of the elevation
+ """
+ scene = np.random.random((180, 360, 4, 1))
+
+ elevation = np.arange(-np.pi/2, np.pi/2, 2*(np.pi/360))
+
+ azimuth = np.arange(0, 2*np.pi, 2*(np.pi/360))
+ viewing_directions = np.zeros((180, 2))
+ viewing_directions[:, 0] = elevation
+ viewing_directions[:, 1] = azimuth[0:180]
+
+ positions = np.array([[-20, -20, 2.6, 1.57079633, 0, 0],
+ [-20, -20, 2.6, 1.57079633, 0, 0.1]])
+
+ 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(scene, viewing_directions,
+ self.velocity)
+ cosel = np.cos(elevation)
+ sinel = np.sin(elevation)
+ has_zeros = cosel[np.where(cosel == 0)] = 1
+ factor = np.array(hof[0, :]/cosel)
+ factor[has_zeros] = factor[1]
+ has_zerossin = sinel[np.where(sinel == 0)] = 1
+ factorsin = np.array(vof[0, :]/sinel)
+ factorsin[has_zerossin] = factorsin[0]
+ # print("cos prop:", vof[0, :]/cosel)
+ # fig, ax = plt.subplots()
+ # ax.plot(elevation, hof[0, :], label='hof')
+ # ax.plot(elevation, vof[0, :], label='vof')
+ # ax.plot(elevation, cosel, label='cos(el)')
+ # ax.plot(elevation, sinel, label='sin(el)')
+ # plt.xlabel('elevation')
+ # ax.legend()
+ # plt.show()
+
+ # fig, ax = plt.subplots()
+ # ax.plot(azimuth[0:180], hof[:, 0], label='hof')
+ # ax.plot(azimuth[0:180], vof[:, 0], label='vof')
+ # plt.xlabel('azimuth')
+ # ax.legend()
+ # plt.show()
+
+ for i in range(1, len(factor)):
+ assert np.isclose(factor[1], factor[i])
+ if i == 90:
+ continue
+ assert np.isclose(factorsin[0], factorsin[i])
+ # print(i)
+ # assert np.all(np.isclose(hof, testhof))
+ # assert np.all(np.isclose(vof, testvof))
+
+ def test_only_roll(self):
+ """
+ this test checks for the correct response if for example
+ the bee rotates only around the pitch axis keeping the other
+ parameters (x,y,z,yaw,roll) constant
+ """
+ positions = np.array([[-20, -20, 2.6, 1.57079633, 0.1, 0.1],
+ [-20, -20, 2.6, 1.57079633, 0.1, 0.2]])
+
+ 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 = [[-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]]
+ testhof = [[1.48878092e-05, 1.75149414e-03, 3.48756694e-03],
+ [-1.61062585e-04, 1.57529538e-03, 3.31112146e-03],
+ [-3.36963918e-04, 1.39861677e-03, 3.13366737e-03]]
+ testvof = [[-0.09950539, -0.09948998, -0.09944426],
+ [-0.09950368, -0.0994883, -0.09944262],
+ [-0.09950195, -0.09948661, -0.09944095]]
+
+ assert np.all(np.isclose(rof, testrof))
+ assert np.all(np.isclose(hof, testhof))
+ assert np.all(np.isclose(vof, testvof))
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/navipy/processing/test_mcode.py b/navipy/processing/test_mcode.py
new file mode 100644
index 0000000000000000000000000000000000000000..b756709b0f1d3283865eec83da00e1c50b467e40
--- /dev/null
+++ b/navipy/processing/test_mcode.py
@@ -0,0 +1,535 @@
+import unittest
+import numpy as np
+import navipy.processing.mcode as mcode
+# import matplotlib.pyplot as plt
+
+
+class TestCase(unittest.TestCase):
+ def setUp(self):
+ self.inM = mcode.Module(size=(1, 1))
+ self.tau = 0.025
+ self.lp = mcode.lp(self.tau, self.inM)
+ self.hp = mcode.hp(self.tau, self.inM)
+ self.hp2 = mcode.hp(self.tau, self.hp)
+
+ def test_step(self):
+ """
+ checks the response of the low and high pass filter to a step input
+ for low pass it should be h(t)=1-e^(-t/tau)
+ for high pass it should be h(t)=e^(-t/tau)
+ """
+ res_low = [[0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.],
+ [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.],
+ [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.],
+ [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.],
+ [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.],
+ [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.],
+ [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.],
+ [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.],
+ [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.],
+ [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.],
+ [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.], [0.],
+ [0.], [0.025], [0.049375], [0.07314063], [0.09631211],
+ [0.11890431], [0.1409317], [0.16240841], [0.1833482],
+ [0.20376449], [0.22367038], [0.24307862], [0.26200165],
+ [0.28045161], [0.29844032], [0.31597931], [0.33307983],
+ [0.34975284], [0.36600901], [0.38185879], [0.39731232],
+ [0.41237951], [0.42707002], [0.44139327], [0.45535844],
+ [0.46897448], [0.48225012], [0.49519387], [0.50781402],
+ [0.52011867], [0.5321157], [0.54381281], [0.55521749],
+ [0.56633705], [0.57717863], [0.58774916], [0.59805543],
+ [0.60810405], [0.61790144], [0.62745391], [0.63676756],
+ [0.64584837], [0.65470216], [0.66333461], [0.67175124],
+ [0.67995746], [0.68795852], [0.69575956], [0.70336557],
+ [0.71078143], [0.7180119], [0.7250616], [0.73193506],
+ [0.73863668], [0.74517077], [0.7515415], [0.75775296],
+ [0.76380914], [0.76971391], [0.77547106], [0.78108428],
+ [0.78655718], [0.79189325], [0.79709592], [0.80216852],
+ [0.8071143], [0.81193645], [0.81663804], [0.82122209],
+ [0.82569153], [0.83004924], [0.83429801], [0.83844056],
+ [0.84247955], [0.84641756], [0.85025712], [0.85400069],
+ [0.85765068], [0.86120941], [0.86467917], [0.86806219],
+ [0.87136064], [0.87457662], [0.87771221], [0.8807694],
+ [0.88375017], [0.88665641], [0.88949], [0.89225275],
+ [0.89494643], [0.89757277], [0.90013345], [0.90263012],
+ [0.90506436], [0.90743776], [0.90975181], [0.91200802],
+ [0.91420782], [0.91635262], [0.91844381], [0.92048271],
+ [0.92247064], [0.92440888], [0.92629865], [0.92814119],
+ [0.92993766], [0.93168922], [0.93339699], [0.93506206],
+ [0.93668551], [0.93826837], [0.93981166], [0.94131637],
+ [0.94278346], [0.94421388], [0.94560853], [0.94696832],
+ [0.94829411], [0.94958676], [0.95084709], [0.95207591],
+ [0.95327401], [0.95444216], [0.95558111], [0.95669158],
+ [0.95777429], [0.95882993], [0.95985918], [0.9608627],
+ [0.96184114], [0.96279511], [0.96372523], [0.9646321],
+ [0.9655163], [0.96637839], [0.96721893], [0.96803846],
+ [0.9688375], [0.96961656], [0.97037614], [0.97111674],
+ [0.97183882], [0.97254285], [0.97322928], [0.97389855],
+ [0.97455108], [0.97518731], [0.97580762], [0.97641243],
+ [0.97700212], [0.97757707], [0.97813764], [0.9786842],
+ [0.9792171], [0.97973667], [0.98024325], [0.98073717],
+ [0.98121874], [0.98168827], [0.98214607], [0.98259242],
+ [0.98302761], [0.98345191], [0.98386562], [0.98426898],
+ [0.98466225], [0.9850457], [0.98541955], [0.98578406],
+ [0.98613946], [0.98648598], [0.98682383], [0.98715323],
+ [0.9874744], [0.98778754], [0.98809285], [0.98839053],
+ [0.98868077], [0.98896375], [0.98923965], [0.98950866],
+ [0.98977095], [0.99002667], [0.99027601], [0.99051911],
+ [0.99075613], [0.99098723], [0.99121254], [0.99143223],
+ [0.99164643], [0.99185526], [0.99205888], [0.99225741],
+ [0.99245098], [0.9926397], [0.99282371], [0.99300312],
+ [0.99317804], [0.99334859], [0.99351487], [0.993677]]
+ res_low = np.array(res_low)
+ res_high = [[0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 9.75000000e-01, 9.26859375e-01, 8.81095693e-01,
+ 8.37591593e-01,
+ 7.96235509e-01, 7.56921380e-01, 7.19548387e-01,
+ 6.84020686e-01,
+ 6.50247164e-01, 6.18141210e-01, 5.87620488e-01,
+ 5.58606727e-01,
+ 5.31025519e-01, 5.04806134e-01, 4.79881332e-01,
+ 4.56187191e-01,
+ 4.33662948e-01, 4.12250840e-01, 3.91895955e-01,
+ 3.72546092e-01,
+ 3.54151629e-01, 3.36665392e-01, 3.20042538e-01,
+ 3.04240438e-01,
+ 2.89218567e-01, 2.74938400e-01, 2.61363316e-01,
+ 2.48458503e-01,
+ 2.36190864e-01, 2.24528940e-01, 2.13442824e-01,
+ 2.02904084e-01,
+ 1.92885695e-01, 1.83361964e-01, 1.74308467e-01,
+ 1.65701986e-01,
+ 1.57520451e-01, 1.49742879e-01, 1.42349324e-01,
+ 1.35320826e-01,
+ 1.28639360e-01, 1.22287792e-01, 1.16249832e-01,
+ 1.10509997e-01,
+ 1.05053566e-01, 9.98665458e-02, 9.49356351e-02,
+ 9.02481881e-02,
+ 8.57921838e-02, 8.15561947e-02, 7.75293576e-02,
+ 7.37013456e-02,
+ 7.00623416e-02, 6.66030135e-02, 6.33144897e-02,
+ 6.01883368e-02,
+ 5.72165377e-02, 5.43914711e-02, 5.17058922e-02,
+ 4.91529138e-02,
+ 4.67259887e-02, 4.44188930e-02, 4.22257102e-02,
+ 4.01408157e-02,
+ 3.81588629e-02, 3.62747691e-02, 3.44837024e-02,
+ 3.27810696e-02,
+ 3.11625042e-02, 2.96238556e-02, 2.81611777e-02,
+ 2.67707196e-02,
+ 2.54489153e-02, 2.41923751e-02, 2.29978766e-02,
+ 2.18623564e-02,
+ 2.07829026e-02, 1.97567468e-02, 1.87812574e-02,
+ 1.78539328e-02,
+ 1.69723949e-02, 1.61343829e-02, 1.53377477e-02,
+ 1.45804464e-02,
+ 1.38605369e-02, 1.31761729e-02, 1.25255993e-02,
+ 1.19071479e-02,
+ 1.13192325e-02, 1.07603453e-02, 1.02290533e-02,
+ 9.72399379e-03,
+ 9.24387160e-03, 8.78745544e-03, 8.35357482e-03,
+ 7.94111707e-03,
+ 7.54902441e-03, 7.17629133e-03, 6.82196195e-03,
+ 6.48512758e-03,
+ 6.16492440e-03, 5.86053126e-03, 5.57116753e-03,
+ 5.29609113e-03,
+ 5.03459663e-03, 4.78601342e-03, 4.54970401e-03,
+ 4.32506238e-03,
+ 4.11151242e-03, 3.90850649e-03, 3.71552399e-03,
+ 3.53206999e-03,
+ 3.35767403e-03, 3.19188888e-03, 3.03428937e-03,
+ 2.88447133e-03,
+ 2.74205056e-03, 2.60666181e-03, 2.47795788e-03,
+ 2.35560871e-03,
+ 2.23930053e-03, 2.12873507e-03, 2.02362877e-03,
+ 1.92371210e-03,
+ 1.82872882e-03, 1.73843533e-03, 1.65260009e-03,
+ 1.57100296e-03,
+ 1.49343469e-03, 1.41969635e-03, 1.34959884e-03,
+ 1.28296240e-03,
+ 1.21961613e-03, 1.15939759e-03, 1.10215233e-03,
+ 1.04773356e-03,
+ 9.96001714e-04, 9.46824129e-04, 9.00074688e-04,
+ 8.55633500e-04,
+ 8.13386596e-04, 7.73225633e-04, 7.35047617e-04,
+ 6.98754641e-04,
+ 6.64253631e-04, 6.31456108e-04, 6.00277962e-04,
+ 5.70639238e-04,
+ 5.42463926e-04, 5.15679769e-04, 4.90218081e-04,
+ 4.66013563e-04,
+ 4.43004143e-04, 4.21130814e-04, 4.00337480e-04,
+ 3.80570817e-04,
+ 3.61780133e-04, 3.43917239e-04, 3.26936325e-04,
+ 3.10793844e-04,
+ 2.95448398e-04, 2.80860633e-04, 2.66993139e-04,
+ 2.53810353e-04,
+ 2.41278467e-04, 2.29365343e-04, 2.18040429e-04,
+ 2.07274683e-04,
+ 1.97040495e-04, 1.87311621e-04, 1.78063110e-04,
+ 1.69271243e-04,
+ 1.60913476e-04, 1.52968373e-04, 1.45415560e-04,
+ 1.38235666e-04,
+ 1.31410280e-04, 1.24921898e-04, 1.18753879e-04,
+ 1.12890406e-04,
+ 1.07316442e-04, 1.02017693e-04, 9.69805695e-05,
+ 9.21921539e-05,
+ 8.76401663e-05, 8.33129330e-05, 7.91993570e-05,
+ 7.52888887e-05,
+ 7.15714998e-05, 6.80376570e-05, 6.46782977e-05,
+ 6.14848068e-05,
+ 5.84489944e-05, 5.55630753e-05, 5.28196485e-05,
+ 5.02116784e-05,
+ 4.77324767e-05, 4.53756857e-05, 4.31352612e-05,
+ 4.10054577e-05]]
+ res_high = np.array(np.transpose(res_high))
+ res_high2 = [[0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0.92685938, 0.85792421, 0.792972, 0.73179162,
+ 0.6741827, 0.61995514, 0.56892856, 0.52093182,
+ 0.47580257, 0.43338679,
+ 0.39353836, 0.35611869, 0.32099634, 0.28804663,
+ 0.25715132, 0.2281983,
+ 0.20108123, 0.17569931, 0.15195697, 0.12976358,
+ 0.10903325, 0.08968457,
+ 0.07164035, 0.05482747, 0.03917662, 0.02462212,
+ 0.01110175, -0.00144343,
+ -0.01306927, -0.02382865, -0.03377167, -0.04294577,
+ -0.05139586, -0.05916446,
+ -0.06629183, -0.07281607, -0.07877324, -0.08419746,
+ -0.08912102, -0.09357446,
+ -0.09758667, -0.10118496, -0.10439517, -0.10724173,
+ -0.1097477, -0.11193492,
+ -0.11382399, -0.1154344, -0.11678453, -0.11789175,
+ -0.11877247, -0.11944216,
+ -0.11991544, -0.12020607, -0.12032708, -0.1202907,
+ -0.1201085, -0.11979136,
+ -0.11934956, -0.11879274, -0.11813002, -0.11736995,
+ -0.1165206, -0.11558954,
+ -0.11458389, -0.11351036, -0.11237524, -0.11118442,
+ -0.10994346, -0.10865755,
+ -0.10733157, -0.10597008, -0.10457737, -0.10315744,
+ -0.10171402, -0.10025062,
+ -0.09877051, -0.09727674, -0.09577215, -0.09425938,
+ -0.09274091, -0.09121902,
+ -0.08969585, -0.08817336, -0.08665339, -0.08513763,
+ -0.08362764, -0.08212486,
+ -0.08063063, -0.07914616, -0.07767256, -0.07621087,
+ -0.07476201, -0.07332684,
+ -0.07190613, -0.07050057, -0.06911079, -0.06773735,
+ -0.06638075, -0.06504143,
+ -0.06371979, -0.06241616, -0.06113083, -0.05986406,
+ -0.05861604, -0.05738695,
+ -0.05617691, -0.05498604, -0.0538144, -0.05266202,
+ -0.05152892, -0.0504151,
+ -0.0493205, -0.04824509, -0.04718878, -0.04615148,
+ -0.04513308, -0.04413346,
+ -0.04315247, -0.04218997, -0.04124579, -0.04031975,
+ -0.03941167, -0.03852136,
+ -0.03764862, -0.03679324, -0.03595501, -0.0351337,
+ -0.0343291, -0.03354097,
+ -0.03276908, -0.0320132, -0.03127309, -0.0305485,
+ -0.02983921, -0.02914496,
+ -0.02846552, -0.02780063, -0.02715005, -0.02651355,
+ -0.02589087, -0.02528178,
+ -0.02468603, -0.02410338, -0.02353359, -0.02297643,
+ -0.02243166, -0.02189904,
+ -0.02137835, -0.02086935, -0.02037182, -0.01988554,
+ -0.01941027, -0.01894581,
+ -0.01849193, -0.01804842, -0.01761507, -0.01719168,
+ -0.01677803, -0.01637392,
+ -0.01597916, -0.01559355, -0.01521689, -0.014849,
+ -0.01448969, -0.01413877,
+ -0.01379607, -0.0134614, -0.0131346, -0.01281548,
+ -0.01250389, -0.01219965,
+ -0.0119026, -0.01161259, -0.01132945, -0.01105304,
+ -0.0107832, -0.01051979,
+ -0.01026266, -0.01001167, -0.00976668, -0.00952755,
+ -0.00929415, -0.00906634,
+ -0.00884401, -0.00862703, -0.00841526, -0.0082086,
+ -0.00800692, -0.0078101,
+ -0.00761804, -0.00743063, -0.00724775, -0.0070693,
+ -0.00689517, -0.00672527,
+ -0.0065595, -0.00639775, -0.00623994, -0.00608596]]
+ res_high2 = np.array(np.transpose(res_high2))
+
+ hlow = np.zeros((300, 1))
+ aclow = np.zeros((300, 1))
+ achigh = np.zeros((300, 1))
+ achigh2 = np.zeros((300, 1))
+ hhigh = np.zeros((300, 1))
+ step = np.zeros((300))
+ step[100::] = 1
+ for i in range(300):
+ self.inM.Input[0, 0] = step[i]
+ self.lp.update()
+ aclow[i] = self.lp.Input[0, 0]
+ # plt.scatter(i, self.lp.Input[0, 0], c='r')
+ self.hp.update()
+ achigh[i] = self.hp.Input[0, 0]
+ # plt.scatter(i, self.hp.Input[0, 0], c='b')
+ self.hp2.update()
+ achigh2[i] = self.hp2.Input[0, 0]
+ # plt.scatter(i, self.hp2.Input[0, 0], c='g')
+ hlow[i] = 1-np.exp(-(i/1600)/self.tau)
+ hhigh[i] = np.exp(-(i/800)/self.tau)
+ # plt.plot(range(300), step)
+ # plt.plot(self.lp.Input[0:300,0])#hlow[0:200], c='r')
+ # plt.plot(range(100, 300), hhigh[0:200], c='b')
+ # plt.plot(aclow[100:300], c = 'r')
+ # plt.plot(hlow, c = 'b')
+ # plt.show()
+ # assert np.all(np.isclose(res_low[100:300, 0], hlow[0:200]))
+ # assert np.all(np.isclose(res_high[100:300, 0], hhigh[0:200]))
+
+ assert np.all(np.isclose(res_low, aclow))
+ assert np.all(np.isclose(res_high, achigh))
+ assert np.all(np.isclose(res_high2, achigh2))
+
+ def test_pulse(self):
+ """
+ checks the response of the low and high pass filter to a pulse input
+ for low pass it should be h(t)=(1/tau)e^(-t/tau)
+ for high pass it should be h(t)=inputsignal(t)-(1/tau)*e^(-t/tau)
+ """
+ res_low = [[0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0.025, 0.049375, 0.07314063, 0.09631211,
+ 0.11890431, 0.1409317, 0.16240841, 0.1833482,
+ 0.20376449, 0.22367038,
+ 0.21807862, 0.21262665, 0.20731099, 0.20212821,
+ 0.19707501, 0.19214813,
+ 0.18734443, 0.18266082, 0.1780943, 0.17364194,
+ 0.16930089, 0.16506837,
+ 0.16094166, 0.15691812, 0.15299517, 0.14917029,
+
+ 0.14544103, 0.141805,
+ 0.13825988, 0.13480338, 0.1314333, 0.12814746,
+ 0.12494378, 0.12182018,
+ 0.11877468, 0.11580531, 0.11291018, 0.11008743,
+ 0.10733524, 0.10465186,
+ 0.10203556, 0.09948467, 0.09699756, 0.09457262,
+ 0.0922083, 0.08990309,
+ 0.08765552, 0.08546413, 0.08332753, 0.08124434,
+ 0.07921323, 0.0772329,
+ 0.07530208, 0.07341952, 0.07158404, 0.06979444,
+ 0.06804957, 0.06634833,
+ 0.06468963, 0.06307239, 0.06149558, 0.05995819,
+ 0.05845923, 0.05699775,
+ 0.05557281, 0.05418349, 0.0528289, 0.05150818,
+ 0.05022047, 0.04896496,
+ 0.04774084, 0.04654732, 0.04538363, 0.04424904,
+ 0.04314282, 0.04206425,
+ 0.04101264, 0.03998732, 0.03898764, 0.03801295,
+ 0.03706263, 0.03613606,
+ 0.03523266, 0.03435184, 0.03349305, 0.03265572,
+ 0.03183933, 0.03104334,
+ 0.03026726, 0.02951058, 0.02877281, 0.02805349,
+ 0.02735216, 0.02666835,
+ 0.02600164, 0.0253516, 0.02471781, 0.02409987,
+ 0.02349737, 0.02290994,
+ 0.02233719, 0.02177876, 0.02123429, 0.02070343,
+ 0.02018585, 0.0196812,
+ 0.01918917, 0.01870944, 0.0182417, 0.01778566,
+ 0.01734102, 0.0169075,
+ 0.01648481, 0.01607269, 0.01567087, 0.0152791,
+ 0.01489712, 0.01452469,
+ 0.01416158, 0.01380754]]
+ res_low = np.array(np.transpose(res_low))
+ res_high = [[0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0., 0, 0., 0., 0.,
+ 0., 0., 0., 0., 0., 0., 0.,
+ 0., 0., 0.975, 0.92685938, 0.88109569, 0.83759159,
+ 0.79623551, 0.75692138, 0.71954839, 0.68402069,
+ 0.65024716, 0.61814121,
+ -0.38737951, -0.36825265, -0.35007017, -0.33278546,
+ -0.31635418, -0.30073419,
+ -0.28588544, -0.27176985, -0.25835121, -0.24559512,
+
+ -0.23346886, -0.22194133,
+ -0.21098298, -0.2005657, -0.19066277, -0.18124879,
+ -0.17229963, -0.16379234,
+ -0.15570509, -0.14801715, -0.14070881, -0.13376131,
+ -0.12715684, -0.12087847,
+ -0.1149101, -0.10923641, -0.10384287, -0.09871562,
+ -0.09384154, -0.08920811,
+ -0.08480346, -0.08061629, -0.07663586, -0.07285197,
+ -0.0692549, -0.06583544,
+ -0.06258482, -0.05949469, -0.05655714, -0.05376463,
+ -0.05111, -0.04858645,
+ -0.04618749, -0.04390698, -0.04173908, -0.03967821,
+ -0.0377191, -0.03585672,
+ -0.03408629, -0.03240328, -0.03080337, -0.02928245,
+ -0.02783663, -0.0264622,
+ -0.02515563, -0.02391357, -0.02273284, -0.0216104,
+ -0.02054339, -0.01952906,
+ -0.01856481, -0.01764817, -0.01677679, -0.01594844,
+ -0.01516099, -0.01441241,
+ -0.0137008, -0.01302432, -0.01238125, -0.01176992,
+ -0.01118878, -0.01063634,
+ -0.01011117, -0.00961193, -0.00913734, -0.00868618,
+ -0.0082573, -0.0078496,
+ -0.00746202, -0.00709359, -0.00674334, -0.00641039,
+ -0.00609388, -0.00579299,
+ -0.00550696, -0.00523506, -0.00497657, -0.00473086,
+ -0.00449727, -0.00427522,
+ -0.00406413, -0.00386346, -0.0036727, -0.00349136,
+ -0.00331898, -0.0031551,
+ -0.00299932, -0.00285123, -0.00271045, -0.00257662,
+ -0.0024494, -0.00232846,
+ -0.00221349, -0.0021042, -0.00200031, -0.00190154,
+ -0.00180765, -0.0017184,
+ -0.00163355, -0.0015529]]
+ res_high = np.array(np.transpose(res_high))
+ res_high2 = [[0.00000000e+00, 0.00000000e+00,
+ 0.00000000e+00, 0.00000000e+00,
+ 0.00000000e+00, 0.00000000e+00,
+ 0.00000000e+00, 0.00000000e+00,
+ 0.00000000e+00, 0.00000000e+00,
+ 0.00000000e+00, 0.00000000e+00,
+ 0.00000000e+00, 0.00000000e+00,
+ 0.00000000e+00, 0.00000000e+00,
+ 0.00000000e+00, 0.00000000e+00,
+ 0.00000000e+00, 0.00000000e+00,
+ 0.00000000e+00, 0.00000000e+00,
+ 0.00000000e+00, 0.00000000e+00,
+ 0.00000000e+00, 0.00000000e+00,
+ 0.00000000e+00, 0.00000000e+00,
+ 0.00000000e+00, 0.00000000e+00,
+ 9.26859375e-01, 8.57924209e-01,
+ 7.92972004e-01, 7.31791619e-01,
+ 6.74182700e-01, 6.19955140e-01,
+ 5.68928559e-01, 5.20931824e-01,
+ 4.75802575e-01, 4.33386788e-01,
+ -5.33321018e-01, -5.01805518e-01,
+ -4.71975665e-01, -4.43744992e-01,
+ -4.17031379e-01, -3.91756844e-01,
+ -3.67847330e-01, -3.45232510e-01,
+ -3.23845607e-01, -3.03623208e-01,
+ -2.84505102e-01, -2.66434121e-01,
+ -2.49355984e-01, -2.33219153e-01,
+ -2.17974700e-01, -2.03576173e-01,
+ -1.89979475e-01, -1.77142741e-01,
+ -1.65026234e-01, -1.53592231e-01,
+ -1.42804928e-01, -1.32630340e-01,
+ -1.23036213e-01, -1.13991933e-01,
+ -1.05468448e-01, -9.74381891e-02,
+ -8.98749929e-02, -8.27540341e-02,
+ -7.60517572e-02, -6.97458127e-02,
+ -6.38149963e-02, -5.82391920e-02,
+ -5.29993165e-02, -4.80772677e-02,
+ -4.34558752e-02, -3.91188535e-02,
+ -3.50507569e-02, -3.12369376e-02,
+ -2.76635054e-02, -2.43172891e-02,
+ -2.11858004e-02, -1.82571997e-02,
+ -1.55202623e-02, -1.29643485e-02,
+ -1.05793729e-02, -8.35577700e-03,
+ -6.28450214e-03, -4.35696418e-03,
+ -2.56502943e-03, -9.00991702e-04,
+ 6.42449431e-04, 2.07220929e-03,
+ 3.39483774e-03, 4.61653781e-03,
+ 5.74318344e-03, 6.78033626e-03,
+ 7.73326160e-03, 8.60694363e-03,
+ 9.40609983e-03, 1.01351946e-02,
+ 1.07984523e-02, 1.13998695e-02,
+ 1.19432270e-02, 1.24321006e-02,
+ 1.28698718e-02, 1.32597379e-02,
+ 1.36047214e-02, 1.39076793e-02,
+ 1.41713114e-02, 1.43981685e-02,
+ 1.45906605e-02, 1.47510631e-02,
+ 1.48815254e-02, 1.49840763e-02,
+ 1.50606305e-02, 1.51129951e-02,
+ 1.51428745e-02, 1.51518766e-02,
+ 1.51415172e-02, 1.51132251e-02,
+ 1.50683470e-02, 1.50081513e-02,
+ 1.49338326e-02, 1.48465157e-02,
+ 1.47472591e-02, 1.46370585e-02,
+ 1.45168504e-02, 1.43875152e-02,
+ 1.42498801e-02, 1.41047219e-02,
+ 1.39527703e-02, 1.37947095e-02,
+ 1.36311815e-02, 1.34627881e-02,
+ 1.32900929e-02, 1.31136238e-02,
+ 1.29338747e-02, 1.27513072e-02,
+ 1.25663530e-02, 1.23794148e-02,
+ 1.21908686e-02, 1.20010647e-02,
+ 1.18103293e-02, 1.16189661e-02,
+ 1.14272571e-02, 1.12354643e-02,
+ 1.10438306e-02, 1.08525808e-02,
+ 1.06619231e-02, 1.04720493e-02]]
+ res_high2 = np.array(np.transpose(res_high2))
+
+ step = np.zeros((150))
+ step[30:40] = 1
+ hlow = np.zeros((150, 1))
+ hhigh = np.zeros((150, 1))
+ aclow = np.zeros((150, 1))
+ achigh = np.zeros((150, 1))
+ achigh2 = np.zeros((150, 1))
+ for i in range(150):
+ self.inM.Input[0, 0] = step[i]
+ self.lp.update()
+ aclow[i] = self.lp.Input[0, 0]
+ # plt.scatter(i, self.lp.Input[0, 0], c='r')
+ self.hp.update()
+ achigh[i] = self.hp.Input[0, 0]
+ # plt.scatter(i, self.hp.Input[0, 0], c='b')
+ self.hp2.update()
+ achigh2[i] = self.hp2.Input[0, 0]
+ # plt.scatter(i, self.hp2.Input[0, 0], c='g')
+ hlow[i] = ((1/420)/self.tau)*np.exp(-((i-30)/1490)/self.tau)
+ hhigh[i] = step[i]-((1/80)/self.tau)*np.exp(-((i-30)/800)/self.tau)
+ # print("achigh2",np.transpose(achigh2))
+ # plt.plot(range(150), step)
+ # plt.plot( hlow[0:110], c='r')
+ # plt.plot( hhigh[30:150], c='b')
+ # plt.plot(aclow[40:150,0], c='r')
+ # plt.plot(hlow[0:110,0],c='b')
+ # plt.show()
+ # assert np.all(np.isclose(aclow[40:150, 0], hlow[0:110]))
+ # assert np.all(np.isclose(achigh[30:150, 0], hhigh[30:150]))
+
+ assert np.all(np.isclose(res_low, aclow))
+ assert np.all(np.isclose(achigh, res_high))
+ assert np.all(np.isclose(achigh2, res_high2))
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/navipy/resources/database2.db b/navipy/resources/database2.db
new file mode 100644
index 0000000000000000000000000000000000000000..7d9ff3935066ecd06f0b7c5a80635e49bd70a5dd
Binary files /dev/null and b/navipy/resources/database2.db differ
diff --git a/navipy/sensors/blendtest_api.py b/navipy/sensors/blendtest_api.py
new file mode 100644
index 0000000000000000000000000000000000000000..37fd4056229d0f052a274dcb2acb831849f135ad
--- /dev/null
+++ b/navipy/sensors/blendtest_api.py
@@ -0,0 +1,60 @@
+"""
+ Unittesting under blender
+"""
+import unittest
+import numpy as np
+from navipy.sensors.renderer import BlenderRender
+
+
+class TestBlenderRender_api(unittest.TestCase):
+ @classmethod
+ def setUpClass(self):
+ self.cyberbee = BlenderRender()
+
+ def test_class_assigments_error(self):
+ """ Test that error are correctly raised
+ List of tested function:
+ * camera_rotation_mode
+ * cycle_samples
+ * camera_fov
+ * camera_gaussian_width
+ * camera_resolution
+ """
+ with self.assertRaises(TypeError):
+ # Should be an integer
+ self.cyberbee.cycle_samples = 0.1
+ with self.assertRaises(TypeError):
+ # Should be a tuple list or np.ndarray
+ self.cyberbee.camera_fov = 'bla'
+ with self.assertRaises(TypeError):
+ # Should be a float or int, so not a complex
+ self.cyberbee.camera_gaussian_width = 4 + 4j
+ with self.assertRaises(TypeError):
+ # Should be a tuple, list or nd.array
+ self.cyberbee.camera_resolution = 'bla'
+
+ def test_class_assigments(self):
+ """ Test set/get match
+
+ * camera_rotation_mode
+ * cycle_samples
+ * camera_fov
+ * camera_gaussian_width
+ * camera_resolution
+ """
+ # camera cycle_samples
+ val = 100
+ self.cyberbee.cycle_samples = val
+ self.assertEqual(val, self.cyberbee.cycle_samples)
+ # camera fov
+ val = np.array([[-90, 90], [-180, 180]])
+ self.cyberbee.camera_fov = val
+ np.testing.assert_allclose(val, self.cyberbee.camera_fov)
+ # camera gaussian
+ val = 1.5
+ self.cyberbee.gaussian_width = val
+ self.assertEqual(val, self.cyberbee.gaussian_width)
+ # camera resolution
+ val = np.array([360, 180])
+ self.cyberbee.camera_resolution = val
+ np.testing.assert_allclose(val, self.cyberbee.camera_resolution)
diff --git a/navipy/sensors/blendtest_renderer.py b/navipy/sensors/blendtest_renderer.py
index 6189b00566a73d98a6259de2e91e2e0dc12a49ef..c4b3b492a9fce982ad2fb373fe1bd10186e4b212 100644
--- a/navipy/sensors/blendtest_renderer.py
+++ b/navipy/sensors/blendtest_renderer.py
@@ -1,67 +1,69 @@
-"""
- Unittesting under blender
-"""
-import unittest
-import numpy as np
from navipy.sensors.renderer import BlenderRender
+from navipy.maths.euler import matrix, from_matrix
+from navipy.maths.quaternion import from_matrix as quat_matrix
+import pandas as pd
+import numpy as np
+import unittest
-class TestBlenderRender(unittest.TestCase):
- @classmethod
- def setUpClass(self):
- self.cyberbee = BlenderRender()
-
- def test_class_assigments_error(self):
- """ Test that error are correctly raised
- List of tested function:
- * camera_rotation_mode
- * cycle_samples
- * camera_fov
- * camera_gaussian_width
- * camera_resolution
+class TestBlenderRender_renderer(unittest.TestCase):
+ def setUp(self):
"""
- with self.assertRaises(TypeError):
- # Should a string
- self.cyberbee.camera_rotation_mode = 0
- with self.assertRaises(TypeError):
- # Should be an integer
- self.cyberbee.cycle_samples = 0.1
- with self.assertRaises(TypeError):
- # Should be a tuple list or np.ndarray
- self.cyberbee.camera_fov = 'bla'
- with self.assertRaises(TypeError):
- # Should be a float or int, so not a complex
- self.cyberbee.camera_gaussian_width = 4 + 4j
- with self.assertRaises(TypeError):
- # Should be a tuple, list or nd.array
- self.cyberbee.camera_resolution = 'bla'
+ Prepare for the test
+ """
+ convention = 'rxyz'
+ index = pd.MultiIndex.from_tuples(
+ [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention, 'alpha_0'),
+ (convention, 'alpha_1'), (convention, 'alpha_2')])
+ self.posorient = pd.Series(index=index)
+ self.posorient.loc['location']['x'] = 0
+ self.posorient.loc['location']['y'] = 0
+ self.posorient.loc['location']['z'] = 1
+ self.posorient.loc[convention]['alpha_0'] = np.pi/4
+ self.posorient.loc[convention]['alpha_1'] = np.pi/7
+ self.posorient.loc[convention]['alpha_2'] = np.pi/3
+
+ convention = self.posorient.index.get_level_values(0)[-1]
+ a, b, c = self.posorient.loc[convention]
+ self.matorient = matrix(a, b, c, axes=convention)
- def test_class_assigments(self):
- """ Test set/get match
+ self.renderer = BlenderRender()
+ self.image_ref = self.renderer.scene(self.posorient)
- * camera_rotation_mode
- * cycle_samples
- * camera_fov
- * camera_gaussian_width
- * camera_resolution
+ def test_diff_euler_xyz2yzx(self):
"""
- # camera cycle_samples
- val = 100
- self.cyberbee.cycle_samples = val
- self.assertEqual(val, self.cyberbee.cycle_samples)
- # camera rotation mode
- val = 'XYZ'
- self.cyberbee.camera_rotation_mode = val
- self.assertEqual(val, self.cyberbee.camera_rotation_mode)
- # camera fov
- val = np.array([[-90, 90], [-180, 180]])
- self.cyberbee.camera_fov = val
- np.testing.assert_allclose(val, self.cyberbee.camera_fov)
- # camera gaussian
- val = 1.5
- self.cyberbee.gaussian_width = val
- self.assertEqual(val, self.cyberbee.gaussian_width)
- # camera resolution
- val = np.array([360, 180])
- self.cyberbee.camera_resolution = val
- np.testing.assert_allclose(val, self.cyberbee.camera_resolution)
+ Test if images rendered from two different conventions match \
+ one another
+ """
+ convention = 'ryzx'
+ index = pd.MultiIndex.from_tuples(
+ [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention, 'alpha_0'),
+ (convention, 'alpha_1'), (convention, 'alpha_2')])
+ posorient2 = pd.Series(index=index)
+ posorient2.loc['location'][:] = self.posorient.loc['location'][:]
+ # An orientation matrix need to be calculated from
+ # the euler angle of the convention of 'reference'
+ # so that it can be decompase in another convention
+ at, bt, ct = from_matrix(self.matorient, axes=convention)
+ posorient2.loc[convention] = [at, bt, ct]
+ image2 = self.renderer.scene(posorient2)
+ np.testing.assert_allclose(image2, self.image_ref)
+
+ def test_euler_xyz_2_quaternion(self):
+ convention = 'quaternion'
+ index = pd.MultiIndex.from_tuples(
+ [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention, 'q_0'),
+ (convention, 'q_1'), (convention, 'q_2'), (convention, 'q_3')],
+ names=['position', 'orientation'])
+ posorient2 = pd.Series(index=index)
+ posorient2.loc['location'][:] = self.posorient.loc['location'][:]
+ # An orientation matrix need to be calculated from
+ # the euler angle of the convention of 'reference'
+ # so that it can be decompase in another convention
+ at, bt, ct, dt = quat_matrix(self.matorient)
+ posorient2.loc[convention] = [at, bt, ct, dt]
+ image2 = self.renderer.scene(posorient2)
+ np.testing.assert_allclose(image2, self.image_ref, atol=1.2)
diff --git a/navipy/sensors/blendtestyxz.py b/navipy/sensors/blendtestyxz.py
new file mode 100644
index 0000000000000000000000000000000000000000..f6569d15399a890e54e878600a24d4de4bf9030d
--- /dev/null
+++ b/navipy/sensors/blendtestyxz.py
@@ -0,0 +1,130 @@
+from navipy.sensors.renderer import BlenderRender
+from navipy.maths.euler import matrix, from_matrix
+from navipy.maths.quaternion import from_matrix as from_quat_matrix
+import pandas as pd
+import numpy as np
+import unittest
+
+
+class TestBlenderRender_renderer(unittest.TestCase):
+ def setUp(self):
+ """
+ Prepare for the test
+ """
+ self.conventions = ['rxyz', 'rxzy', 'ryzx', 'ryxz', 'rzxy', 'rzyx']
+ self.renderer = BlenderRender()
+
+ def test_diff_euler_2_euler(self):
+ """
+ Test if images rendered from two different conventions match \
+ one another
+ """
+ for convention in self.conventions:
+ index = pd.MultiIndex.from_tuples(
+ [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention, 'alpha_0'),
+ (convention, 'alpha_1'), (convention, 'alpha_2')])
+ posorient = pd.Series(index=index)
+ posorient.loc['location']['x'] = 0
+ posorient.loc['location']['y'] = 0
+ posorient.loc['location']['z'] = 1
+ posorient.loc[convention]['alpha_0'] = np.pi/4
+ posorient.loc[convention]['alpha_1'] = np.pi/7
+ posorient.loc[convention]['alpha_2'] = np.pi/3
+
+ a, b, c = posorient.loc[convention]
+ matorient = matrix(a, b, c, axes=convention)
+
+ image_ref = self.renderer.scene(posorient)
+
+ for convention2 in self.conventions:
+ index2 = pd.MultiIndex.from_tuples(
+ [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention2, 'alpha_0'),
+ (convention2, 'alpha_1'), (convention2, 'alpha_2')])
+ posorient2 = pd.Series(index=index2)
+ posorient2.loc['location'][:] = posorient.loc['location'][:]
+ # An orientation matrix need to be calculated from
+ # the euler angle of the convention of 'reference'
+ # so that it can be decompase in another convention
+ at, bt, ct = from_matrix(matorient, axes=convention2)
+ posorient2.loc[convention2] = [at, bt, ct]
+ image2 = self.renderer.scene(posorient2)
+ np.testing.assert_allclose(image2, image_ref)
+
+ def test_euler_2_quaternion(self):
+ convention2 = 'quaternion'
+ index2 = pd.MultiIndex.from_tuples(
+ [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention2, 'q_0'),
+ (convention2, 'q_1'), (convention2, 'q_2'), (convention2, 'q_3')],
+ names=['position', 'orientation'])
+ posorient2 = pd.Series(index=index2)
+
+ for convention in self.conventions:
+ index = pd.MultiIndex.from_tuples(
+ [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention, 'alpha_0'),
+ (convention, 'alpha_1'), (convention, 'alpha_2')])
+ posorient = pd.Series(index=index)
+ posorient.loc['location']['x'] = 0
+ posorient.loc['location']['y'] = 0
+ posorient.loc['location']['z'] = 1
+ posorient.loc[convention]['alpha_0'] = np.pi/4
+ posorient.loc[convention]['alpha_1'] = np.pi/7
+ posorient.loc[convention]['alpha_2'] = np.pi/3
+
+ a, b, c = posorient.loc[convention]
+ matorient = matrix(a, b, c, axes=convention)
+
+ image_ref = self.renderer.scene(posorient)
+
+ posorient2.loc['location'][:] = posorient.loc['location'][:]
+ # An orientation matrix need to be calculated from
+ # the euler angle of the convention of 'reference'
+ # so that it can be decompase in another convention
+ at, bt, ct, dt = from_quat_matrix(matorient)
+ posorient2.loc[convention2] = [at, bt, ct, dt]
+ image2 = self.renderer.scene(posorient2)
+ np.testing.assert_allclose(image2, image_ref, atol=1.2)
+ """
+ def test_quaternion_2_euler(self):
+ convention = 'quaternion'
+ index = pd.MultiIndex.from_tuples(
+ [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention, 'q_0'),
+ (convention, 'q_1'), (convention, 'q_2'), (convention, 'q_3')],
+ names=['position', 'orientation'])
+ posorient = pd.Series(index=index)
+ posorient = pd.Series(index=index)
+ posorient.loc['location']['x'] = 0
+ posorient.loc['location']['y'] = 0
+ posorient.loc['location']['z'] = 1
+ posorient.loc[convention]['q_0'] = np.pi/4
+ posorient.loc[convention]['q_1'] = np.pi/7
+ posorient.loc[convention]['q_2'] = np.pi/3
+ posorient.loc[convention]['q_3'] = np.pi/2
+
+ a, b, c, d = posorient.loc[convention]
+ matorient = quat_matrix([a, b, c, d])
+
+ image_ref = self.renderer.scene(posorient)
+
+ for convention2 in conventions:
+ index2 = pd.MultiIndex.from_tuples(
+ [('location', 'x'), ('location', 'y'),
+ ('location', 'z'), (convention, 'alpha_0'),
+ (convention, 'alpha_1'), (convention, 'alpha_2')])
+ posorient2.loc['location'][:] = posorient.loc['location'][:]
+
+
+ at, bt, ct = from_quat_matrix(matorient, convention2)
+ posorient2.loc['location'][:] = posorient.loc['location'][:]
+ # An orientation matrix need to be calculated from
+ # the euler angle of the convention of 'reference'
+ # so that it can be decompase in another convention
+ at, bt, ct, dt = from_quat_matrix(matorient)
+ posorient2.loc[convention2] = [at, bt, ct]
+ image2 = self.renderer.scene(posorient2)
+ np.testing.assert_allclose(image2, self.image_ref, atol=1.2)
+ """
diff --git a/navipy/sensors/renderer.py b/navipy/sensors/renderer.py
index 2c998493cfbdd0dec00c574f83cd80bf9b2daa84..f1cb8b97680b5213d38de7c16c36f8f0fb36df59 100644
--- a/navipy/sensors/renderer.py
+++ b/navipy/sensors/renderer.py
@@ -12,6 +12,8 @@ import numpy as np
import tempfile
import os
import pandas as pd
+from navipy.maths.homogeneous_transformations import compose_matrix
+import navipy.maths.constants as constants
class BlenderRender():
@@ -47,7 +49,6 @@ class BlenderRender():
# Filtering props
bpy.context.scene.cycles.filter_type = 'GAUSSIAN'
# Call all set function with default values
- self.camera_rotation_mode = 'XYZ'
self.camera_fov = [[-90, 90], [-180, 180]]
self.camera_gaussian_width = 1.5
self.camera_resolution = [360, 180]
@@ -86,29 +87,6 @@ class BlenderRender():
)
self.tmp_fileoutput = tmp_fileoutput
- @property
- def camera_rotation_mode(self):
- """get the current camera rotation mode
-
- :returns: the mode of rotation used by the camera
- :rtype: string
- """
- return bpy.data.scenes["Scene"].camera.rotation_mode
-
- @camera_rotation_mode.setter
- def camera_rotation_mode(self, mode):
- """change the camera rotation mode
-
- :param mode: the mode of rotation for the camera see blender do
- :type mode: a string
-
- .. seealso: blender bpy.data.scenes["Scene"].camera.rotation_mode
- """
- if not isinstance(mode, str):
-
- raise TypeError('mode must be a string')
- bpy.data.scenes["Scene"].camera.rotation_mode = mode
-
@property
def cycle_samples(self):
"""get the samples for rendering with cycle
@@ -285,28 +263,53 @@ class BlenderRender():
"""assign the position and the orientation of the camera.
:param posorient: is a 1x6 vector containing:
- x,y,z, angle_1, angle_2, angle_3,
+ *in case of euler angeles the index should be
+ ['location']['x']
+ ['location']['y']
+ ['location']['z']
+ [convention][alpha_0]
+ [convention][alpha_1]
+ [convention][alpha_2]
+ **where convention can be:
+ rxyz, rxzy, ryxz, ryzx, rzyx, rzxy
+ *in case of quaternions the index should be
+ ['location']['x']
+ ['location']['y']
+ ['location']['z']
+ [convention]['q_0']
+ [convention]['q_1']
+ [convention]['q_2']
+ [convention]['q_3']
+ **where convention can be:
+ quaternion
here the angles are euler rotation around the axis
specified by scene.camera.rotation_mode
- :type posorient: 1x6 double array
+ :type posorient: pandas Series with multi-index
"""
- print(posorient)
- if (isinstance(posorient, np.ndarray) or
- isinstance(posorient, list)):
-
- if len(posorient) != 6:
- raise Exception('posorient should be a 1x6 double array')
- self.camera.location = posorient[:3]
- self.camera.rotation_euler = posorient[3:]
- elif isinstance(posorient, pd.Series):
- self.camera.location = \
- posorient.loc[['x', 'y', 'z']].values
- self.camera.rotation_euler = \
- posorient.loc[['alpha_0', 'alpha_1', 'alpha_2']].values
+ if isinstance(posorient, pd.Series):
+ # set roation mode
+ conv_found = False
+ index = posorient.index
+ convention = index.get_level_values(0)[-1]
+ for key, _ in constants._AXES2TUPLE.items():
+ if convention == key:
+ conv_found = True
+ break
+ if convention == 'quaternion':
+ conv_found = True
+ if not conv_found:
+ msg = 'The convention {} used for the orientation'
+ msg = msg.format(convention)
+ msg += ' is not supported'
+ raise ValueError(msg)
else:
raise TypeError(
'posorient must be of type array, list, or pandas Series')
# Render
+ self.camera.matrix_world = compose_matrix(
+ angles=posorient.loc[convention],
+ translate=posorient.loc['location'],
+ axes=convention)
bpy.ops.render.render()
def scene(self, posorient):
diff --git a/setup.py b/setup.py
index 69c820f31c680f387ab40911330978d0e967c710..ef697e0974005a80082a6c6ee4aa96c706b40dbe 100644
--- a/setup.py
+++ b/setup.py
@@ -45,7 +45,7 @@ setup_dict = {'name': 'navipy',
'flake8',
'tox'],
'package_data': {'navipy':
- ['resources/database.db',
+ ['resources/*.db',
'resources/*.blend',
'resources/confgis/*.yaml']},
'include_package_data': True,