diff --git a/navipy/trajectories/tools.py b/navipy/trajectories/tools.py index 704b206c5247816cf5eac01c95a17587d99fdf53..84a8a29ffe0760cd643f35d50e0042745962818c 100644 --- a/navipy/trajectories/tools.py +++ b/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'): diff --git a/setup.py b/setup.py index fc5d77a9e0c4031e7d717ce1f46c85c1b3b461dd..279282804467aa19554a015536de201189e70680 100644 --- a/setup.py +++ b/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,