Reorientation

mobgap/Reorientation/__init__.py

@@ -0,0 +1,7 @@
+"""Algorithms to rotate the signal according to the position of the IMU."""
+
+from mobgap.Reorientation import _filteringsignals_100Hz
+from mobgap.Reorientation._correct_orientation_sensor_axes import CorrectOrientationSensorAxes
+from mobgap.Reorientation._correct_sensor_orientation_dynamic import CorrectSensorOrientationDynamic
+
+__all__ = ["CorrectSensorOrientationDynamic", "CorrectOrientationSensorAxes", "_filteringsignals_100Hz"]

mobgap/Reorientation/_correct_orientation_sensor_axes.py

@@ -0,0 +1,143 @@
+import pandas as pd
+import numpy as np
+import scipy.constants
+from typing_extensions import Self
+from mobgap.pipeline import iter_gs
+from mobgap.Reorientation._correct_sensor_orientation_dynamic import CorrectSensorOrientationDynamic
+from mobgap.gait_sequences import GsdAdaptiveIonescu, GsdIonescu
+from mobgap.Reorientation._filteringsignals_100Hz import filtering_signals_100hz
+from mobgap.initial_contacts._hklee_algo_improved import groupfind
+from mobgap.data_transform import (
+    SavgolFilter,
+    chain_transformers,
+)
+
+
+class CorrectOrientationSensorAxes:
+    """
+
+      Updates the orientation of the IMU data based on the orientation of the sensor.
+      Parameters
+      ----------
+      data
+          Accelerometer and gyroscope data. The data should be in m/s2 and deg/s.
+      sampling_rate_hz
+          Sampling rate of the data in Hz.
+
+      Returns
+      -------
+      corIMUdata
+          Corrected IMU data.
+      corIMUdataSequence
+          Sequence of the corrected IMU data including start and stop of each walking bout.
+
+      Notes
+      -----
+      Deviations from the MATLAB implementation:
+      - th constant was amended to 0.65 * 9.81 to convert it to m/s^2 from g-units
+      - Within the loop MATLAB calculated the 'start' and 'end' samples by multiplying with fs.
+      This is not necessary, since 'start' and 'end' are already in samples.
+      - Simple rotation applied in areas of the signal without a walking bout would only occur if more than
+      2 walking bouts were present. Now this is applied to the whole signal even if walking bouts are less than 2.
+      """
+
+
+    corIMUdata: pd.DataFrame
+    corIMUdataSequence: pd.DataFrame
+
+    def __init__(self, sampling_rate_hz: float):
+        self.sampling_rate_hz = sampling_rate_hz
+
+    def update_orientation(self, data: pd.DataFrame) -> Self:
+        self.data = data
+
+        acc = self.data.loc[:, ["acc_is", "acc_ml", "acc_pa"]]
+
+        self.corIMUdata = self.data.copy()
+        self.corIMUdataSequence = pd.DataFrame(columns=['Start', 'End'])
+
+        # Threshold to test alignment of av with gravity.
+        # In matlab it was 0.65 and used with g-units.
+        # In the present implementation it is converted to m/s^2
+        th = 0.65 * scipy.constants.g
+
+        # parameter for smoothing filter
+        n_sgfilt = 9041
+        accx = acc.loc[:, ["acc_is"]].values
+
+        # Condition to support the minimal signal length required for the filter parameter
+        # low pass filtering of vertical acc
+        if n_sgfilt < len(accx):
+            av_filt = filtering_signals_100hz(accx, 'low', 0.1, sampling_rate=self.sampling_rate_hz)
+
+            savgol_win_size_samples = n_sgfilt
+
+            savgol = SavgolFilter(
+                window_length_s=savgol_win_size_samples / self.sampling_rate_hz,
+                polyorder_rel=1 / savgol_win_size_samples,
+            )
+
+            filter_chain = [("savgol", savgol)]
+
+            av_filt1 = chain_transformers(av_filt, filter_chain, sampling_rate_hz=self.sampling_rate_hz)
+
+            # Output of filter chains should be a pd.DataFrame so the iter_gs function can work
+            av_filt1 = pd.DataFrame(av_filt1, columns=['acc_is'])
+
+            gs = GsdIonescu().detect(data, sampling_rate_hz=self.sampling_rate_hz).gs_list_
+
+            # Adding a specific index to gs so the iter_gs function can work
+            if gs.index.name is None:
+                gs['gs_id'] = range(len(gs))
+            else:
+                if gs.index.name != 'gs_id' and gs.index.name != 'wb_id':
+                    gs['gs_id'] = range(len(gs))
+
+            gsLabel = np.zeros(len(acc.loc[:, "acc_is"]))
+            n = len(gs)
+            k = 0
+
+            if n > 2:
+                for GS, data_slice in iter_gs(av_filt1, gs):
+
+                    gsLabel[GS.start:GS.end] = 1
+
+                    avm = np.mean(data_slice)
+
+                    if avm >= th:
+                        # TODO: check maybe this is not needed because corIMUdata is allready = data so acc is there
+                        self.corIMUdata.iloc[GS.start:GS.end, 0:3] = acc.iloc[GS.start:GS.end, :]
+                    elif avm <= -th:
+                        self.corIMUdata.iloc[GS.start:GS.end, 0] = -acc.iloc[GS.start:GS.end, 0]
+                        self.corIMUdataSequence.loc[k, ['Start', 'End']] = [GS.start, GS.end]
+                        k = k + 1
+                    else:
+                        self.corIMUdata.iloc[GS.start:GS.end, :] = CorrectSensorOrientationDynamic(
+                            self.data.iloc[GS.start:GS.end, :], self.sampling_rate_hz)
+                        self.corIMUdataSequence.loc[k, ['Start', 'End']] = [GS.start, GS.end]
+                        k = k + 1
+
+            ind_noGS = groupfind(gsLabel == 0)
+
+            # ind_noGS should be a pd.DataFrame so the iter_gs function can work
+            ind_noGS = pd.DataFrame(ind_noGS, columns=['start', 'end'])
+
+            # Adding a specific index to ind_noGS so the iter_gs function can work
+            if ind_noGS.index.name is None:
+                ind_noGS['gs_id'] = range(len(ind_noGS))
+            else:
+                if ind_noGS.index.name != 'gs_id' and ind_noGS.index.name != 'wb_id':
+                    ind_noGS['gs_id'] = range(len(ind_noGS))
+
+            for GS, data_slice in iter_gs(av_filt1, ind_noGS):
+                avm = np.mean(data_slice)
+
+                if avm >= th:
+                    self.corIMUdata.iloc[GS.start:GS.end, 0:3] = acc.iloc[GS.start:GS.end, :]
+                elif avm <= -th:
+                    self.corIMUdata.iloc[GS.start:GS.end+1, 0] = -acc.iloc[GS.start:GS.end+1, 0]
+
+        else:
+            return self
+
+        return self

