Merge pull request #2 from qualisys/fix-filter-performance-issue

Fix filter performance issue

.gitignore

@@ -0,0 +1 @@
+Data

Templates/qtm2opensim.py

@@ -13,7 +13,7 @@
 import opensim as osim
 import numpy as np
 # import matplotlib.pyplot as plt # include this when generating plots to verify force zeroing
-from utils import create_opensim_storage, lowess_bell_shape_kern, mm_to_m, rotate_data_table
+from utils import create_opensim_storage, mm_to_m, rotate_data_table, lowpass_filter
 
 parser = argparse.ArgumentParser()
 parser.add_argument('--c3d_dir', required=True, help="Path to c3d file")
@@ -91,9 +91,10 @@ def convert_c3d(c3d_dir, c3d_file):
 
     # interpolate and fit splines to smooth the data
     list_mat = list()
+    sample_rate = 1/(t[1]-t[0])
     for label in labels:
         f = forces.getDependentColumn(label)
-        list_mat.append(lowess_bell_shape_kern(t, f, label, tau=.00005, output_dir=c3d_dir))
+        list_mat.append(lowpass_filter(f, label, sample_rate, order=2, cutoff=10, padtype="odd", output_dir=c3d_dir))
 
     # construct the matrix of the forces (forces, moments, torques / right and left)
     # (type opensim.Matrix)
@@ -102,7 +103,7 @@ def convert_c3d(c3d_dir, c3d_file):
     for n in range(6):
         for j in range(3):
             for i in range(len(t)):
