Add correction matrix

The markers on the animal may not always be perfectly position relative to the anatomy
A correction matrix (4x4) can now be used to do that

navipy/trajectories/__init__.py

@@ -37,7 +37,7 @@ def velocities_columns(convention):
     toreturn = []
     # Prepend d on dimention for derivative
     for it1, it2 in posorient_columns(convention):
-        toreturn.append((it1, 'd'+it2))
+        toreturn.append((it1, 'd' + it2))
     return toreturn
 
 
@@ -47,8 +47,9 @@ def _markers2position(x, kwargs):
     mark2 = pd.Series(x[6:], index=['x', 'y', 'z'])
     triangle_mode = kwargs['triangle_mode']
     euler_axes = kwargs['euler_axes']
+    correction = kwargs['correction']
     return markers2translate(mark0, mark1, mark2,
-                             triangle_mode, euler_axes)
+                             triangle_mode, euler_axes, correction)
 
 
 def _markers2angles(x, kwargs):
@@ -57,8 +58,9 @@ def _markers2angles(x, kwargs):
     mark2 = pd.Series(x[6:], index=['x', 'y', 'z'])
     triangle_mode = kwargs['triangle_mode']
     euler_axes = kwargs['euler_axes']
+    correction = kwargs['correction']
     return markers2euler(mark0, mark1, mark2,
-                         triangle_mode, euler_axes)
+                         triangle_mode, euler_axes, correction)
 
 
 def _markerstransform(index_i, trajectory,
@@ -474,7 +476,7 @@ class Trajectory(pd.DataFrame):
                 raise KeyError('df should contains q_2 or alpha_2')
         return self
 
-    def from_markers(self, markers, triangle_mode,
+    def from_markers(self, markers, triangle_mode, correction=np.eye(4),
                      error=None, markers_labels=[0, 1, 2]):
         indeces = markers.index
         # Reinit the pandas dataframe super class
@@ -492,7 +494,8 @@ class Trajectory(pd.DataFrame):
         mark2 = markers2use.loc[:, markers_labels[2]]
         x = np.zeros(9)  # 3points with x,y,z
         kwargs = {'triangle_mode': triangle_mode,
-                  'euler_axes': self.rotation_mode}
+                  'euler_axes': self.rotation_mode,
+                  'correction': correction}
         for index_i in markers2use.index:
             # Assign mark to pos
             x[0:3] = mark0.loc[index_i, ['x', 'y', 'z']].values

navipy/trajectories/transformations.py

@@ -192,27 +192,45 @@ def triangle2homogeous_transform(triangle, triangle_mode):
     return transform
 
 
-def markers2decompose(mark0, mark1, mark2, triangle_mode, euler_axes):
+def markers2decompose(mark0, mark1, mark2, triangle_mode,
+                      euler_axes, correction=np.eye(4)):
+    """ Decompose matrix formed by markers:
+    mark0,mark1,mark2
+
+    according to triangle_mode and euler_axes
+
+    homogeneous transformation is corrected by the 4x4coorection matrix as:
+
+    T -> CT
+    T: homogeneous_transformations (from markers)
+    C: correction matrix (default identity, no correction)
+    """
     triangle = Triangle(mark0, mark1, mark2)
     transform = triangle2homogeous_transform(triangle, triangle_mode)
+    transform = correction.dot(transform)
     return ht.decompose_matrix(transform, axes=euler_axes)
 
 
-def markers2euler(mark0, mark1, mark2, triangle_mode, euler_axes):
+def markers2euler(mark0, mark1, mark2, triangle_mode,
+                  euler_axes, correction=np.eye(4)):
     _, angles = markers2posorient(mark0, mark1, mark2,
-                                  triangle_mode, euler_axes)
+                                  triangle_mode, euler_axes, correction)
     return angles
 
 
-def markers2translate(mark0, mark1, mark2, triangle_mode, euler_axes):
+def markers2translate(mark0, mark1, mark2, triangle_mode,
+                      euler_axes, correction=np.eye(4)):
     translate, _ = markers2posorient(mark0, mark1, mark2,
-                                     triangle_mode, euler_axes)
+                                     triangle_mode, euler_axes, correction)
+
     return translate
 
 
-def markers2posorient(mark0, mark1, mark2, triangle_mode, euler_axes):
+def markers2posorient(mark0, mark1, mark2, triangle_mode,
+                      euler_axes, correction=np.eye(4)):
     _, _, angle, translate, _ = markers2decompose(mark0, mark1, mark2,
-                                                  triangle_mode, euler_axes)
+                                                  triangle_mode, euler_axes,
+                                                  correction)
     return translate, angle