From a1f12f70b3047fe9ccb492ab78ee5db555760cf2 Mon Sep 17 00:00:00 2001
From: "Olivier J.N. Bertrand" <olivier.bertrand@uni-bielefeld.de>
Date: Tue, 19 Jun 2018 17:44:17 +0200
Subject: [PATCH] Merger tra3dpy -> navipy Add plots of frame Add marker
functionality to decompose 3 markers into x,y,z yaw pitch roll
---
navipy/markers/__init__.py | 1 +
navipy/markers/test_markers.py | 132 +++++++++++++
navipy/markers/test_triangle.py | 107 ++++++++++
navipy/markers/tools.py | 41 ++++
navipy/markers/transformations.py | 319 ++++++++++++++++++++++++++++++
navipy/markers/triangle.py | 179 +++++++++++++++++
navipy/plots/__init__.py | 46 +++++
7 files changed, 825 insertions(+)
create mode 100644 navipy/markers/__init__.py
create mode 100644 navipy/markers/test_markers.py
create mode 100644 navipy/markers/test_triangle.py
create mode 100644 navipy/markers/tools.py
create mode 100644 navipy/markers/transformations.py
create mode 100644 navipy/markers/triangle.py
create mode 100644 navipy/plots/__init__.py
diff --git a/navipy/markers/__init__.py b/navipy/markers/__init__.py
new file mode 100644
index 0000000..5bb534f
--- /dev/null
+++ b/navipy/markers/__init__.py
@@ -0,0 +1 @@
+# package
diff --git a/navipy/markers/test_markers.py b/navipy/markers/test_markers.py
new file mode 100644
index 0000000..6ccc2a3
--- /dev/null
+++ b/navipy/markers/test_markers.py
@@ -0,0 +1,132 @@
+import unittest
+import numpy as np
+import pandas as pd
+from tra3dpy.maths.homogeneous_transformations import compose_matrix
+import tra3dpy.markers.transformations as mtf
+from tra3dpy.markers.triangle import Triangle
+
+
+class TestMarkersTransform(unittest.TestCase):
+ def setUp(self):
+ # Build an equilateral triangle
+ self.markers = pd.Series(data=0,
+ index=pd.MultiIndex.from_product(
+ [[0, 1, 2], ['x', 'y', 'z']]))
+ self.markers.loc[(0, 'x')] = -1
+ self.markers.loc[(2, 'y')] = np.sin(np.pi / 3)
+ self.markers.loc[(1, 'y')] = -np.sin(np.pi / 3)
+ self.markers.loc[(1, 'x')] = np.cos(np.pi / 3)
+ self.markers.loc[(2, 'x')] = np.cos(np.pi / 3)
+ self.equilateral = Triangle(self.markers.loc[0],
+ self.markers.loc[1],
+ self.markers.loc[2])
+
+ def random_apex(self):
+ return pd.Series(data=np.random.rand(3),
+ index=['x', 'y', 'z'])
+
+ def test_normalise_vec(self):
+ vec = np.random.rand(10)
+ vec = mtf.normalise_vec(vec)
+ self.assertAlmostEqual(np.linalg.norm(vec), 1)
+
+ def test_triangle2bodyaxis(self):
+ for cmode in ['x-axis=median-from-0',
+ 'y-axis=1-2']:
+ origin, x_axis, y_axis, z_axis = \
+ mtf.triangle2bodyaxis(self.equilateral,
+ mode=cmode)
+ np.testing.assert_allclose(origin, [0, 0, 0],
+ atol=1e-07,
+ err_msg='origin&mode '+cmode)
+ np.testing.assert_allclose(x_axis, [1, 0, 0],
+ atol=1e-07,
+ err_msg='x-axis&mode '+cmode)
+ np.testing.assert_allclose(y_axis, [0, 1, 0],
+ atol=1e-07,
+ err_msg='y-axis&mode '+cmode)
+ np.testing.assert_allclose(z_axis, [0, 0, 1],
+ atol=1e-07,
+ err_msg='z-axis&mode '+cmode)
+
+ def test_triangle2bodyaxis_wrongmode(self):
+ """Certain mode are not supported"""
+ apex0 = self.random_apex()
+ apex1 = self.random_apex()
+ apex2 = self.random_apex()
+ triangle = Triangle(apex0, apex1, apex2)
+ mode = 'Thisisafakemodeandshouldfail'
+ with self.assertRaises(KeyError):
+ mtf.triangle2bodyaxis(triangle, mode)
+
+ def test_triangle2bodyaxis_cross(self):
+ """The cross product of x-axis and y-axis should \
+ be the same than the z-axis"""
+ apex0 = self.random_apex()
+ apex1 = self.random_apex()
+ apex2 = self.random_apex()
+ triangle = Triangle(apex0, apex1, apex2)
+ for mode in mtf._modes:
+ _, x_axis, y_axis, z_axis = \
+ mtf.triangle2bodyaxis(triangle, mode)
+ np.testing.assert_almost_equal(np.cross(x_axis, y_axis), z_axis)
+
+ def test_triangle2bodyaxis_ortho(self):
+ """Determinant of frame should be 1, it is a rotation matrix"""
+ apex0 = self.random_apex()
+ apex1 = self.random_apex()
+ apex2 = self.random_apex()
+ triangle = Triangle(apex0, apex1, apex2)
+ for mode in mtf._modes:
+ _, x_axis, y_axis, z_axis = \
+ mtf.triangle2bodyaxis(triangle, mode)
+ frame = mtf.bodyaxistransformations(
+ x_axis, y_axis, z_axis)
+ np.testing.assert_almost_equal(np.linalg.det(frame), 1)
+
+ def test_markers2decompose(self):
+ apex0 = self.random_apex()
+ apex1 = self.random_apex()
+ apex2 = self.random_apex()
+ euler_axes = 'rxyz'
+ for triangle_mode in mtf._modes:
+ scale, shear, angles, translate, perspective =\
+ mtf.markers2decompose(
+ apex0, apex1, apex2, triangle_mode, euler_axes)
+ np.testing.assert_almost_equal(scale, [1, 1, 1])
+ np.testing.assert_almost_equal(shear, [0, 0, 0])
+ np.testing.assert_almost_equal(perspective, [0, 0, 0, 1])
+
+ def test_twomarker2euler(self):
+ angles = np.pi*(np.random.rand(3) - 0.5)
+ angles[0] *= 2
+ # angle[1] is not multipliy because in range [-pi/2,pi/2]
+ angles[2] *= 2
+ mark0 = pd.Series(data=0, index=['x', 'y', 'z'])
+ for axis_alignement, euler_axes, known_angle in zip(
+ ['x-axis', 'z-axis', 'y-axis'],
+ ['rzyx', 'ryxz', 'rzxy'],
+ [angles[2], angles[2], angles[2]]):
+
+ triangle_mode = 'x-axis=median-from-0'
+ transform = compose_matrix(angles=angles,
+ axes=euler_axes)
+ equilateral = Triangle(self.markers.loc[0],
+ self.markers.loc[1],
+ self.markers.loc[2])
+ equilateral.transform(transform)
+ origin, x_axis, y_axis, z_axis = mtf.triangle2bodyaxis(
+ equilateral, triangle_mode)
+ if axis_alignement == 'x-axis':
+ mark1 = pd.Series(x_axis, index=['x', 'y', 'z'])
+ elif axis_alignement == 'y-axis':
+ mark1 = pd.Series(y_axis, index=['x', 'y', 'z'])
+ elif axis_alignement == 'z-axis':
+ mark1 = pd.Series(z_axis, index=['x', 'y', 'z'])
+ angles_estimate = mtf.twomarkers2euler(
+ mark0, mark1, axis_alignement, known_angle, euler_axes)
+ np.testing.assert_almost_equal(angles, angles_estimate)
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/navipy/markers/test_triangle.py b/navipy/markers/test_triangle.py
new file mode 100644
index 0000000..76a06b9
--- /dev/null
+++ b/navipy/markers/test_triangle.py
@@ -0,0 +1,107 @@
+"""
+"""
+import unittest
+import numpy as np
+import pandas as pd
+from tra3dpy.markers.triangle import Triangle
+
+
+class TestTriangle(unittest.TestCase):
+ @classmethod
+ def setUpClass(cls):
+ cls.apex0 = pd.Series(data=np.random.rand(3),
+ index=['x', 'y', 'z'],
+ dtype=float)
+ cls.apex1 = pd.Series(data=np.random.rand(3),
+ index=['x', 'y', 'z'],
+ dtype=float)
+ cls.apex2 = pd.Series(data=np.random.rand(3),
+ index=['x', 'y', 'z'],
+ dtype=float)
+
+ def test_init_notpandas_first(self):
+
+ with self.assertRaises(TypeError):
+ Triangle(np.random.rand(3), self.apex1, self.apex2)
+
+ def test_init_notpandas_second(self):
+ with self.assertRaises(TypeError):
+ Triangle(self.apex0, np.random.rand(3), self.apex2)
+
+ def test_init_notarray_third(self):
+ with self.assertRaises(TypeError):
+ Triangle(self.apex0, self.apex1, np.random.rand(3))
+
+ def test_init_not3el_first(self):
+ apex_false = pd.Series(index=['x', 'y'])
+ with self.assertRaises(IOError):
+ Triangle(apex_false, self.apex1, self.apex2)
+
+ def test_init_not3el_second(self):
+ apex_false = pd.Series(index=['x', 'y'])
+ with self.assertRaises(IOError):
+ Triangle(self.apex0, apex_false, self.apex2)
+
+ def test_init_not3el_third(self):
+ apex_false = pd.Series(index=['x', 'y'])
+ with self.assertRaises(IOError):
+ Triangle(self.apex0, self.apex1, apex_false)
+
+ def test_init_notfloat_first(self):
+ apex_false = pd.Series(index=['x', 'y', 'z'],
+ dtype=object)
+ with self.assertRaises(IOError):
+ Triangle(apex_false, self.apex1, self.apex2)
+
+ def test_init_notfloat_second(self):
+ apex_false = pd.Series(index=['x', 'y', 'z'],
+ dtype=object)
+ with self.assertRaises(IOError):
+ Triangle(self.apex0, apex_false, self.apex2)
+
+ def test_init_notfloat_third(self):
+ apex_false = pd.Series(index=['x', 'y', 'z'],
+ dtype=object)
+ with self.assertRaises(IOError):
+ Triangle(self.apex0, self.apex1, apex_false)
+
+ def test_init(self):
+ triangle = Triangle(self.apex0, self.apex1, self.apex2)
+ condition = np.allclose(triangle.apexes.apex0, self.apex0)
+ condition &= np.allclose(triangle.apexes.apex1, self.apex1)
+ condition &= np.allclose(triangle.apexes.apex2, self.apex2)
+ self.assertTrue(condition)
+
+ def test_center_of_mass(self):
+ triangle = Triangle(self.apex0, self.apex1, self.apex2)
+ cm = triangle.center_of_mass()
+ cm_test = (self.apex0 + self.apex1 + self.apex2) / 3
+ self.assertTrue(np.allclose(cm, cm_test))
+
+ def test_apexes2vectors(self):
+ """The vectors are from one apex to the other in a directed \
+manner. apex0 -> apex1 -> apex2. So the sum of all vector \
+should be zero
+ """
+ triangle = Triangle(self.apex0, self.apex1, self.apex2)
+ vec = triangle.apexes2vectors()
+ self.assertTrue(np.allclose(vec.sum(axis=1).data, [0, 0, 0]))
+
+ def test_apexes2edges_norm(self):
+ triangle = Triangle(self.apex0, self.apex1, self.apex2)
+ en = triangle.apexes2edges_norm()
+ test_norm = np.sqrt(np.sum((self.apex0 - self.apex1)**2))
+ self.assertAlmostEqual(en.loc[(0, 1)], test_norm)
+
+ def test_medians(self):
+ """The median intersect should be at the middle of an edge
+ """
+ triangle = Triangle(self.apex0, self.apex1, self.apex2)
+ md = triangle.medians()
+ test_apex1 = np.sqrt(np.sum((md.loc[:, 0] - self.apex1)**2))
+ test_apex2 = np.sqrt(np.sum((md.loc[:, 0] - self.apex2)**2))
+ self.assertAlmostEqual(test_apex1, test_apex2)
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/navipy/markers/tools.py b/navipy/markers/tools.py
new file mode 100644
index 0000000..396b94d
--- /dev/null
+++ b/navipy/markers/tools.py
@@ -0,0 +1,41 @@
+"""
+ Tools for markers
+"""
+from scipy import signal
+from scipy.interpolate import interp1d
+
+
+def interpolate_markers(markers, kind='cubic'):
+ """
+ Interpolate marker position where Nan are
+ """
+ columns = markers.columns
+ markers_inter = markers.copy()
+ for col in columns:
+ y = markers_inter.loc[:, col]
+ valid_y = y.dropna()
+ valid_t = valid_y.index
+ nan_y = y.isnull().nonzero()[0]
+
+ func = interp1d(valid_t, valid_y, kind='cubic')
+ markers_inter.loc[nan_y, col] = func(nan_y)
+ return markers_inter
+
+
+def filter_markers(markers, cutfreq, order):
+ if cutfreq < order:
+ raise ValueError(
+ 'cutoffrequency {} can not be lower than order {}'.format(
+ cutfreq, order))
+ nyquist = markers.index.name / 2
+ if cutfreq > nyquist:
+ raise ValueError(
+ 'cutoffrequency {} can not be higher than Nyquist freq {}'.format(
+ cutfreq, nyquist))
+ b, a = signal.butter(order, cutfreq/nyquist)
+
+ columns = markers.columns
+ markers_filt = markers.copy()
+ for col in columns:
+ markers_filt.loc[:, col] = signal.filtfilt(b, a, markers.loc[:, col])
+ return markers_filt
diff --git a/navipy/markers/transformations.py b/navipy/markers/transformations.py
new file mode 100644
index 0000000..4b61734
--- /dev/null
+++ b/navipy/markers/transformations.py
@@ -0,0 +1,319 @@
+"""
+"""
+import numpy as np
+from tra3dpy.markers.triangle import Triangle
+import tra3dpy.maths.homogeneous_transformations as ht
+from scipy.optimize import minimize
+
+_modes = []
+for axel in ['x-axis', 'y-axis', 'z-axis']:
+ for marker in range(3):
+ _modes.append(axel+'=median-from-{}'.format(marker))
+
+for axel in ['x-axis', 'y-axis', 'z-axis']:
+ for marki in range(3):
+ for markj in range(3):
+ if marki == markj:
+ continue
+ _modes.append(axel+'={}-{}'.format(marki, markj))
+
+
+def determine_mode(mode):
+ cmode = mode.split('=')
+ if len(cmode) != 2:
+ raise KeyError('mode should contain one and only one sign =')
+ axes = ['x-axis', 'y-axis', 'z-axis']
+ if cmode[0] not in axes:
+ raise KeyError(
+ 'left side of sign = should be {}, {}, or {}'.format(
+ axes[0], axes[1], axes[2]))
+ axel = cmode[0]
+ cmode = cmode[1].split('-')
+ if len(cmode) == 2:
+ apexes = (int(cmode[0]), int(cmode[1]))
+ method = 'aligned-with'
+ elif len(cmode) == 3:
+ apexes = int(cmode[-1])
+ method = cmode[0]+'-'+cmode[1]
+ return axel, apexes, method
+
+
+def normalise_vec(vec):
+ """Normalise vector when its norm is >0"""
+ # assert np.linalg.norm(vec) > 0, 'vec axis has a null norm'
+ if np.linalg.norm(vec) > 0:
+ vec = vec / np.linalg.norm(vec)
+ else:
+ vec = vec*np.nan
+ return vec
+
+
+def median_from(triangle, axel, marker):
+ """One axel of the body is assumed to be aligned with the median \
+ from an apex of the triangle.
+ """
+ # find next marker
+ next_marker = marker + 1
+ if next_marker > 2:
+ next_marker = 0
+ medians = triangle.medians()
+ vectors = triangle.apexes2vectors()
+ x_axis = medians.loc[:, marker] - triangle.apexes.apex0
+ x_axis = x_axis.loc[['x', 'y', 'z']].values
+ if ((marker, next_marker) == (0, 1)) or \
+ ((marker, next_marker) == (1, 2)) or \
+ ((marker, next_marker) == (2, 0)):
+ vector = vectors.loc[['x', 'y', 'z'], (marker, next_marker)]
+ elif ((next_marker, marker) == (0, 1)) or \
+ ((next_marker, marker) == (1, 2)) or \
+ ((next_marker, marker) == (2, 0)):
+ vector = -vectors.loc[['x', 'y', 'z'], (next_marker, marker)]
+ else:
+ raise KeyError(
+ 'Can not compute median from with apexes {}'.format(marker))
+ z_axis = np.cross(vector.values,
+ x_axis)
+ y_axis = np.cross(z_axis, x_axis)
+ x_axis = normalise_vec(x_axis)
+ y_axis = normalise_vec(y_axis)
+ z_axis = normalise_vec(z_axis)
+ if axel == 'x-axis':
+ return x_axis, y_axis, z_axis
+ elif axel == 'y-axis':
+ return y_axis, z_axis, x_axis
+ elif axel == 'z-axis':
+ return z_axis, x_axis, y_axis
+ else:
+ raise KeyError('{} axis is not supported'.format(axel))
+
+ return x_axis, y_axis, z_axis
+
+
+def aligned_with(triangle, axel, markers):
+ """One axel of the body is assumed to be aligned with one edge of the triangle.
+ """
+ # find third marker
+ third_marker = None
+ for mark in [0, 1, 2]:
+ if mark not in markers:
+ third_marker = mark
+ break
+
+ vectors = triangle.apexes2vectors()
+ if (markers == (0, 1)) or \
+ (markers == (1, 2)) or \
+ (markers == (2, 0)):
+ y_axis = vectors.loc[:, (markers[0], markers[1])]
+ elif (markers[::-1] == (0, 1)) or \
+ (markers[::-1] == (1, 2)) or \
+ (markers[::-1] == (2, 0)):
+ y_axis = - vectors.loc[['x', 'y', 'z'],
+ (markers[1], markers[0])]
+ else:
+ raise KeyError(
+ 'Can not compute aligned-with with apexes {}, {}'.format(
+ markers[0], markers[1]))
+
+ y_axis = y_axis.loc[['x', 'y', 'z']].values
+ if ((markers[0], third_marker) == (0, 1)) or \
+ ((markers[0], third_marker) == (1, 2)) or \
+ ((markers[0], third_marker) == (2, 0)):
+ vector = - vectors.loc[['x', 'y', 'z'],
+ (markers[0], third_marker)]
+ elif ((third_marker, markers[0]) == (0, 1)) or \
+ ((third_marker, markers[0]) == (1, 2)) or \
+ ((third_marker, markers[0]) == (2, 0)):
+ vector = vectors.loc[['x', 'y', 'z'],
+ (third_marker, markers[0])]
+ else:
+ raise KeyError(
+ 'Can not compute aligned-with with apexes {}, {}'.format(
+ markers[0], markers[1]))
+
+ z_axis = np.cross(vector, y_axis)
+ x_axis = np.cross(y_axis, z_axis)
+
+ x_axis = normalise_vec(x_axis)
+ y_axis = normalise_vec(y_axis)
+ z_axis = normalise_vec(z_axis)
+
+ if axel == 'y-axis':
+ return x_axis, y_axis, z_axis
+ elif axel == 'x-axis':
+ return y_axis, z_axis, x_axis
+ elif axel == 'z-axis':
+ return z_axis, x_axis, y_axis
+ else:
+ raise KeyError('{} axis is not supported'.format(axel))
+
+
+def triangle2bodyaxis(triangle, mode):
+ """
+ The center of mass of the triangle is the center of all axis of the body.
+ The triangle may not be always placed in the same relative to \
+the body axis. Therefore several methods can be used to compute \
+the body axis from a three points placed on a body. Those methods \
+are accesible via the mode.
+
+ modes:
+ - 'x-axis=median-from-0'
+ - 'y-axis=1-2'
+ """
+ axel, markers, method = determine_mode(mode)
+ origin = triangle.center_of_mass()
+ if method == 'median-from':
+ x_axis, y_axis, z_axis = median_from(triangle,
+ axel, markers)
+ elif method == 'aligned-with':
+ x_axis, y_axis, z_axis = aligned_with(triangle,
+ axel, markers)
+ else:
+ print('supported modes')
+ print('---------------')
+ for m in _modes:
+ print(m)
+ raise ValueError('mode {} is not supported.'.format(mode))
+ return origin, x_axis, y_axis, z_axis
+
+
+def bodyaxistransformations(x_axis, y_axis, z_axis):
+ frame = np.zeros((4, 4))
+ frame[:3, 0] = x_axis
+ frame[:3, 1] = y_axis
+ frame[:3, 2] = z_axis
+ frame[3, 3] = 1
+ return frame
+
+
+def triangle2homogeous_transform(triangle, triangle_mode):
+ origin, x_axis, y_axis, z_axis = triangle2bodyaxis(triangle, triangle_mode)
+ transform = bodyaxistransformations(x_axis, y_axis, z_axis)
+ transform[:3, 3] = origin
+ return transform
+
+
+def markers2decompose(mark0, mark1, mark2, triangle_mode, euler_axes):
+ triangle = Triangle(mark0, mark1, mark2)
+ transform = triangle2homogeous_transform(triangle, triangle_mode)
+ return ht.decompose_matrix(transform, axes=euler_axes)
+
+
+def markers2euler(mark0, mark1, mark2, triangle_mode, euler_axes):
+ _, angles = markers2posorient(mark0, mark1, mark2,
+ triangle_mode, euler_axes)
+ return angles
+
+
+def markers2translate(mark0, mark1, mark2, triangle_mode, euler_axes):
+ translate, _ = markers2posorient(mark0, mark1, mark2,
+ triangle_mode, euler_axes)
+ return translate
+
+
+def markers2posorient(mark0, mark1, mark2, triangle_mode, euler_axes):
+ _, _, angle, translate, _ = markers2decompose(mark0, mark1, mark2,
+ triangle_mode, euler_axes)
+ return translate, angle
+
+
+def twomarkers2euler_easy_euleraxes(mark0, mark1, axis_alignement,
+ known_angle):
+ """ Determine euler angles from two markers and a known angle
+
+ The functions require a known angle express in the temporary \
+rotation convention.
+
+ The known angle is:
+
+ * rotation around x, for axis alignment x-axis
+ * rotation around y, for axis alignment y-axis
+ * rotation around z, for axis alignment z-axis
+
+ The temporary rotation convention used two determine the euler angles are:
+
+ * rzyx, for axis alignment x-axis
+ * rzxy, for axis alignment y-axis
+ * ryxz, for axis alignment z-axis
+
+ Note: If you know the angle in your rotation convention, then you \
+can run this function through a minimisation procedure with free parameter \
+known angle until you get the desired orientation (see twomarkers2euler)
+
+ """
+ vector = mark1-mark0
+ vector = normalise_vec(vector)
+ if axis_alignement == 'x-axis':
+ axes_convention = 'rzyx'
+ beta = np.arcsin(-vector.z)
+ alpha = np.arctan2(vector.y/np.cos(beta),
+ vector.x/np.cos(beta))
+ gamma = known_angle
+ angles = [alpha, beta, gamma]
+ elif axis_alignement == 'y-axis':
+ axes_convention = 'rzxy'
+ gamma = np.arcsin(vector.z)
+ alpha = np.arctan2(-vector.x/np.cos(gamma),
+ vector.y/np.cos(gamma))
+ beta = known_angle
+ angles = [alpha, gamma, beta]
+ elif axis_alignement == 'z-axis':
+ axes_convention = 'ryxz'
+ gamma = np.arcsin(-vector.y)
+ beta = np.arctan2(vector.x/np.cos(gamma),
+ vector.z/np.cos(gamma))
+ alpha = known_angle
+ angles = [beta, gamma, alpha]
+ else:
+ raise KeyError(
+ 'Axis aligment {} is not supported'.format(
+ axis_alignement))
+ return angles, axes_convention
+
+
+def twomarkers2euler_score(x, mark0, mark1,
+ axis_alignement, known_angles, euler_axes):
+ """ A root mean square score between wished angles and obtained \
+ angles from two markers
+
+ This function is used by twomarkers2euler within the minimisation of scipy
+ """
+ angles, axes_convention = twomarkers2euler_easy_euleraxes(
+ mark0, mark1, axis_alignement, x)
+ matrix = ht.compose_matrix(
+ angles=angles, axes=axes_convention)
+ _, _, angles, _, _ = ht.decompose_matrix(matrix, axes=euler_axes)
+
+ return np.nanmean((angles - known_angles)**2)
+
+
+def twomarkers2euler(mark0, mark1, axis_alignement,
+ known_angle, euler_axes,
+ method='Nelder-Mead', tol=1e-6):
+ """ Determine euler angles from two markers and a known angle.
+
+ The known angle is assumed to be the one around the axis aligment.
+ If euler_axes contains twice the same axis the first rotation axis is used.
+
+ """
+ if axis_alignement not in ['x-axis', 'y-axis', 'z-axis']:
+ raise KeyError(
+ 'Axis aligment {} is not supported'.format(
+ axis_alignement))
+ known_angles = np.nan*np.zeros(3)
+ # Find the first rotation axis
+ index = euler_axes.find(axis_alignement[0])
+ if index < 1:
+ raise KeyError(
+ 'Axis aligment {} can not work with euler_axes {}'.format(
+ axis_alignement, euler_axes))
+ known_angles[index-1] = known_angle
+ args = (mark0, mark1, axis_alignement, known_angles, euler_axes)
+ res = minimize(twomarkers2euler_score, x0=known_angle, args=args,
+ method=method, tol=tol)
+ angles, axes_convention = twomarkers2euler_easy_euleraxes(
+ mark0, mark1, axis_alignement, res.x)
+ # Build rotation matrix and decompse
+ matrix = ht.compose_matrix(
+ angles=angles, axes=axes_convention)
+ _, _, angles, _, _ = ht.decompose_matrix(matrix, axes=euler_axes)
+ return angles
diff --git a/navipy/markers/triangle.py b/navipy/markers/triangle.py
new file mode 100644
index 0000000..737cb9f
--- /dev/null
+++ b/navipy/markers/triangle.py
@@ -0,0 +1,179 @@
+"""
+"""
+import numpy as np
+import pandas as pd
+import matplotlib.pyplot as plt
+import mpl_toolkits.mplot3d as a3
+
+
+class Triangle():
+ """
+ A Triangle is defined by three apexes
+ - apex 0
+ - apex 1
+ - apex 2
+
+ This class provides methods to calculate triangle properties when the \
+ apexes of a triangle are known
+ """
+
+ def __init__(self, apex0, apex1, apex2):
+ msg = 'should be a pandas Series with x,y,z as index'
+ if not isinstance(apex0, pd.Series):
+ raise TypeError('apex0 ' + msg)
+ if not isinstance(apex1, pd.Series):
+ raise TypeError('apex1 ' + msg)
+ if not isinstance(apex2, pd.Series):
+ raise TypeError('apex2 ' + msg)
+ if apex0.shape[0] != 3:
+ raise IOError('apex0 ' + msg)
+ if apex1.shape[0] != 3:
+ raise IOError('apex1 ' + msg)
+ if apex2.shape[0] != 3:
+ raise IOError('apex2 ' + msg)
+ if ('x' not in apex0.index) or \
+ ('y' not in apex0.index) or \
+ ('z' not in apex0.index):
+ raise IOError('apex0 ' + msg)
+ if ('x' not in apex1.index) or \
+ ('y' not in apex1.index) or \
+ ('z' not in apex1.index):
+ raise IOError('apex1 ' + msg)
+ if ('x' not in apex2.index) or \
+ ('y' not in apex2.index) or \
+ ('z' not in apex2.index):
+ raise IOError('apex2 ' + msg)
+ if apex0.dtype != float:
+ raise IOError('apex0 does not contains float')
+ if apex1.dtype != float:
+ raise IOError('apex1 does not contains float')
+ if apex2.dtype != float:
+ raise IOError('apex2 does not contains float')
+
+ # The apexes are stored in a pandas dataframe
+ self.apexes = pd.DataFrame(index=['x', 'y', 'z'],
+ columns=['apex0', 'apex1', 'apex2'],
+ dtype=float)
+ self.apexes.apex0 = apex0
+ self.apexes.apex1 = apex1
+ self.apexes.apex2 = apex2
+
+ def __convert_list2array(self, clist):
+ if isinstance(clist, list):
+ clist = np.array(clist)
+ return clist
+
+ def center_of_mass(self):
+ """center of mass
+
+ :returns: center of mass of the triangle
+ :rtype: pandas series
+
+ """
+ cm = pd.Series(index=['x', 'y', 'z'], name='center_of_mass')
+ cm.x = self.apexes.loc['x', :].mean()
+ cm.y = self.apexes.loc['y', :].mean()
+ cm.z = self.apexes.loc['z', :].mean()
+ return cm
+
+ def apexes2vectors(self):
+ """apexes2vectors return a vector between each apexes
+ the vector originating from apex a and going to apex b, \
+can be access as (a,b), here a and b are the apex index.
+ for example for the vector between apex0 and apex1, \
+the tuple is (0,1)
+
+ :returns: return the vectors between edges
+ :rtype: pandas multiindexed DataFrame
+
+ """
+ vec = pd.DataFrame(data=0,
+ index=['x', 'y', 'z'],
+ columns=pd.MultiIndex.from_tuples([(0, 1),
+ (1, 2),
+ (2, 0)]))
+ vec.name = 'vectors'
+ vec.loc[:, (0, 1)] = self.apexes.apex1 - self.apexes.apex0
+ vec.loc[:, (1, 2)] = self.apexes.apex2 - self.apexes.apex1
+ vec.loc[:, (2, 0)] = self.apexes.apex0 - self.apexes.apex2
+ return vec
+
+ def apexes2edges_norm(self):
+ """apexes2edges_norm return the edges norm.
+
+ the edges are accessed by a tuple (a,b), here a and b \
+are the apex indeces.
+ for example for the edge between apex0 and apex1, the \
+tuple is (0,1)
+
+ :returns: return edges norm
+ :rtype: pandas multindexed Series
+
+ """
+ en = self.apexes2vectors()
+ en = np.sqrt((en**2).sum(axis=0))
+ en.name = 'edges_norm'
+ return en
+
+ def medians(self):
+ """medians
+ :returns: the point on the facing edge of an apex at which the median \
+ emanating from this apex cross the facing edge.
+ :rtype: pd.dataframe
+ """
+ md = pd.DataFrame(index=['x', 'y', 'z'],
+ columns=[0, 1, 2])
+ vec = self.apexes2vectors()
+ md.loc[:, 0] = (vec.loc[:, (1, 2)] / 2) + self.apexes.apex1
+ md.loc[:, 1] = (vec.loc[:, (2, 0)] / 2) + self.apexes.apex2
+ md.loc[:, 2] = (vec.loc[:, (0, 1)] / 2) + self.apexes.apex0
+ return md
+
+ def transform(self, matrix):
+ if not isinstance(matrix, (np.ndarray, np.generic)):
+ raise TypeError('frame should be a numpy array')
+ if len(matrix.shape) != 2:
+ raise TypeError('frame should have 2 dimensions')
+ if not np.all(matrix.shape == [4, 4]):
+ matrix = matrix[:3, :]
+ if not np.all(matrix.shape != [3, 4]):
+ raise TypeError('frame should be a 3x4 or 4x4 matrix')
+ for apexname, row in self.apexes.transpose().iterrows():
+ self.apexes.loc[['x', 'y', 'z'], apexname] = matrix.dot(
+ [row.x, row.y, row.z, 1])[:3]
+
+ def plot(self, ax=plt.gca(),
+ facecolor='k',
+ edgecolor='k',
+ alpha=1,
+ apex_marker=None):
+ vtx = np.zeros((3, 3))
+ vtx[:, 0] = self.apexes.apex0.values
+ vtx[:, 1] = self.apexes.apex1.values
+ vtx[:, 2] = self.apexes.apex2.values
+ tri = a3.art3d.Poly3DCollection([vtx.transpose()])
+ tri.set_facecolor(facecolor)
+ tri.set_edgecolor(edgecolor)
+ tri.set_alpha(alpha)
+ ax.add_collection3d(tri)
+ if apex_marker is not None:
+ for _, row in self.apexes.transpose().iterrows():
+ plt.plot([row.x], [row.y], [row.z], apex_marker)
+ xlim = ax.get_xlim()
+ ylim = ax.get_ylim()
+ zlim = ax.get_zlim()
+ if min(xlim) < min(vtx[0, :]):
+ xlim[0] = min(vtx[0, :])
+ if max(xlim) > max(vtx[0, :]):
+ xlim[1] = max(vtx[0, :])
+ if min(ylim) < min(vtx[1, :]):
+ ylim[0] = min(vtx[1, :])
+ if max(ylim) > max(vtx[1, :]):
+ ylim[1] = max(vtx[1, :])
+ if min(zlim) < min(vtx[2, :]):
+ zlim[0] = min(vtx[2, :])
+ if max(xlim) > max(vtx[2, :]):
+ zlim[1] = max(vtx[2, :])
+ ax.set_xlim(xlim)
+ ax.set_ylim(ylim)
+ ax.set_zlim(zlim)
diff --git a/navipy/plots/__init__.py b/navipy/plots/__init__.py
new file mode 100644
index 0000000..67c0226
--- /dev/null
+++ b/navipy/plots/__init__.py
@@ -0,0 +1,46 @@
+"""
+Ploting tools
+"""
+from matplotlib import pyplot as plt
+from matplotlib.patches import FancyArrowPatch
+from mpl_toolkits.mplot3d import proj3d
+import numpy as np
+
+
+class Arrow3D(FancyArrowPatch):
+ def __init__(self, xs, ys, zs, *args, **kwargs):
+ FancyArrowPatch.__init__(self, (0, 0), (0, 0), *args, **kwargs)
+ self._verts3d = xs, ys, zs
+
+ def draw(self, renderer):
+ xs3d, ys3d, zs3d = self._verts3d
+ xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, renderer.M)
+ self.set_positions((xs[0], ys[0]), (xs[1], ys[1]))
+ FancyArrowPatch.draw(self, renderer)
+
+
+def draw_frame(frame,
+ ax=plt.gca(),
+ mutation_scale=20, lw=3,
+ arrowstyle="-|>", colors=['r', 'g', 'b']):
+ if not isinstance(frame, (np.ndarray, np.generic)):
+ raise TypeError('frame should be a numpy array')
+ if len(frame.shape) != 2:
+ raise TypeError('frame should have 2 dimensions')
+ if not np.all(frame.shape == [4, 4]):
+ frame = frame[:3, :]
+ if not np.all(frame.shape != [3, 4]):
+ raise TypeError('frame should be a 3x4 or 4x4 matrix')
+
+ origin = frame[:, 3]
+ for (i, color) in enumerate(colors):
+ v = frame[:, i]
+ xs = [origin[0], origin[0]+v[0]]
+ ys = [origin[1], origin[1]+v[1]]
+ zs = [origin[2], origin[2]+v[2]]
+ a = Arrow3D(xs, ys, zs,
+ mutation_scale=mutation_scale,
+ lw=lw,
+ arrowstyle=arrowstyle,
+ color=color)
+ ax.add_artist(a)
--
GitLab