{"metadata":{"kernelspec":{"language":"python","display_name":"Python 3","name":"python3"},"language_info":{"pygments_lexer":"ipython3","nbconvert_exporter":"python","version":"3.6.4","file_extension":".py","codemirror_mode":{"name":"ipython","version":3},"name":"python","mimetype":"text/x-python"}},"nbformat_minor":4,"nbformat":4,"cells":[{"cell_type":"markdown","source":"I tried to create features from IMU sensor data for sequence to sequence modelling for local trajectories. The approach is to generate one datapoint per path file and use a single model to train on data from all buildings (Groupkfold cv approach).\n\n### Encoder Input\n1. In AHRS data, calculate q0 / qw component of the unit quaternion\n2. Convert unit quaternion to Rotation matrix\n3. Convert unit quaternion to Euler angles (roll, pitch, yaw angles w.r.t magnetic north - **3 features**\n4. ax,ay,az readings indicate the total specific force on the device inclduing the gravity vector. The raw values are corrupted by sensor noise, hence used a exponentially weighted average to smoothen the output. Similar filter for gyroscope measurements too. \n5. Use the Rotation matrix to convert raw acceleration to linear acceleration.  To cancel the gravity influence, lin_acc = Rotation_matrix * raw_acceleration - gravity (assumed as [0, 0, 9,8] constant). Took only 2D accelerations - lin_ax, lin_ay - **2 features**\n6. Filterted Gyroscope readings, only Gz(yaw) component - **1 feature**\n7. Timestamps of the imu data - **1 features**\n\n\nThese 7 features are to be used as input to encoder. Since number of datapoints varies across path files, I used scipy's [splrep, splev functions](https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.splrep.html) functions to fit spline to pathData for each feature and evaluate spline at N (~100) timestamps per path file. Example plot at end of notebook\n\n### Decoder Input\nThe timestamp of the inferring waypoint relative to first imu data in path file\n\n### Decoder Output\nx_position, y_position, floor values can be used directly or converted to local coordinates (translated with first point as origin)\n\n\n## References\n1. [Quaternion conversion](https://github.com/daniel-s-ingram/self_driving_cars_specialization/blob/master/2_state_estimation_and_localization_for_self_driving_cars/c2m5_assignment_files/rotations.py)\n2. [pytorch seq2seq models](https://github.com/bentrevett/pytorch-seq2seq)","metadata":{}},{"cell_type":"markdown","source":"## Library imports","metadata":{}},{"cell_type":"code","source":"!pip install pickle5","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"import os\nimport gc\nimport csv\nimport glob\nimport pickle\nimport random\nimport collections\nimport numpy as np\nimport pandas as pd\nfrom tqdm import tqdm\nimport pickle5 as pickle\nfrom pathlib import Path\nfrom dataclasses import dataclass\nfrom typing import List, Tuple, Any\n\n## for spline interpolation and evaluation\nfrom scipy.interpolate import splev, splrep\n\n## for ESEKF imports\nimport sys\nsys.path.append('../input/idln-temp-files-version-1/')\nfrom rotations import Quaternion, skew_symmetric\n\n## plotting library\nimport matplotlib.pyplot as plt\nimport seaborn as sns\n\nimport dask\nimport multiprocessing\nfrom dask.distributed import wait\nfrom dask.distributed import Client, wait, LocalCluster","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# set n_workers to number of cores\n## client = Client(n_workers=multiprocessing.cpu_count(), threads_per_worker=1)\n## client","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Configuration parameters","metadata":{}},{"cell_type":"code","source":"floor_map = {\"B2\": -2, \"B1\": -1, \"F1\": 0, \"F2\": 1, \"F3\": 2, \"F4\": 3, \"F5\": 4, \"F6\": 5, \"F7\": 6, \"F8\": 7, \"F9\": 8,\n             \"1F\": 0, \"2F\": 1, \"3F\": 2, \"4F\": 3, \"5F\": 4, \"6F\": 5, \"7F\": 6, \"8F\": 7, \"9F\": 8}\n\nsampleCsvPath = '../input/indoor-location-navigation/sample_submission.csv'\nwaypointData_trainPath = '../input/idln-temp-files-version-1/wayPointData_train.pickle'\nssubm = pd.read_csv(sampleCsvPath)\nssubm_df = ssubm[\"site_path_timestamp\"].apply(lambda x: pd.Series(x.split(\"_\")))\n\noutputDir = '.'\npictureSaveDir = '.'\nACC_COLS  = ['ts', 'ax', 'ay', 'az', 'a_acc']\nGYRO_COLS = ['ts', 'gx', 'gy', 'gz', 'g_acc']\nAHRS_COLS = ['ts', 'qx', 'qy', 'qz', 'q_acc']\n\n## exp weighted moving avg parameter\nsmoothSpan = 10\n\n## gravity vector to calculte linear accelearation\ngravity = np.array([0.0, 0.0, -9.8])\n\n## number of time sequences to give as input to encoder\nimuInputSequenceLength = 100\n\n## max number of time sequences in decoder\nwayPointMaxSequenceLength = 107","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## Helper functions","metadata":{}},{"cell_type":"code","source":"def input_dir() -> Path:\n    return Path('../input/indoor-location-navigation')\n    #return Path('.')\n\ndef generate_target_buildings() -> List[str]:\n    ssubm = pd.read_csv(sampleCsvPath)\n    ssubm_df = ssubm[\"site_path_timestamp\"].apply(\n        lambda x: pd.Series(x.split(\"_\")))\n    buildingsList = sorted(ssubm_df[0].value_counts().index.tolist()) # type: ignore\n    return buildingsList","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def getBuildingPathFiles_test(building):\n    pathFilesTest = list(set(sorted(ssubm_df[ssubm_df[0] == building][1].values.tolist())))\n    buildingPathFilesTest = [f\"{input_dir()}/test/{path}.txt\" for path in pathFilesTest]\n    return buildingPathFilesTest","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def getWayPointData_train():\n    with open(waypointData_trainPath,'rb') as inputFile:\n        waypointData_train = pickle.load(inputFile)\n    return waypointData_train\n\ndef getWayPointCount():\n    wayPointData_train = getWayPointData_train()\n    buildingList = sorted(wayPointData_train.building.unique().tolist())\n    pathList     = sorted(wayPointData_train.path.unique().tolist())\n\n    output = []\n    wayPointBins = [0,5,10,20,84,110]\n    for building, buildingData in wayPointData_train.groupby(by='building'):\n        for path, pathData in buildingData.groupby(by='path'):\n            output.append([building, path, pathData.shape[0]])\n\n    output = pd.DataFrame(output, columns =['building', 'path', 'count'])  \n    output['countBin'] = pd.cut(output['count'], bins=wayPointBins)\n    output.to_pickle('wayPoint_count.pickle')\n    return output","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def extract_IMUData(pathFile):\n    acce, gyro, ahrs = [], [], []\n    with open(pathFile) as f:\n        for line_data in csv.reader(f, delimiter=\"\\t\", doublequote=True):\n            if line_data[1] == 'TYPE_ACCELEROMETER':\n                if len(line_data) > 5:\n                    accuracy = np.int16(line_data[-1])\n                else:\n                    accuracy = np.int16(3)\n                acce.append([np.int64(line_data[0]), np.float32(line_data[2]), np.float32(line_data[3]), np.float32(line_data[4]), accuracy])\n                continue\n\n            elif line_data[1] == 'TYPE_GYROSCOPE':\n                if len(line_data) > 5:\n                    accuracy = np.int16(line_data[-1])\n                else:\n                    accuracy = np.int16(3)               \n                gyro.append([np.int64(line_data[0]), np.float32(line_data[2]), np.float32(line_data[3]), np.float32(line_data[4]), accuracy])\n                continue\n\n            if line_data[1] == 'TYPE_ROTATION_VECTOR':\n                if len(line_data)>5:\n                    accuracy = np.int16(line_data[-1])\n                else:\n                    accuracy = np.int16(3)\n                if len(line_data)>=5:        \n                    ahrs.append([np.int64(line_data[0]), np.float32(line_data[2]), np.float32(line_data[3]), np.float32(line_data[4]), accuracy])\n                continue\n\n    ## sort data by timestamps\n    acce = sorted(acce, key=lambda x: x[0])\n    gyro = sorted(gyro, key=lambda x: x[0])\n    ahrs = sorted(ahrs, key=lambda x: x[0])\n    \n    acce = pd.DataFrame(acce, columns = ACC_COLS)\n    gyro = pd.DataFrame(gyro, columns = GYRO_COLS)\n    ahrs = pd.DataFrame(ahrs, columns = AHRS_COLS)\n    return acce, gyro, ahrs","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def get_qw(qx,qy,qz):\n    qw = 0.0\n    temp = 1 - (qx**2 + qy**2 + qz**2)\n    if temp > 0.0:\n        qw = np.sqrt(temp)\n    return qw\n    \n\ndef convertQuat(ahrsData):\n    numRows = ahrsData.shape[0]\n    rotMatList= []\n    rollList, pitchList, yawList = [], [], []    \n    for row in range(numRows):\n        quat = Quaternion(w=ahrsData['qw'][row], x=ahrsData['qx'][row],\\\n                          y=ahrsData['qy'][row], z=ahrsData['qz'][row])\n        eulerAngles = np.float64(quat.to_euler())\n        \n        ## add to output\n        rotMatList.append(np.float64(quat.to_mat()))\n        rollList.append(eulerAngles[0])\n        pitchList.append(eulerAngles[1])\n        yawList.append(eulerAngles[2])\n    return rotMatList, rollList, pitchList, yawList\n\ndef processAHRSData(ahrs):\n    ahrs['qw'] = ahrs.apply(lambda row : get_qw(row['qx'], row['qy'], row['qz']), axis=1) \n    ahrs['rotMat'], ahrs['roll'], ahrs['pitch'], ahrs['yaw'] = convertQuat(ahrs)\n    ahrs = ahrs.drop(columns=['qw', 'qx', 'qy', 'qz'])\n    return ahrs","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def acceVector(ax,ay,az):\n    return np.array([ax,ay,az])\n\ndef getLinearAccFromRawAcc(rotMatrix, rawAcc):\n    numRows = rotMatrix.shape[0]\n    linAcc_x, linAcc_y = [], []\n    for row in range(numRows):\n        linearAcceleration = (rotMatrix[row] @ rawAcc[row]) + gravity\n        linAcc_x.append(linearAcceleration[0])\n        linAcc_y.append(linearAcceleration[1])\n    return linAcc_x, linAcc_y\n\ndef processAcceData(acceData, ahrsData):\n    acceData['ax_s'] = acceData['ax'].ewm(span=smoothSpan, adjust=True).mean()\n    acceData['ay_s'] = acceData['ay'].ewm(span=smoothSpan, adjust=True).mean()\n    acceData['az_s'] = acceData['az'].ewm(span=smoothSpan, adjust=True).mean()\n    acceData['acc'] = acceData.apply(lambda row : acceVector(row['ax_s'], row['ay_s'], row['az_s']), axis=1)\n    acceData['lin_ax'], acceData['lin_ay'] = getLinearAccFromRawAcc(ahrsData['rotMat'], acceData['acc'])\n    return acceData","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def processGyroData(gyroData):\n    gyroData['gz_s'] = gyroData['gz'].ewm(span=smoothSpan, adjust=True).mean()\n    return gyroData","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def selectCommonData(acceData, gyroData, ahrsData):\n    if((len(acceData) == len(gyroData)) and (len(gyroData)== len(ahrsData)) and (len(acceData)== len(ahrsData))):\n        return acceData, gyroData, ahrsData    \n    else:\n        acceTsSet = set(acceData.ts.values.tolist())\n        gyroTsSet = set(gyroData.ts.values.tolist())\n        ahrsTsSet = set(ahrsData.ts.values.tolist())\n        commonTs = sorted(list(acceTsSet.intersection(gyroTsSet.intersection(ahrsTsSet))))\n\n        acceData = acceData[acceData['ts'].isin(commonTs)].reset_index(drop=True)\n        gyroData = gyroData[gyroData['ts'].isin(commonTs)].reset_index(drop=True)\n        ahrsData = ahrsData[ahrsData['ts'].isin(commonTs)].reset_index(drop=True)\n        return acceData, gyroData, ahrsData\n\ndef getIMUData(acceData, gyroData, ahrsData):\n    ahrsData = processAHRSData(ahrsData);\n    acceData = processAcceData(acceData, ahrsData);\n    gyroData = processGyroData(gyroData);\n    imuData = pd.concat([acceData[['ts','lin_ax', 'lin_ay']], gyroData[['gz_s']], \\\n                         ahrsData[['roll', 'pitch', 'yaw']]], axis=1);\n    return imuData","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def getPathImuInput(imuData):\n    pathInitialTime = imuData['ts'].values[0]\n    ## 7 rows and 100 columns, each row represents each feature\n    pathImuInput = np.zeros((7, imuInputSequenceLength))\n    \n    ## calculating sampling frequency for path\n    numRowsInPath = imuData.shape[0]\n    samplingFactor = int(np.floor_divide(numRowsInPath, imuInputSequenceLength))\n\n    ## unix time to seconds\n    imuData['ts'] = (imuData['ts'] - pathInitialTime) / 1000.0    \n\n    ## new sampled timestamps\n    ## if timestamps is less than 100 take full row\n    if samplingFactor > 0:\n        newTs = imuData['ts'].values[::samplingFactor][0:imuInputSequenceLength]\n    else:\n        newTs = imuData['ts'].values\n    pathImuInput[0, 0:len(newTs)] = newTs\n    \n    ## fitting spline for each feature\n    for index,col in enumerate(['lin_ax', 'lin_ay', 'gz_s', 'roll', 'pitch', 'yaw']):\n        spl = splrep(imuData['ts'].values, imuData[col].values)\n        fitSpline = splev(newTs, spl)\n        pathImuInput[index+1, 0:len(newTs)] = fitSpline\n        \n    return pathImuInput, pathInitialTime","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def plotSplineFitOutput(pathName, imuData, pathImuInput, saveFig=False):\n    fig, axes = plt.subplots(nrows=6, ncols=1, figsize=(20, 20))\n    for index,col in enumerate(['lin_ax', 'lin_ay', 'gz_s', 'roll', 'pitch', 'yaw']):\n        axes[index].plot(imuData['ts'], imuData[col], label=f\"orig_{col}\")\n        axes[index].plot(pathImuInput[0], pathImuInput[index+1], label=f\"fit_{col}\")\n        axes[index].legend(loc='best')\n    if saveFig == True:\n        plt.savefig(f\"{pictureSaveDir}/{pathName}_splineFitOutput.png\", dpi=200)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def getDecoderData(pathName, floorString):\n    ## output variables\n    decoderTs = np.zeros(wayPointMaxSequenceLength)\n    wayPointOutput = np.zeros((wayPointMaxSequenceLength, 3))\n\n    pathWayPointData = wayPointData_train[wayPointData_train.path == pathName].reset_index(drop=True)\n    numWaypoints = pathWayPointData.shape[0]    \n\n    ## get inference timestamps and store in decoderTs variable\n    inferenceTs = (pathWayPointData['timestamp'].values - pathInitialTime) / 1000.0\n    decoderTs[0:numWaypoints] = inferenceTs\n\n    ## get local position information\n    initialWayPoint = pathWayPointData.loc[0,['x', 'y']].values.astype(np.float64)\n    localWayPoints  = pathWayPointData.loc[:, ['x','y']].values.astype(np.float64) - initialWayPoint\n    globalWayPoints = pathWayPointData.loc[:, ['x','y']].values.astype(np.float64)\n    \n    \n    wayPointOutput[0:numWaypoints, 0:2] = localWayPoints   ## globalWayPoints\n    wayPointOutput[0:numWaypoints, 2] = floor_map[folder.name]\n\n    return decoderTs, wayPointOutput, numWaypoints","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"buildingsList = generate_target_buildings()\nwayPointData_train = getWayPointData_train()","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"pathNameList = []\nbuildingList = []\nencoderDataList = []\ndecoderDataList = []\ninferenceTsList = []\nnumWayPointsList = []\npathInitialTimeList = []\n\nfor index,building in enumerate(buildingsList):\n    print(f\"{index+1} bdg = {building} -----------------\")\n    building_path = input_dir() / 'train' / building\n    folders = sorted(building_path.glob('*'))\n    ## print(f\"There are {len(list(folders))} floors in building\")   \n    \n    ## iterate through each floor \n    for folder in folders:\n        floorFiles = sorted(folder.glob(\"*.txt\"))\n        ## iterate through each path file\n        for pathFile in tqdm(floorFiles):\n            pathName = pathFile.name.split('.')[0]\n            acceData, gyroData, ahrsData = extract_IMUData(pathFile)\n            acceData, gyroData, ahrsData = selectCommonData(acceData, gyroData, ahrsData)\n            \n            ## get encoder data from imu input\n            imuData = getIMUData(acceData, gyroData, ahrsData)\n            pathImuInput, pathInitialTime = getPathImuInput(imuData)\n            plotSplineFitOutput(pathName, imuData,pathImuInput, saveFig=False) \n\n            ## get decoder data from waypoint data\n            inferenceTs, decoderInput, numWayPoints = getDecoderData(pathName, folder.name)\n            \n            ## store output to list\n            pathNameList.append(pathName)\n            buildingList.append(building)\n            encoderDataList.append(pathImuInput)\n            decoderDataList.append(decoderInput)\n            inferenceTsList.append(inferenceTs)\n            numWayPointsList.append(numWayPoints)\n            pathInitialTimeList.append(pathInitialTime)\n            \n            break\n        break\n    break","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"imuDataOutput = pd.DataFrame()\nimuDataOutput['path'] = pathNameList\nimuDataOutput['building'] = buildingList\nimuDataOutput['encoderData'] = encoderDataList\nimuDataOutput['decoderData'] = decoderDataList\nimuDataOutput['inferenceTsList'] = inferenceTsList\nimuDataOutput['numWayPoints'] = numWayPointsList\nimuDataOutput['pathInitialTime'] = pathInitialTimeList\nimuDataOutput.to_pickle(f\"imuSeq2SeqDataLocal_train_32.pickle\")\nprint(imuDataOutput.shape)\nimuDataOutput.head()","metadata":{"trusted":true},"execution_count":null,"outputs":[]}]}