-                forces_task_mat.set(i, 3 * n + j, forces_task_np[n, i, j])
+                forces_task_mat.set(i, 3 * n + j, forces_task_np[n, j, i])
 
     # export forces
     labels_list = ['ground_force_vx', 'ground_force_vy', 'ground_force_vz',

Templates/utils.py

@@ -3,13 +3,10 @@
 # author: Dimitar Stanev <[email protected]>
 # https://github.com/mitkof6/opensim_automated_pipeline
 ##
-import re
-import os
+import scipy.signal
 import opensim
 import numpy as np
-import pandas as pd
-from scipy import linalg
-# import matplotlib.pyplot as plt # include this when generating plots to verify force filter
+import matplotlib.pyplot as plt # include this when generating plots to verify force filter
 
 def rotate_data_table(table, axis, deg):
     """Rotate OpenSim::TimeSeriesTableVec3 entries using an axis and angle.
@@ -42,82 +39,77 @@ def mm_to_m(table, label):
     for i in range(c.size()):
         c[i] = opensim.Vec3(c[i][0] * 0.001, c[i][1] * 0.001, c[i][2] * 0.001)
 
-def lowess_bell_shape_kern(t, v, label, tau=.0005, output_dir='.'):
-    """lowess_bell_shape_kern(t, v, tau = .005) -> vest Locally weighted
-    regression: fits a nonparametric regression curve to a
-    scatterplot.  The arrays t and v contain an equal number of
-    elements; each pair (t[i], v[i,j]) defines a data point in the
-    scatterplot. Depending on j, this corresponds to the x,y or z
-    column of the matrix v. The function returns the estimated
-    (smooth) values of each columns of y in a matrix.  The kernel
-    function is the bell shaped function with parameter tau. Larger
-    tau will result in a smoother curve.
+def get_valid_padlen(signal, A, B):
+    """ if signal is too short the defaultpadlen needs to change in order for the scipy filtfilt to work with padding """
+    padlen = 3 * max(len(A), len(B))  # from scipy default
+    signal_length = len(signal)
+    if signal_length <= padlen:
+        padlen = signal_length - 1
+    return padlen
 
-    """
-    r = len(t)
+def lowpass_filter(signal, label, sampling_freq, order=4, cutoff=12, padtype="odd", output_dir='.'):
+    """ 
+    Given a signal, its sampling frequency in Hz and filter order
 
-    # convert tuple into np.array
-    t_np = np.zeros(r)
-    for i in range(r):
-        t_np[i] = t[i]
+    return the filtered signal
 
+    The defaul filter is a butterworth lowpass filter with given order, which gets applied twice
+    """
+    # instantiate variables
+    n_frames = signal.nrow()
+    signal_np = np.zeros((n_frames))
+    smooth_signal_list = []
+    # filter settings
+    nyq = 0.5 * sampling_freq
+    # The double filter should have 1/sqrt(2) transfer at cutoff, so we need correction for filter order
+    cutoff = cutoff / (np.sqrt(2) - 1) ** (0.5 / order)
+    Wn = cutoff / nyq
+    B, A = scipy.signal.butter(order, Wn, output="ba")
     # convert Vec3 into np.array
-    v_np = np.zeros((r, 3))
-    for i in range(r):
-        v_np[i, 0] = v[i][0]  # extract x component at each time step
-        v_np[i, 1] = v[i][1]
-        v_np[i, 2] = v[i][2]
-
-    # interpolate to replace NaN
-    v_int = np.zeros((r, 3))
-    for j in range(3):
-        v_pd = pd.Series(v_np[:, j])
-        v_pd = v_pd.interpolate(limit_direction="both", kind="cubic")
-        v_int[:, j] = v_pd.to_numpy()
-
-    # initializing all weights from the bell shape kernel function
-    for j in range(3):
-        w = [np.exp(- (t_np - t_np[i])**2/(2*tau)) for i in range(r)]
-
-    # looping through all v-points
-    vest = np.zeros((r, 3))
-    for j in range(3):
-        for i in range(r):
-            weights = w[i]
-            b = np.array([np.sum(weights * v_int[:, j]),
-                          np.sum(weights * v_int[:, j] * t_np)])
-            A = np.array([[np.sum(weights), np.sum(weights * t_np)],
-                          [np.sum(weights * t_np), np.sum(weights * t_np * t_np)]])
-            theta = linalg.solve(A, b)
-            vest[i, j] = theta[0] + theta[1] * t_np[i]
-
-    # validation
+    for i in range(3):
+        for j in range(n_frames):
+            signal_np[j] = signal[j][i]  # extract component at each time step
+
+        # padding
+        padlen = get_valid_padlen(signal_np, A, B)
+
+        # smoothing
+        smooth_signal_list.append(scipy.signal.filtfilt(B, A, signal_np, padtype=padtype, padlen=padlen))
+
+    # validation (uncomment the next block and the figures will be exported in the session folder)
+    # signal_val_np = np.zeros((n_frames, 3))
+    # for i in range(n_frames):
+    #     signal_val_np[i, 0] = signal[i][0]  # extract x component at each time step
+    #     signal_val_np[i, 1] = signal[i][1]  # extract y component at each time step
+    #     signal_val_np[i, 2] = signal[i][2]  # extract z component at each time step
+    # temp = np.array(smooth_signal_list)
+    # smooth_signal_np = temp.transpose()
     # plt.figure()
-    # plt.plot(v_np[:, 0], label='raw')
+    # plt.plot(signal_val_np[:, 0], label='raw')
     # plt.ylabel(label)
     # plt.xlabel('sample')
-    # plt.plot(vest[:, 0], label='filtered')
+    # plt.plot(smooth_signal_np[:, 0], label='filtered')
     # plt.legend()
     # plt.savefig(output_dir + '/{}_x.pdf'.format(label))
 
     # plt.figure()
-    # plt.plot(v_np[:, 1], label='raw')
+    # plt.plot(signal_val_np[:, 1], label='raw')
     # plt.ylabel(label)
     # plt.xlabel('sample')
-    # plt.plot(vest[:, 1], label='filtered')
+    # plt.plot(smooth_signal_np[:, 1], label='filtered')
     # plt.legend()
     # plt.savefig(output_dir + '/{}_y.pdf'.format(label))
 
     # plt.figure()
-    # plt.plot(v_np[:, 2], label='raw')
+    # plt.plot(signal_val_np[:, 2], label='raw')
     # plt.ylabel(label)
     # plt.xlabel('sample')
-    # plt.plot(vest[:, 2], label='filtered')
+    # plt.plot(smooth_signal_np[:, 2], label='filtered')
     # plt.legend()
     # plt.savefig(output_dir + '/{}_z.pdf'.format(label))
     # plt.close('all')
 
-    return vest
+    return np.array(smooth_signal_list)
 
 def create_opensim_storage(time, data, column_names):
     """Creates a OpenSim::Storage.