{"metadata":{"kernelspec":{"language":"python","display_name":"Python 3","name":"python3"},"language_info":{"name":"python","version":"3.10.12","mimetype":"text/x-python","codemirror_mode":{"name":"ipython","version":3},"pygments_lexer":"ipython3","nbconvert_exporter":"python","file_extension":".py"},"kaggle":{"accelerator":"none","dataSources":[{"sourceId":60095,"databundleVersionId":6542333,"sourceType":"competition"}],"dockerImageVersionId":30626,"isInternetEnabled":true,"language":"python","sourceType":"notebook","isGpuEnabled":false}},"nbformat_minor":4,"nbformat":4,"cells":[{"cell_type":"code","source":"import glob\nfrom dataclasses import dataclass\nimport numpy as np\nimport pandas as pd\nfrom tqdm.notebook import tqdm\nfrom scipy.interpolate import InterpolatedUnivariateSpline\nfrom geographiclib.geodesic import Geodesic\n\n# Constants\nINPUT_PATH = '/kaggle/input/smartphone-decimeter-2023'\nWGS84_SEMI_MAJOR_AXIS = 6378137.0\nWGS84_SEMI_MINOR_AXIS = 6356752.314245\nWGS84_SQUARED_FIRST_ECCENTRICITY = 6.69437999013e-3\nWGS84_SQUARED_SECOND_ECCENTRICITY = 6.73949674226e-3\n\n# Read data\ndf_sample_trail_gnss = pd.read_csv(f\"{INPUT_PATH}/sdc2023/train/2020-06-25-00-34-us-ca-mtv-sb-101/pixel4/device_gnss.csv\")\ndf_sample_trail_gt = pd.read_csv(f\"{INPUT_PATH}/sdc2023/train/2020-06-25-00-34-us-ca-mtv-sb-101/pixel4/ground_truth.csv\")\n\n# Data classes\n@dataclass\nclass ECEF:\n    x: np.array\n    y: np.array\n    z: np.array\n\n    def to_numpy(self):\n        return np.stack([self.x, self.y, self.z], axis=0)\n\n    @staticmethod\n    def from_numpy(pos):\n        x, y, z = [np.squeeze(w) for w in np.split(pos, 3, axis=-1)]\n        return ECEF(x=x, y=y, z=z)\n\n@dataclass\nclass BLH:\n    lat: np.array\n    lng: np.array\n    hgt: np.array\n\n# Conversion functions\ndef ECEF_to_BLH(ecef):\n    a = WGS84_SEMI_MAJOR_AXIS\n    b = WGS84_SEMI_MINOR_AXIS\n    e2 = WGS84_SQUARED_FIRST_ECCENTRICITY\n    e2_ = WGS84_SQUARED_SECOND_ECCENTRICITY\n    x = ecef.x\n    y = ecef.y\n    z = ecef.z\n    r = np.sqrt(x**2 + y**2)\n    t = np.arctan2(z * (a / b), r)\n    B = np.arctan2(z + (e2_ * b) * np.sin(t)**3, r - (e2 * a) * np.cos(t)**3)\n    L = np.arctan2(y, x)\n    n = a / np.sqrt(1 - e2 * np.sin(B)**2)\n    H = (r / np.cos(B)) - n\n    return BLH(lat=B, lng=L, hgt=H)\n\n# def haversine_distance(blh_1, blh_2):\n#     dlat = blh_2.lat - blh_1.lat\n#     dlng = blh_2.lng - blh_1.lng\n#     a = np.sin(dlat / 2)**2 + np.cos(blh_1.lat) * np.cos(blh_2.lat) * np.sin(dlng / 2)**2\n#     dist = 2 * HAVERSINE_RADIUS * np.arcsin(np.sqrt(a))\n#     return dist\n\n# def pandas_haversine_distance(df1, df2):\n#     blh1 = BLH(\n#         lat=np.deg2rad(df1['LatitudeDegrees'].to_numpy()),\n#         lng=np.deg2rad(df1['LongitudeDegrees'].to_numpy()),\n#         hgt=0,\n#     )\n#     blh2 = BLH(\n#         lat=np.deg2rad(df2['LatitudeDegrees'].to_numpy()),\n#         lng=np.deg2rad(df2['LongitudeDegrees'].to_numpy()),\n#         hgt=0,\n#     )\n#     return haversine_distance(blh1, blh2)\n\n# Vincenty distance function\ndef vincenty_distance(lat1, lon1, lat2, lon2):\n    distances = []\n    for i in range(len(lat1)):\n        geod = Geodesic.WGS84\n        g = geod.Inverse(lat1[i], lon1[i], lat2[i], lon2[i])\n        distance = g['s12']\n        distances.append(distance)\n    return np.array(distances)\n\n\n# ECEF to Lat/Lng conversion\ndef ecef_to_lat_lng(tripID, gnss_df, UnixTimeMillis):\n    ecef_columns = ['WlsPositionXEcefMeters', 'WlsPositionYEcefMeters', 'WlsPositionZEcefMeters']\n    columns = ['utcTimeMillis'] + ecef_columns\n    ecef_df = (gnss_df.drop_duplicates(subset='utcTimeMillis')[columns]\n               .dropna().reset_index(drop=True))\n    ecef = ECEF.from_numpy(ecef_df[ecef_columns].to_numpy())\n    blh = ECEF_to_BLH(ecef)\n\n    TIME = ecef_df['utcTimeMillis'].to_numpy()\n    lat = InterpolatedUnivariateSpline(TIME, blh.lat, ext=3)(UnixTimeMillis)\n    lng = InterpolatedUnivariateSpline(TIME, blh.lng, ext=3)(UnixTimeMillis)\n    return pd.DataFrame({\n        'tripId': tripID,\n        'UnixTimeMillis': UnixTimeMillis,\n        'LatitudeDegrees': np.degrees(lat),\n        'LongitudeDegrees': np.degrees(lng),\n    })\n\n# Calculation of score\ndef calc_score(tripID, pred_df, gt_df):\n    lat1 = np.deg2rad(pred_df['LatitudeDegrees'].to_numpy())\n    lon1 = np.deg2rad(pred_df['LongitudeDegrees'].to_numpy())\n    lat2 = np.deg2rad(gt_df['LatitudeDegrees'].to_numpy())\n    lon2 = np.deg2rad(gt_df['LongitudeDegrees'].to_numpy())\n\n    distances = vincenty_distance(lat1, lon1, lat2, lon2)\n    score = np.mean([np.quantile(distances, 0.50), np.quantile(distances, 0.95)])\n    return score\n\n# Processing training data\npred_dfs = []\nscore_list = []\nfor dirname in sorted(glob.glob(f'{INPUT_PATH}/sdc2023/train/*/*')):\n    drive, phone = dirname.split('/')[-2:]\n    tripID = f'{drive}/{phone}'\n    gnss_df = pd.read_csv(f'{dirname}/device_gnss.csv')\n    gt_df = pd.read_csv(f'{dirname}/ground_truth.csv')\n    pred_df = ecef_to_lat_lng(tripID, gnss_df, gt_df['UnixTimeMillis'].to_numpy())\n    pred_dfs.append(pred_df)\n    score = calc_score(tripID, pred_df, gt_df)\n    print(f'{tripID:<45}: score = {score:.3f}')\n    score_list.append(score)\nbaseline_train_df = pd.concat(pred_dfs)\nbaseline_train_df.to_csv('baseline_train.csv', index=False)\nmean_score = np.mean(score_list)\nprint(f'mean_score = {mean_score:.3f}')\n\n# Processing test data\nsample_df = pd.read_csv(f'{INPUT_PATH}/sdc2023/sample_submission.csv')\npred_dfs = []\nfor dirname in tqdm(sorted(glob.glob(f'{INPUT_PATH}/sdc2023/test/*/*'))):\n    drive, phone = dirname.split('/')[-2:]\n    tripID = f'{drive}/{phone}'\n    gnss_df = pd.read_csv(f'{dirname}/device_gnss.csv')\n    UnixTimeMillis = sample_df[sample_df['tripId'] == tripID]['UnixTimeMillis'].to_numpy()\n    pred_dfs.append(ecef_to_lat_lng(tripID, gnss_df, UnixTimeMillis))\n","metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","execution":{"iopub.status.busy":"2023-12-28T08:18:48.012731Z","iopub.execute_input":"2023-12-28T08:18:48.013331Z","iopub.status.idle":"2023-12-28T08:23:13.456571Z","shell.execute_reply.started":"2023-12-28T08:18:48.013274Z","shell.execute_reply":"2023-12-28T08:23:13.455396Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"baseline_test_df = pd.concat(pred_dfs)\nbaseline_test_df.to_csv('baseline_test.csv', index=False)\nbaseline_test_df.to_csv('submission.csv', index=False)","metadata":{"execution":{"iopub.status.busy":"2023-12-28T08:23:18.178216Z","iopub.execute_input":"2023-12-28T08:23:18.178644Z","iopub.status.idle":"2023-12-28T08:23:19.440854Z","shell.execute_reply.started":"2023-12-28T08:23:18.178613Z","shell.execute_reply":"2023-12-28T08:23:19.439555Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{},"execution_count":null,"outputs":[]}]}