{"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":"code","source":"!pip install pymap3d\n\nimport numpy as np\nimport pandas as pd\nimport pymap3d as pm\nimport pymap3d.vincenty as pmv\nimport matplotlib.pyplot as plt\nimport glob as gl\nimport scipy.optimize\nfrom tqdm.auto import tqdm\nfrom scipy.interpolate import InterpolatedUnivariateSpline\nfrom scipy.spatial import distance\nfrom scipy.signal import savgol_filter\nfrom copy import deepcopy\n\nimport pyproj\nfrom pyproj import Proj, transform\n\n# Constants\nCLIGHT = 299_792_458   # speed of light (m/s)\nRE_WGS84 = 6_378_137   # earth semimajor axis (WGS84) (m)\nOMGE = 7.2921151467E-5  # earth angular velocity (IS-GPS) (rad/s)","metadata":{"execution":{"iopub.status.busy":"2022-07-28T15:25:05.734794Z","iopub.execute_input":"2022-07-28T15:25:05.735273Z","iopub.status.idle":"2022-07-28T15:25:22.325693Z","shell.execute_reply.started":"2022-07-28T15:25:05.73518Z","shell.execute_reply":"2022-07-28T15:25:22.324602Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"RTK_FILE = '../input/gsdc22train/puck_rp_RTK.csv'\nWLS_FILE = '../input/gsdc22train/cauchy_train_CauchySoftforXiaomi.csv'","metadata":{"execution":{"iopub.status.busy":"2022-07-28T15:25:22.32802Z","iopub.execute_input":"2022-07-28T15:25:22.328494Z","iopub.status.idle":"2022-07-28T15:25:22.33302Z","shell.execute_reply.started":"2022-07-28T15:25:22.328447Z","shell.execute_reply":"2022-07-28T15:25:22.332347Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Score Computation","metadata":{}},{"cell_type":"code","source":"import glob\nfrom dataclasses import dataclass\nfrom tqdm.notebook import tqdm\nfrom pathlib import Path\nimport numpy as np\nimport pandas as pd\nimport copy\nimport pyproj\nimport plotly.express as px\nimport plotly.graph_objects as go\nimport json\nimport bisect\nimport matplotlib.pyplot as plt\nfrom matplotlib.collections import LineCollection\nfrom matplotlib.colors import ListedColormap, BoundaryNorm\n\nimport warnings\nwarnings.simplefilter('ignore')\npd.set_option('display.max_rows',60)\npd.set_option('display.max_columns',None)\n\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\nHAVERSINE_RADIUS = 6_371_000\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\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\ndef 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\ndef 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)\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\ndef WGS84_to_ECEF(lat, lon, alt):\n    # convert to radians\n    rad_lat = lat * (np.pi / 180.0)\n    rad_lon = lon * (np.pi / 180.0)\n    a    = 6378137.0\n    # f is the flattening factor\n    finv = 298.257223563\n    f = 1 / finv   \n    # e is the eccentricity\n    e2 = 1 - (1 - f) * (1 - f)    \n    # N is the radius of curvature in the prime vertical\n    N = a / np.sqrt(1 - e2 * np.sin(rad_lat) * np.sin(rad_lat))\n    x = (N + alt) * np.cos(rad_lat) * np.cos(rad_lon)\n    y = (N + alt) * np.cos(rad_lat) * np.sin(rad_lon)\n    z = (N * (1 - e2) + alt)        * np.sin(rad_lat)\n    return x, y, z\n\ntransformer = pyproj.Transformer.from_crs(\n    {\"proj\":'geocent', \"ellps\":'WGS84', \"datum\":'WGS84'},\n    {\"proj\":'latlong', \"ellps\":'WGS84', \"datum\":'WGS84'},)\n\ndef ECEF_to_WGS84(x,y,z):\n    lon, lat, alt = transformer.transform(x,y,z,radians=False)\n    return lon, lat, alt\n\ndef calc_score_(tripID, pred_df, gt_df):\n    d = pandas_haversine_distance(pred_df, gt_df)\n    score = np.mean([np.quantile(d, 0.50), np.quantile(d, 0.95)])    \n    return score\n\ndef calc_score_all(bl_train, detail=True):\n    score_list = []\n\n    for tripID in sorted(bl_train.tripId.unique()):\n        gt_df   = pd.read_csv(f'../input/smartphone-decimeter-2022/train/{tripID}/ground_truth.csv')\n        pred_df = bl_train[bl_train.tripId == tripID]\n    \n        score = calc_score_(tripID, pred_df, gt_df)\n        if detail:\n            print(f'{tripID} Score        {score:.4f} [m]')\n        #if score>10:\n        #    print(tripID,score)\n        score_list.append(score)\n\n    mean_score = np.mean(score_list)\n    return mean_score","metadata":{"execution":{"iopub.status.busy":"2022-07-28T15:25:22.390175Z","iopub.execute_input":"2022-07-28T15:25:22.390663Z","iopub.status.idle":"2022-07-28T15:25:23.473706Z","shell.execute_reply.started":"2022-07-28T15:25:22.390625Z","shell.execute_reply":"2022-07-28T15:25:23.472644Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def calc_haversine(lat1, lon1, lat2, lon2):\n    \"\"\"Calculates the great circle distance between two points\n    on the earth. Inputs are array-like and specified in decimal degrees.\n    \"\"\"\n    RADIUS = 6_367_000\n    lat1, lon1, lat2, lon2 = map(np.radians, [lat1, lon1, lat2, lon2])\n    dlat = lat2 - lat1\n    dlon = lon2 - lon1\n    a = np.sin(dlat/2)**2 + \\\n        np.cos(lat1) * np.cos(lat2) * np.sin(dlon/2)**2\n    dist = 2 * RADIUS * np.arcsin(a**0.5)\n    return dist\n\n\n","metadata":{"execution":{"iopub.status.busy":"2022-07-28T15:25:23.568258Z","iopub.execute_input":"2022-07-28T15:25:23.568722Z","iopub.status.idle":"2022-07-28T15:25:23.611239Z","shell.execute_reply.started":"2022-07-28T15:25:23.568685Z","shell.execute_reply":"2022-07-28T15:25:23.610186Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# train_LB2953 = pd.read_csv('../input/smartphone-csv-0606/train_LB2953.csv',index_col=False)\n# train_LB2953 = train_LB2953[train_LB2953.tripId!='2020-08-03-US-MTV-2/GooglePixel5'].reset_index(drop=True)\n\n#时间戳错误的route\nerror_trips = [\n    '2020-08-03-US-MTV-2/GooglePixel5',\n    '2021-04-26-US-SVL-2/XiaomiMi8',\n    '2021-04-26-US-SVL-2/SamsungGalaxyS20Ultra',\n    '2020-07-08-US-MTV-2/GooglePixel4',\n    '2020-09-04-US-MTV-1/GooglePixel4',\n    '2020-06-24-US-MTV-1/GooglePixel4',\n    '2020-06-24-US-MTV-1/GooglePixel4XL',\n    '2020-06-24-US-MTV-2/GooglePixel4',\n    '2020-06-24-US-MTV-2/GooglePixel4XL',\n    '2020-08-03-US-MTV-2/GooglePixel4',\n    '2020-08-03-US-MTV-2/GooglePixel4XL',\n    '2020-09-04-US-MTV-1/GooglePixel4',\n    '2020-12-10-US-SJC-1/GooglePixel',\n    '2021-01-05-US-MTV-2/GooglePixel4',\n    '2021-03-10-US-MTV-1/GooglePixel4XL',\n    '2021-04-08-US-MTV-1/GooglePixel4',\n    '2020-07-08-US-MTV-2/GooglePixel4XL',\n    '2020-12-10-US-SJC-1/GooglePixel4',\n    '2020-12-10-US-SJC-1/GooglePixel4XL',\n    '2021-07-14-US-MTV-1/SamsungGalaxyS20Ultra',\n    '2021-08-04-US-SJC-1/SamsungGalaxyS20Ultra',\n    '2021-08-24-US-SVL-1/SamsungGalaxyS20Ultra',\n    '2021-03-16-US-MTV-3/XiaomiMi8',\n    '2021-04-02-US-SJC-1/XiaomiMi8',\n    '2021-07-14-US-MTV-1/XiaomiMi8',\n    '2021-07-19-US-MTV-1/XiaomiMi8',\n    '2021-08-24-US-SVL-1/XiaomiMi8',\n    '2021-03-16-US-MTV-1/GooglePixel4XL',\n    '2021-03-16-US-MTV-3/SamsungGalaxyS20Ultra',\n    '2021-04-02-US-SJC-1/SamsungGalaxyS20Ultra',\n]\nerror_gt = [\n     '2020-08-06-US-MTV-1',\n     '2020-08-06-US-MTV-2',\n     '2020-11-23-US-MTV-1',\n     '2021-03-16-US-MTV-2',\n     '2021-04-21-US-MTV-2',\n     '2021-04-29-US-MTV-1',\n     '2021-04-29-US-MTV-2',\n     '2021-12-15-US-MTV-1',\n     '2021-12-28-US-MTV-1',\n]\n\n\ntrain_wls  = pd.read_csv(WLS_FILE)\ntrain_wls['trip'] = train_wls['tripId'].apply(lambda x:x.split('/')[0])\ntrain_wls = train_wls[~train_wls.tripId.isin(error_trips)].reset_index(drop=True)\ntrain_wls = train_wls[~train_wls.trip.isin(error_gt)].reset_index(drop=True)\n\ntrain_wls = train_wls.sort_values(by=['tripId','UnixTimeMillis']).reset_index(drop=True)\n\ncalc_score_all(train_wls, detail=False)","metadata":{"execution":{"iopub.status.busy":"2022-07-28T15:29:19.824325Z","iopub.execute_input":"2022-07-28T15:29:19.824716Z","iopub.status.idle":"2022-07-28T15:29:25.147239Z","shell.execute_reply.started":"2022-07-28T15:29:19.824686Z","shell.execute_reply":"2022-07-28T15:29:25.146096Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"rtk_train  = pd.read_csv(RTK_FILE)\nrtk_train = rtk_train[~rtk_train.tripId.isin(error_trips)].reset_index(drop=True)\n\n\nrtk_train = rtk_train.iloc[:,:4]\nrtk_train.columns = ['tripId','UnixTimeMillis','LatitudeDegrees','LongitudeDegrees']\n\nrtk_train['trip'] = rtk_train['tripId'].apply(lambda x:x.split('/')[0])\nrtk_train = rtk_train[~rtk_train.trip.isin(error_gt)].reset_index(drop=True)\nrtk_train = rtk_train[~rtk_train.tripId.isin(error_trips)].reset_index(drop=True)\n\ncalc_score_all(rtk_train, detail=False)","metadata":{"execution":{"iopub.status.busy":"2022-07-28T15:29:27.400874Z","iopub.execute_input":"2022-07-28T15:29:27.401988Z","iopub.status.idle":"2022-07-28T15:29:32.387445Z","shell.execute_reply.started":"2022-07-28T15:29:27.401934Z","shell.execute_reply":"2022-07-28T15:29:32.386434Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"PHONES = ['GooglePixel4', 'GooglePixel4XL', 'GooglePixel5', 'GooglePixel6Pro', 'SamsungGalaxyS20Ultra', 'XiaomiMi8']\nscore_df = pd.DataFrame()\nscore_df['PHONES'] = PHONES\n\ndef calc_score_phones(df, detail=True):\n    df['phoneName']   = df['tripId'].apply(lambda x:x.split('/')[1])\n    phone_score = []\n    for Phone in PHONES:\n        tmp = df[df.phoneName==Phone]\n        score = calc_score_all(tmp, detail=detail)\n        phone_score.append(score)\n        if detail: print(f'{Phone} score:{score}')\n    if detail: print(f'PHONES mean score: {np.mean(phone_score)}')\n    return phone_score\n\nscore_df['rtk'] = calc_score_phones(rtk_train, detail=True)\n","metadata":{"execution":{"iopub.status.busy":"2022-07-28T15:29:35.727901Z","iopub.execute_input":"2022-07-28T15:29:35.728326Z","iopub.status.idle":"2022-07-28T15:29:37.79629Z","shell.execute_reply.started":"2022-07-28T15:29:35.72829Z","shell.execute_reply":"2022-07-28T15:29:37.795113Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"score_df['wls'] = calc_score_phones(train_wls, detail=False)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"score_df","metadata":{},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"for w in np.arange(0.1,0.6,0.01):\n    tmp = train_wls.copy()\n    tmp['LatitudeDegrees']  = (w*train_wls['LatitudeDegrees'].values + (1-w)*rtk_train['LatitudeDegrees'].values)\n    tmp['LongitudeDegrees'] = (w*train_wls['LongitudeDegrees'].values + (1-w)*rtk_train['LongitudeDegrees'].values)\n    score_df[f'wls{w}_rtk{1-w}'] = calc_score_phones(tmp, detail=False)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"score_df.to_csv('wls_rtk.csv', index=False)","metadata":{"trusted":true},"execution_count":null,"outputs":[]}]}