Add average trajectories with dtw

navipy/trajectories/tools.py

@@ -6,6 +6,52 @@ from scipy.interpolate import interp1d
 import pandas as pd
 import numpy as np
 import navipy.tools as tratools
+import fastdtw
+from navipy.trajectories import Trajectory
+
+
+def averaged_trajectory(alltraj):
+    """ Calculate an average trajectory
+
+    Trajectories are aligned by using dynamic time wraping (dtw). \
+The alignment is relative to the first trajectory in the list. Therefore \
+the average trajectory will have the same length that the first trajectory.
+
+    Aligned trajectory are aligned as follow. The mean of average position \
+corresponding to the time of the first trajectory devided by the number of \
+trajectory are summed over all trajectory. For the orientation, the circular \
+mean is used according to `Statistics on Sphere` Geoffrey S. Watson 1983.
+
+    :param alltraj: A list of trajectories
+    :returns: An average trajectory
+    """
+    rotconv = alltraj[0].rotation_mode
+    traj_1 = alltraj[0].dropna().values
+    avg_traj = traj_1.copy()
+    avg_traj[:, 0:3] /= len(alltraj)
+    for traj_2 in alltraj[1:]:
+        traj_2 = traj_2.dropna().values
+        x = np.array(traj_1[:, [0, 1, 2]], dtype='float')
+        y = np.array(traj_2[:, [0, 1, 2]], dtype='float')
+        test = fastdtw.fastdtw(x, y)
+
+        # average
+        for ii, group in pd.DataFrame(test[1]).groupby(0):
+            # average position
+            avg_traj[ii, 0:3] += np.mean(traj_2[group.loc[:, 1],
+                                                0:3], axis=0)/len(alltraj)
+            # average angles
+            # see Statistics On Spheres", Geoffrey S. Watson,
+            # University of Arkansas Lecture Notes
+            # in the Mathematical Sciences, 1983 John Wiley & Son
+            for kk in range(3, 6):
+                sinsum = np.sum(np.sin(traj_2[group.loc[:, 1], kk]), axis=0)
+                cossum = np.sum(np.cos(traj_2[group.loc[:, 1], kk]), axis=0)
+                sinsum += np.sin(avg_traj[ii, kk])
+                cossum += np.cos(avg_traj[ii, kk])
+                avg_traj[ii, kk] = np.arctan2(sinsum, cossum)
+
+    return Trajectory().from_array(avg_traj, rotconv=rotconv)
 
 
 def interpolate_markers(markers, kind='cubic'):

setup.py

@@ -50,7 +50,8 @@ setup_dict = {'name': 'navipy',
                            'ipython',
                            'yaml',
                            'PIL',
-                           'cv2'],
+                           'cv2',
+                           'fastdtw'],
               'install_requires': ["numpy",
                                    'pandas',
                                    'matplotlib',
@@ -66,7 +67,8 @@ setup_dict = {'name': 'navipy',
                                    'tables',
                                    'nbsphinx',
                                    'opencv-python',
-				   'coverage'],
+                                   'coverage',
+                                   'fastdtw'],
               'package_data': {'navipy':
                                package_data_files("navipy")},
               'include_package_data': True,