mobgap/Reorientation/_correct_sensor_orientation_dynamic.py

@@ -0,0 +1,184 @@
+import pandas as pd
+import numpy as np
+from scipy.signal import correlate
+from sklearn.decomposition import PCA
+from mobgap.orientation_estimation._madgwick import MadgwickAHRS
+from mobgap.Reorientation._utils import acceleration
+from mobgap.data_transform import (
+    SavgolFilter,
+    chain_transformers,
+)
+
+
+def CorrectSensorOrientationDynamic(data: pd.DataFrame, sampling_rate_hz: float) -> pd.DataFrame:
+
+    """
+    Corrects the orientation of the IMU data based on the orientation of the sensor.
+
+    Parameters
+    ----------
+    data
+        Accelerometer and gyroscope data. The data should be in m/s2 and deg/s.
+    sampling_rate_hz
+        Sampling rate of the data in Hz.
+
+    Returns
+    -------
+    IMU_corrected
+        Corrected IMU data.
+
+    Notes
+    -----
+    Points of deviation from the MATLAB implementation:
+    - The Madgwick filter is applied to the entire data array (walking bout) at once, as opposed to row by row in the
+    original implementation.
+    - The signal is already sliced before calling the present function (CorrectSensorOrientationDynamic).
+    Thus, this script has been simplified by removing a loop and a variable which were not needed.
+    - In MATLAB quaternions are represented as xyzw but python requires wxyz. Thus, the quaternions are adjusted.
+    - Parameters are expressed in the units used in mobgap,specifically, we use m/s^2 instead of g.
+    """
+
+    acc = data.loc[:, ["acc_is", "acc_ml", "acc_pa"]]
+    gyr = data.loc[:, ["gyr_is", "gyr_ml", "gyr_pa"]]
+
+    # Madwick is called with the initial orientation specified inside the Matlab Madgwick function used in the Matlab
+    # implementation (UpdateIMUslt2). This choice was made since the first "initial orientation"
+    # specified in the Matlab implementation is overwritten in the UpdateIMUslt2 function.
+
+    mad = MadgwickAHRS(beta=0.1, initial_orientation=np.array([-0.0863212587840360, -0.639984676042049, 
+                                                               0, 0.763523578361070]))
+
+    chosenacc = acc.copy()
+    chosengyr = gyr.copy()
+
+    # applying the madgwick algo to the entire data array
+    data = pd.concat((chosenacc, chosengyr), axis=1)
+    mad = mad.estimate(data, sampling_rate_hz=sampling_rate_hz)
+    # Slicing quaternion to make it identical in length to the data
+    # this removes the first instance which is the initial orientation
+    quaternion = mad.orientation_.iloc[1:, :]
+
+    quaternion = quaternion.to_numpy()
+
+    # Adjust quaternion from x, y, z, w to w, x, y, z
+    quaternion = quaternion[:, [3, 0, 1, 2]]
+
+    av = acceleration(data, quaternion)
+
+    # Principal component analysis
+    pca_acc = PCA()
+    newacc = pca_acc.fit_transform(chosenacc)
+
+    # newacc to pd dataframe so named access can be used later
+    newacc = pd.DataFrame(newacc, columns=['PC1', 'PC2', 'PC3'])
+
+    pca_gyr = PCA()
+    newgyr = pca_gyr.fit_transform(chosengyr)
+
+    # newgyr to pd dataframe so named access can be used later
+    newgyr = pd.DataFrame(newgyr, columns=['PC1', 'PC2', 'PC3'])
+
+    if newacc['PC1'].mean() < 0:
+        newacc['PC1'] = -newacc['PC1']
+
+    # av_magpca
+    av_magpca = av.copy()
+    av_magpca.loc[:, 'acc_is'] = newacc.loc[:, 'PC1']    # Vertical acceleration is replaced by the first PCA component,
+                                                        # indicating the higher variance (like the vertical acceleration)
+
+    # gyr_magpca
+    gyr_magpca = chosengyr.reset_index(drop=True).copy()    # Resetting index (to remove time) to match the newgyr index
+    gyr_magpca.loc[:, 'gyr_is'] = newgyr.loc[:, 'PC1']        # Yaw: Vertical gyroscope is replaced by the first PCA component,
+                                                            # indicating the higher variance (yaw)
+
+    # Standardisation vectorised
+    av_standardized = ((av - av.mean()) / av.std())
+    newacc_standardized = ((newacc - newacc.mean()) / newacc.std())
+
+    sig1 = av_standardized.loc[:, 'acc_pa']
+    sig2 = newacc_standardized.loc[:, 'PC2']
+    sig3 = newacc_standardized.loc[:, 'PC3']
+    sig4 = av_standardized.loc[:, 'acc_ml']
+
+    # Calculating dot products to compare directionality and magnitude of agreement between different axes
+    # The PCA components are ordered by variance explained: the first component has the highest variance,
+    # followed by the second, and then the third.
+    cor1 = np.dot(sig1, sig2)   # 'agreement' between AP and 2nd PCA component
+    cor2 = np.dot(sig1, sig3)   # 'agreement' between AP and 3rd PCA component
+    cor3 = np.dot(sig3, sig4)   # 'agreement' between 3rd PCA component and ML
+    cor4 = np.dot(sig2, sig4)   # 'agreement' between ML and 2nd PCA component
+
+    if abs(cor1) > abs(cor2):   # AP and 2nd PCA component are more 'aligned' than AP and 3rd PCA component
+        if cor1 > 0:    # AP and ML have same direction
+            av_magpca.loc[:, 'acc_pa'] = newacc.loc[:, 'PC2']  # AP is replaced with 2nd component of PCA
+            gyr_magpca.loc[:, 'gyr_pa'] = newgyr.loc[:, 'PC2']
+        else:   # AP and ML have opposite direction
+            av_magpca.loc[:, 'acc_pa'] = -newacc.loc[:, 'PC2']  # AP is replaced with the neg of the 2nd PCA component
+            gyr_magpca.loc[:, 'gyr_pa'] = newgyr.loc[:, 'PC2']
+
+        if cor3 > 0:  # AP (PCA) and ML have same direction
+            av_magpca.loc[:, 'acc_ml'] = newacc.loc[:, 'PC3']  # ML is replaced with the 3rd component of PCA
+            gyr_magpca.loc[:, 'gyr_ml'] = newgyr.loc[:, 'PC3']
+        else:   # AP (PCA) and ML have opposite direction
+            av_magpca.loc[:, 'acc_ml'] = -newacc.loc[:, 'PC3']  # ML is replaced with the neg of the 3rd PCA component
+            gyr_magpca.loc[:, 'gyr_ml'] = newgyr.loc[:, 'PC3']
+    else:   # AP and AP (following PCA) are more 'aligned' than AP and ML
+        if cor2 > 0:    # AP and AP (PCA) have same direction
+            av_magpca.loc[:, 'acc_pa'] = newacc.loc[:, 'PC3']  # AP is replaced with the 3rd component of PCA
+            gyr_magpca.loc[:, 'gyr_pa'] = newgyr.loc[:, 'PC3']
+        else:   # AP and AP (PCA) have opposite direction
+            av_magpca.loc[:, 'acc_pa'] = -newacc.loc[:, 'PC3']  # AP is replaced with the neg of the 3rd PCA component
+            gyr_magpca.loc[:, 'gyr_pa'] = newgyr.loc[:, 'PC3']
+
+        if cor4 > 0:    # ML and ML (PCA) have same direction
+            av_magpca.loc[:, 'acc_ml'] = newacc.loc[:, 'PC2']  # ML is replaced with 2nd component of PCA
+            gyr_magpca.loc[:, 'gyr_ml'] = newgyr.loc[:, 'PC2']
+        else:   # ML and ML (PCA) have opposite direction
+            av_magpca.loc[:, 'acc_ml'] = -newacc.loc[:, 'PC2']  # ML is replaced with the neg of the 2nd component of PCA
+            gyr_magpca.loc[:, 'gyr_ml'] = newgyr.loc[:, 'PC2']
+    av_pca_final = av_magpca.copy()
+    gyr_pca_final = gyr_magpca.copy()
+
+    #  Savitzky-Golay filter
+    savgol_win_size_samples = 11
+    savgol = SavgolFilter(
+        window_length_s=savgol_win_size_samples / sampling_rate_hz,
+        polyorder_rel=0 / savgol_win_size_samples,
+    )
+
+    filter_chain = [("savgol", savgol)]
+
+    # Filtering vectorised
+    av_pca_filt = np.zeros_like(av_magpca)
+
+    for i in range(av_magpca.shape[1]):
+        av_pca_filt[:, i] = chain_transformers(av_magpca.iloc[:, i], filter_chain, sampling_rate_hz=sampling_rate_hz)
+
+    # Standardization vectorised
+    av_pca_filt_standardized = (av_pca_filt - np.mean(av_pca_filt, axis=0)) / np.std(av_pca_filt, axis=0)
+
+    sig5 = av_pca_filt_standardized[:, 2]
+    sig6 = av_pca_filt_standardized[:, 1]
+    sig7 = av_pca_filt_standardized[:, 0]
+
+    # Cross-correlation
+    r1 = correlate(sig7, sig5)  # Cross-correlation indicating lag of signals, this uses the filtered signals
+    r2 = correlate(sig7, sig6)
+
+    if np.max(r1) > np.max(r2):    # AP and V correlate more strongly than V and ML. Nothing changes
+        av_pca_final.loc[:, "acc_pa"] = av_magpca.loc[:, "acc_pa"]
+        av_pca_final.loc[:, "acc_ml"] = av_magpca.loc[:, "acc_ml"]
+
+        gyr_pca_final.loc[:, "gyr_pa"] = gyr_magpca.loc[:, "gyr_pa"]
+        gyr_pca_final.loc[:, "gyr_ml"] = gyr_magpca.loc[:, "gyr_ml"]
+
+    else:   # V and ML correlate more strongly than AP and V.
+        av_pca_final.loc[:, "acc_pa"] = av_magpca.loc[:, "acc_ml"]   # AP is replaced with ML
+        av_pca_final.loc[:, "acc_ml"] = av_magpca.loc[:, "acc_pa"]
+
+        gyr_pca_final.loc[:, "gyr_pa"] = gyr_magpca.loc[:, "gyr_ml"]
+        gyr_pca_final.loc[:, "gyr_ml"] = gyr_magpca.loc[:, "gyr_pa"]
+
+    IMU_corrected = pd.concat([av_pca_final, gyr_pca_final], axis=1)
+
+    return IMU_corrected