{"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 osmnx momepy geopandas astropy","metadata":{"execution":{"iopub.status.busy":"2021-07-29T05:56:38.879924Z","iopub.execute_input":"2021-07-29T05:56:38.880494Z","iopub.status.idle":"2021-07-29T05:56:46.274109Z","shell.execute_reply.started":"2021-07-29T05:56:38.880458Z","shell.execute_reply":"2021-07-29T05:56:46.272725Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"import numpy as np # linear algebra\nimport pandas as pd # data processing, CSV file I/O (e.g. pd.read_csv)\nimport os\nimport scipy.optimize as opt\nimport matplotlib.pyplot as plt\nfrom pathlib import Path\nfrom sklearn.neighbors import KDTree\nfrom tqdm import tqdm\nimport datetime \nimport pymap3d as pm\nfrom astropy.time import Time\nimport scipy.interpolate\nimport scipy.sparse\n\n\nfrom pathlib import Path\nfrom shapely.geometry import Polygon, Point, LineString, shape\nimport osmnx as ox\nimport momepy\nimport geopandas as gpd\nimport plotly.express as px\n\nimport heapq\nfrom heapq import heappush as push_\nfrom heapq import heappop  as pop_\n\nfrom sklearn.neighbors import KDTree\nimport pickle\nfrom scipy.spatial import distance_matrix\n\nimport pyproj\nfrom pyproj import Proj, transform\n\npd.options.display.max_rows    = 500\npd.options.display.max_columns = 500","metadata":{"execution":{"iopub.status.busy":"2021-07-29T05:56:46.276419Z","iopub.execute_input":"2021-07-29T05:56:46.276798Z","iopub.status.idle":"2021-07-29T05:56:49.470386Z","shell.execute_reply.started":"2021-07-29T05:56:46.27676Z","shell.execute_reply":"2021-07-29T05:56:49.469307Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def show_gt(target_gt_df, lat0=None, lng0=None, index_col='index'):\n    if lat0 is None:\n        lat0, lng0 = target_gt_df[['latDeg', 'lngDeg']].values[len(target_gt_df) // 2]\n\n    fig = px.scatter_mapbox(target_gt_df,\n\n                        # Here, plotly gets, (x,y) coordinates\n                        lat  = \"latDeg\",\n                        lon  = \"lngDeg\",\n                        text ='phoneName',\n\n                        #Here, plotly detects color of series\n                        color  = \"collectionName\",\n                        labels = \"collectionName\",\n                        hover_name = index_col,\n                            \n                        zoom   = 15,\n                        center = {\"lat\":lat0, \"lon\":lng0},\n                        height = 600,\n                        width  = 800)\n\n    fig.update_layout(mapbox_style='stamen-terrain')\n    fig.update_layout(margin={\"r\": 0, \"t\": 0, \"l\": 0, \"b\": 0})\n    fig.update_layout(title_text=\"GPS trafic\")\n    fig.show()\n    \nSPEED_OF_LIGHT      = 2.99792458e8  # m/s\nEARTH_GM            =      3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)\nEARTH_RADIUS        = 6.3781e6  # m\nEARTH_ROTATION_RATE = 7.2921151467e-005\nLeapSecond          = 18\n\ndef ecef2lla(x, y, z):\n    # x, y and z are scalars or vectors in meters\n    x = np.array([x]).reshape(np.array([x]).shape[-1], 1)\n    y = np.array([y]).reshape(np.array([y]).shape[-1], 1)\n    z = np.array([z]).reshape(np.array([z]).shape[-1], 1)\n\n    a    = 6378137\n    a_sq = a**2\n    e    = 8.181919084261345e-2\n    e_sq = 6.69437999014e-3\n\n    f = 1/298.257223563\n    b = a*(1-f)\n\n    # calculations:\n    r = np.sqrt(x**2 + y**2)\n    ep_sq  = (a**2-b**2)/b**2\n    ee = (a**2-b**2)\n    f = (54*b**2)*(z**2)\n    g = r**2 + (1 - e_sq)*(z**2) - e_sq*ee*2\n    c = (e_sq**2)*f*r**2/(g**3)\n    s = (1 + c + np.sqrt(c**2 + 2*c))**(1/3.)\n    p = f/(3.*(g**2)*(s + (1./s) + 1)**2)\n    q = np.sqrt(1 + 2*p*e_sq**2)\n    r_0 = -(p*e_sq*r)/(1+q) + np.sqrt(0.5*(a**2)*(1+(1./q)) - p*(z**2)*(1-e_sq)/(q*(1+q)) - 0.5*p*(r**2))\n    u = np.sqrt((r - e_sq*r_0)**2 + z**2)\n    v = np.sqrt((r - e_sq*r_0)**2 + (1 - e_sq)*z**2)\n    z_0 = (b**2)*z/(a*v)\n    h = u*(1 - b**2/(a*v))\n    phi = np.arctan((z + ep_sq*z_0)/r)\n    lambd = np.arctan2(y, x)\n\n    return phi*180/np.pi, lambd*180/np.pi, h\n\n\ndef 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    \n    return dist\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 get_train_score(df, latCol='latDeg', lngCol='lngDeg'):\n    def percentile50(x):\n        return np.percentile(x, 50)\n    \n    def percentile95(x):\n        return np.percentile(x, 95)\n\n    # calc_distance_error\n    df['err']    = calc_haversine(df['latDeg_truth'], df['lngDeg_truth'], df[latCol], df[lngCol])\n    df['phone']  = df['collectionName'] + '_' + df['phoneName']\n    res          = df.groupby('phone')['err'].agg([percentile50, percentile95])\n    \n    res['p50_p90_mean'] = (res['percentile50'] + res['percentile95']) / 2 \n    score               = res['p50_p90_mean'].mean()\n    \n    return score\n\ndef filter_outlier(df_ori, thres=2):\n    df_vel       = df_ori.copy()\n    df_vel.index = range(len(df_vel))\n    \n    for vel_col in ['Vx', 'Vy', 'Vz']:\n        t_diff_pred = df_vel['millisSinceGpsEpoch'].diff(+1).fillna(1000) / 1000\n        t_diff_forw = df_vel['millisSinceGpsEpoch'].diff(-1).fillna(1000) / 1000\n\n        v_diff_pred = abs(df_vel[vel_col].diff(+1).fillna(0) / t_diff_pred)\n        v_diff_forw = abs(df_vel[vel_col].diff(-1).fillna(0) / t_diff_forw)\n\n        df_ = pd.concat([v_diff_pred.rename('diff_pred'),\n                         v_diff_forw.rename('diff_forw')], axis=1)\n\n        outlier_idx = np.where(np.sum(abs(df_.values) > thres, axis=1) == 2)[0]\n        \n        df_vel.loc[outlier_idx, vel_col] = np.nan\n        df_vel[vel_col] = df_vel[vel_col].interpolate(method='linear')\n        \n    return df_vel\n\ndef compute_dist(fname, fname2 = 'gt.csv'):\n    if isinstance(fname, str) == True:\n        oof = pd.read_csv(fname)\n    else:\n        oof = fname.copy()\n    \n    if 'phone' not in oof.columns:\n        oof['phone'] = oof['collectionName'] + '_' + oof['phoneName']\n        \n    gt  = pd.read_csv(fname2)\n    \n    df      = oof.merge(gt, on = ['phone','millisSinceGpsEpoch'])\n    dst_oof = calc_haversine(df.latDeg_x,df.lngDeg_x, df.latDeg_y, df.lngDeg_y)\n    scores     = pd.DataFrame({'phone': df.phone,'dst': dst_oof})\n    scores_grp = scores.groupby('phone')\n    d50 = scores_grp.quantile(.50).reset_index()\n    d50.columns = ['phone','q50']\n    d95 = scores_grp.quantile(.95).reset_index()\n    d95.columns = ['phone','q95']\n    \n    return (scores_grp.quantile(.50).mean() + scores_grp.quantile(.95).mean())/2, d50.merge(d95)\n\ndef mean_with_other_phones(df):\n    \n    from scipy.ndimage import gaussian_filter1d\n    from scipy.interpolate import interp1d\n    \n    if 'collectionName' not in df.columns:\n        df['collectionName'] = df['phone'].str.split('_', expand=True)[0]\n        df['phoneName']      = df['phone'].str.split('_', expand=True)[1]\n        \n    collections_list = df[['collectionName']].drop_duplicates().to_numpy()\n\n    for collection in collections_list:\n        phone_list = df[df['collectionName'].to_list() == collection][['phoneName']].drop_duplicates().to_numpy()\n\n        phone_data = {}\n        corrections = {}\n        for phone in phone_list:\n            cond = np.logical_and(df['collectionName'] == collection[0], df['phoneName'] == phone[0]).to_list()\n            phone_data[phone[0]] = df[cond][['millisSinceGpsEpoch', 'latDeg', 'lngDeg']].to_numpy()\n\n        for current in phone_data:\n            correction = np.ones(phone_data[current].shape, dtype=np.float)\n            correction[:,1:] = phone_data[current][:,1:]\n            \n            # Telephones data don't complitely match by time, so - interpolate.\n            for other in phone_data:\n                if other == current:\n                    continue\n\n                loc = interp1d(phone_data[other][:,0], \n                               phone_data[other][:,1:], \n                               axis=0, \n                               kind='linear', \n                               copy=False, \n                               bounds_error=None, \n                               fill_value='extrapolate', \n                               assume_sorted=True)\n                \n                start_idx = 0\n                stop_idx = 0\n                for idx, val in enumerate(phone_data[current][:,0]):\n                    if val < phone_data[other][0,0]:\n                        start_idx = idx\n                    if val < phone_data[other][-1,0]:\n                        stop_idx = idx\n\n                if stop_idx - start_idx > 0:\n                    correction[start_idx:stop_idx,0] += 1\n                    correction[start_idx:stop_idx,1:] += loc(phone_data[current][start_idx:stop_idx,0])                    \n\n            correction[:,1] /= correction[:,0]\n            correction[:,2] /= correction[:,0]\n            \n            corrections[current] = correction.copy()\n        \n        for phone in phone_list:\n            cond = np.logical_and(df['collectionName'] == collection[0], df['phoneName'] == phone[0]).to_list()\n            \n            df.loc[cond, ['latDeg', 'lngDeg']] = corrections[phone[0]][:,1:]            \n            \n    return df\n\ndef add_velocity(test_file, \\\n                 predict_type='test'):\n    \n    datapath        = '../input/google-smartphone-decimeter-challenge/'\n    sub             = pd.read_csv('../input/google-smartphone-decimeter-challenge/sample_submission.csv')\n\n    if predict_type == 'train':\n        baseline_test   = pd.read_csv(datapath + \"baseline_locations_train.csv\")\n    else:\n        baseline_test   = pd.read_csv(datapath + \"baseline_locations_test.csv\")\n\n        \n    #######################\n    if isinstance(test_file, str) == True: \n        test_df         = pd.read_csv(test_file)\n    else:\n        test_df         = test_file.copy() \n        \n        \n    ori_index       = test_df.index\n\n    test_df['collectionName'] = test_df['phone'].str.split('_', expand=True)[0]\n    test_df['phoneName']      = test_df['phone'].str.split('_', expand=True)[1]\n\n    baseline_test   = baseline_test.sort_values(by=['collectionName', 'millisSinceGpsEpoch'])\n    test_df         = test_df.sort_values(by=['collectionName', 'millisSinceGpsEpoch'])\n\n    test_df['heightAboveWgs84EllipsoidM'] = baseline_test['heightAboveWgs84EllipsoidM'].values\n\n    ######################\n    \n    ecef_arr = []\n    for x, y, z in zip(test_df['latDeg'].values, \\\n                       test_df['lngDeg'].values, \\\n                       test_df['heightAboveWgs84EllipsoidM'].values):\n        x_, y_, z_ = pm.geodetic2ecef(x, y, z)\n        ecef_arr.append([x_, y_, z_])\n    ecef_arr = np.array(ecef_arr)\n\n    test_df['X'] = ecef_arr[:, 0]\n    test_df['Y'] = ecef_arr[:, 1]\n    test_df['Z'] = ecef_arr[:, 2]\n\n    dXYZs = []\n    for (collection, device_name), _ in tqdm(test_df.groupby(['collectionName', 'phoneName'])):\n        if predict_type == 'train':\n            df_xyz = pd.read_csv(f'../input/train-velocity/train_{collection}_{device_name}_velocity.csv')\n        else:\n            df_xyz = pd.read_csv(f'../input/position-optimize-014/test_{collection}_{device_name}_velocity.csv') \n        \n        dXYZs.append(df_xyz)\n\n        \n    dXYZs   = pd.concat(dXYZs)\n    test_df = pd.merge_asof(test_df.sort_values('millisSinceGpsEpoch'), \\\n                            dXYZs.sort_values('millisSinceGpsEpoch'), \\\n                            on = \"millisSinceGpsEpoch\", \\\n                            by = ['collectionName', 'phoneName'], \n                            direction = 'nearest', \\\n                            tolerance=int(100000))\n    \n    return test_df\n\ndef remove_collection_device(input_df, collectionName, phoneName_arr):\n    input_df['index'] = input_df.index\n    input_df          = input_df.sort_values('millisSinceGpsEpoch')\n    input_df.index    = input_df['millisSinceGpsEpoch'].values\n\n    cond1     = (input_df['collectionName']==collectionName)\n    df_       =  input_df.loc[cond1].copy()\n    origin_df =  df_.copy() \n    \n    cond2     = (df_['phoneName'].isin(phoneName_arr))\n    df_.loc[cond2, 'latDeg'] = np.nan\n    df_.loc[cond2, 'lngDeg'] = np.nan\n    df_  = df_.interpolate(method='index', limit_area='inside')\n\n    \n    _index = df_['latDeg'].isnull()\n    \n    if sum(_index) > 0:\n        df_.loc[_index, 'latDeg'] = origin_df.loc[_index, 'latDeg'].values\n        df_.loc[_index, 'lngDeg'] = origin_df.loc[_index, 'lngDeg'].values\n        \n    output_df = input_df.copy()\n    output_df.loc[cond1, ['latDeg', 'lngDeg']]  = df_[['latDeg', 'lngDeg']].values\n    \n    return output_df\n\nremove_list = \\\n{'2020-05-14-US-MTV-2': ['Pixel4XLModded'],\n '2020-09-04-US-SF-1':  ['Pixel4'],\n '2021-01-04-US-RWC-1': ['Pixel4XLModded', 'Pixel4XL'],\n '2021-01-04-US-RWC-2': ['Pixel4XLModded', 'Pixel4XL'], \n '2021-01-05-US-SVL-1': ['Pixel4XL', 'Pixel5'],\n '2021-01-05-US-SVL-2': ['Pixel4XL'],\n '2021-04-15-US-MTV-1': ['SamsungS20Ultra'],\n '2021-04-28-US-MTV-1': ['SamsungS20Ultra'],}\n\n\ndef stopped_process_v2(test_df):\n    sub_df = test_df.copy()\n\n    idx = 0\n    for (collection, phone), df_collection in tqdm(sub_df.groupby(['collectionName', 'phoneName'])): \n        cond     = (sub_df['collectionName']==collection)&(sub_df['phoneName']==phone)\n\n        sub_df.loc[cond, ['Vx', 'Vy', 'Vz']] = filter_outlier(sub_df.loc[cond])[['Vx', 'Vy', 'Vz']].values\n\n        df_         = sub_df.loc[cond].copy()\n        df_.index   = range(len(df_))\n\n        df_['vel']  = df_[['Vx', 'Vy', 'Vz']].apply(lambda tup: np.linalg.norm(tup), axis=1)\n\n        if collection in ['2021-04-28-US-SJC-1', '2021-04-29-US-SJC-2', '2021-04-22-US-SJC-1']:\n            vel_thres = 0.9   # 0.87\n        else:\n            vel_thres = 0.9   # 0.87\n\n        first_idx, first_point = df_.index[0],  df_[['latDeg', 'lngDeg']].values[0] \n        last_idx,  last_point  = df_.index[-1], df_[['latDeg', 'lngDeg']].values[-1]\n\n        df_.loc[df_['vel'] < vel_thres, 'latDeg'] = np.nan\n        df_.loc[df_['vel'] < vel_thres, 'lngDeg'] = np.nan\n\n        if pd.isnull(df_.loc[first_idx, 'latDeg']) == True:\n            df_.loc[first_idx, ['latDeg', 'lngDeg']]  = first_point\n\n        if pd.isnull(df_.loc[last_idx, 'latDeg']) == True:\n            df_.loc[last_idx,  ['latDeg', 'lngDeg']]  = last_point\n\n        df_ = df_.interpolate(method='pad')\n\n        sub_df.loc[cond, ['latDeg', 'lngDeg']]  = df_[['latDeg', 'lngDeg']].values\n        sub_df.loc[cond, 'vel']                 = df_['vel'].values\n        \n    return sub_df\n\ndef position_shift(fname, a, save_name=None):\n    msge = 'millisSinceGpsEpoch'\n    \n    if isinstance(fname, str) == True:\n        d = pd.read_csv(fname)\n    else:\n        d = fname.copy()\n    if 'phone' not in d.columns:\n        d['phone'] = d['collectionName'] + '_' + d['phoneName']\n         \n    d['heightAboveWgs84EllipsoidM'] = 63.5\n    d['x'], d['y'], d['z'] = zip(*d.apply(lambda x: WGS84_to_ECEF(x.latDeg, x.lngDeg, x.heightAboveWgs84EllipsoidM), axis=1))\n\n    #a = -0.2\n    d.sort_values(['phone', msge], inplace=True)\n    for fi in ['x','y','z']:\n        d[[fi+'p']] = d[fi].shift().where(d['phone'].eq(d['phone'].shift()))\n        d[[fi+'diff']] = d[fi]-d[fi+'p']\n    #d[['yp']] = d['y'].shift().where(d['phone'].eq(d['phone'].shift()))\n    d[['dist']] = np.sqrt(d['xdiff']**2 + d['ydiff']**2+ d['zdiff']**2)\n    for fi in ['x','y','z']:\n        d[[fi+'new']] = d[fi+'p'] + d[fi+'diff']*(1-a/d['dist'])\n    lng, lat, alt = ECEF_to_WGS84(d['xnew'].values,d['ynew'].values,d['znew'].values)\n    \n    lng[np.isnan(lng)] = d.loc[np.isnan(lng),'lngDeg']\n    lat[np.isnan(lat)] = d.loc[np.isnan(lat),'latDeg']\n    d['latDeg'] = lat\n    d['lngDeg'] = lng\n    \n    d.sort_values(['phone',msge],inplace = True)\n        \n    return d #[sub_columns]","metadata":{"execution":{"iopub.status.busy":"2021-07-29T07:04:56.90723Z","iopub.execute_input":"2021-07-29T07:04:56.907665Z","iopub.status.idle":"2021-07-29T07:04:57.015228Z","shell.execute_reply.started":"2021-07-29T07:04:56.907631Z","shell.execute_reply":"2021-07-29T07:04:57.013958Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"-------\n## Load groundtruth","metadata":{}},{"cell_type":"code","source":"datadir  = Path('/kaggle/input/google-smartphone-decimeter-challenge/')\ntraindir = datadir / 'train'\n\ngt = pd.DataFrame()\nfor d in os.listdir(traindir):\n    for p in os.listdir(traindir/d):\n        gt = gt.append(pd.read_csv(traindir/d/p/'ground_truth.csv'))\n\nsample_sub  = pd.read_csv('../input/google-smartphone-decimeter-challenge/sample_submission.csv')\nsub_columns = sample_sub.columns\ngt['phone'] = gt['collectionName'] + '_' + gt['phoneName']\ngt[sub_columns].to_csv('gt.csv', index = False)","metadata":{"execution":{"iopub.status.busy":"2021-07-29T05:56:49.585944Z","iopub.execute_input":"2021-07-29T05:56:49.586245Z","iopub.status.idle":"2021-07-29T05:56:51.700986Z","shell.execute_reply.started":"2021-07-29T05:56:49.586217Z","shell.execute_reply":"2021-07-29T05:56:51.699812Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"-----------------------","metadata":{}},{"cell_type":"markdown","source":"------------------\n## Advanced optimize","metadata":{}},{"cell_type":"code","source":"def map_all_route(df, \\\n                  train_collection_arr, \\\n                  test_collection_arr, \\\n                  train_phone_arr, \\\n                  test_phone_arr, \\\n                  map_thres=None):\n    \n    test_df = df.copy()\n    \n    for train_collection, test_collection, phone_train, phone_test in zip(train_collection_arr, \\\n                                                                          test_collection_arr, \\\n                                                                          train_phone_arr, \\\n                                                                          test_phone_arr):\n\n        df_soi = test_df[test_df['phone']==f'{test_collection}_{phone_test}']\\\n                .rename(columns={'phone': 'phoneName'})[['phoneName', 'millisSinceGpsEpoch', 'latDeg', 'lngDeg']]\n\n        df_soi.index = range(len(df_soi))\n\n        gt = pd.read_csv(f'../input/google-smartphone-decimeter-challenge/train/{train_collection}/{phone_train}/ground_truth.csv')\n        gt = gt[['phoneName', 'millisSinceGpsEpoch', 'latDeg', 'lngDeg']]\n        gt['phoneName'] = train_collection + '_' + gt['phoneName']\n\n\n        base         = gt[['latDeg', 'lngDeg']].values.astype('float64')\n        route        = df_soi[['latDeg', 'lngDeg']].values.astype('float64')\n\n        base_reduce  = []\n        ori_idx      = []\n\n        for i, P in enumerate(base):\n            if i == 0:\n                base_reduce.append(base[0])\n                ori_idx.append(0)\n                continue\n\n            dist = calc_haversine(*P, *base_reduce[-1])\n\n            if dist >= 3:\n                base_reduce.append(P)\n                ori_idx.append(i)\n\n        base_reduce   = np.array(base_reduce)\n        base_idx      = np.array(ori_idx)\n\n        INF   = 9 * 1e9\n        dp    = np.ones((route.shape[0], base_reduce.shape[0])) * INF\n        trace = np.ones((route.shape[0], base_reduce.shape[0])) * -1\n\n        cands = []\n        for P in route:\n            P_rep  = np.tile(P[None, :], (base_reduce.shape[0], 1))\n            dist   = calc_haversine(P_rep[:, 0], P_rep[:, 1], base_reduce[:, 0], base_reduce[:, 1])\n\n            idx    = np.where(dist<50)[0]\n            cands.append(idx)\n\n\n        for cand_idx in cands[0]:\n            dp[0][cand_idx] = calc_haversine(*route[0], *base_reduce[cand_idx])\n\n        for route_idx in tqdm(range(1, route.shape[0])):\n            route_dis = calc_haversine(*route[route_idx], *route[route_idx-1])\n\n            for cand_idx in cands[route_idx]:\n                gap_dist  = calc_haversine(*route[route_idx], *base_reduce[cand_idx])\n\n                map_dis   = 0\n                for pre_idx in range(cand_idx, -1, -1): \n                    map_dis += calc_haversine(*base_reduce[pre_idx], *base_reduce[pre_idx+1]) if pre_idx < cand_idx else 0\n\n                    if map_dis > route_dis + 40: break\n\n                    new_val = gap_dist + abs(map_dis - route_dis) + dp[route_idx-1][pre_idx]\n\n                    if new_val < dp[route_idx][cand_idx]:\n                        dp[route_idx][cand_idx]    = new_val\n                        trace[route_idx][cand_idx] = pre_idx    \n\n        map_idx = [np.argmin(dp[-1])]\n\n        for route_idx in range(route.shape[0]-1, 0, -1):\n            idx = map_idx[-1]\n            map_idx.append(trace[route_idx][int(idx)])\n\n        # map_idx = [base_idx[int(idx)] for idx in map_idx]\n        map_idx = np.array(map_idx[::-1]).astype('int32')\n\n\n        map_point = []\n\n        for reduce_idx, P in zip(map_idx, route):\n            P_point  = Point(P) \n            l, r     =  max(0, reduce_idx-5), min(reduce_idx+6, base_reduce.shape[0])\n            segment  =  LineString(list(base_reduce[l : r]))\n            \n            map_P    = (segment.interpolate(segment.project(P_point)).coords[0])\n            choose   = map_P \n            \n            if map_thres is not None and calc_haversine(map_P[0], map_P[1], P[0], P[1]) > map_thres:\n                choose = P\n                \n            map_point.append(choose)\n\n        map_point = np.array(map_point).astype('float64')\n\n        test_df.loc[test_df['phone']==f'{test_collection}_{phone_test}', ['latDeg', 'lngDeg']] = map_point\n        \n    return test_df\n\n\ndef map_by_chunk(test_df, \\\n                 test_phone, \\\n                 test_collection, \\\n                 all_point, map_point, global2base, whichBase, base2global, \\\n                 map_thres       = 9, \\\n                 candidate_thres = 100, \\\n                 CHUNK_SIZE      = 20, \\\n                 show  = True):\n\n    sub = test_df.copy()\n    \n    if isinstance(test_phone, list) == True:\n        cond    = (test_df['phoneName'].isin(test_phone))&(test_df['collectionName']==test_collection) \n    else:\n        cond    = (test_df['phoneName']==test_phone)&(test_df['collectionName']==test_collection) \n\n    df_soi  = sub.loc[cond]\\\n                    .rename(columns={'phone': 'phoneName'})[['phoneName', 'millisSinceGpsEpoch', 'latDeg', 'lngDeg']]\n    \n    df_soi       = df_soi.sort_values(by='millisSinceGpsEpoch') \n    df_soi.index = range(len(df_soi))\n\n    route        = df_soi[['latDeg', 'lngDeg']].values.astype('float64')\n    df_soi.index = range(len(df_soi))\n\n\n    candidate = []\n\n    for P in route:\n        P_rep  = np.tile(P[None, :], (map_point.shape[0], 1))\n        dist   = calc_haversine(P_rep[:, 0], P_rep[:, 1], map_point[:, 0], map_point[:, 1])\n        idx    = np.where(dist<candidate_thres)[0]\n        candidate.append(idx)\n\n    route_concat = []\n    diffs        = []\n    map_segment  = []\n    \n    for chunk_idx in tqdm(range(0, route.shape[0], CHUNK_SIZE)):\n\n        segment = route[chunk_idx: min(chunk_idx+CHUNK_SIZE, route.shape[0])]\n        cands   = candidate[chunk_idx: min(chunk_idx+CHUNK_SIZE, route.shape[0])]\n\n        INF    = 9 * 1e9\n        dp     = np.ones((segment.shape[0], map_point.shape[0])) * INF\n        trace  = np.ones((segment.shape[0], map_point.shape[0])) * -1\n\n        for cand_idx in cands[0]:\n            dp[0][cand_idx] = calc_haversine(*route[0], *map_point[cand_idx])\n\n        for route_idx in range(1, segment.shape[0]):\n            route_dis = calc_haversine(*segment[route_idx], *segment[route_idx-1])\n\n            for cand_idx in cands[route_idx]:\n                gap_dist  = calc_haversine(*segment[route_idx], *map_point[cand_idx])\n\n                map_dis   = 0\n                baseIndex = global2base[cand_idx]\n                basePos   = whichBase[cand_idx]\n\n                for preBaseIdx in range(baseIndex, -1, -1): \n                    preGlobalIndex = base2global[(basePos, preBaseIdx)]\n\n                    map_dis += calc_haversine(*all_point[basePos][preBaseIdx], *all_point[basePos][preBaseIdx+1]) \\\n                               if preBaseIdx < baseIndex else 0\n\n                    if map_dis > route_dis + 40: break\n\n                    new_val = 2 * gap_dist + abs(map_dis - route_dis) + dp[route_idx-1][preGlobalIndex]\n\n                    if new_val < dp[route_idx][cand_idx]:\n                        dp[route_idx][cand_idx]    = new_val\n                        trace[route_idx][cand_idx] = preGlobalIndex    \n\n        map_idx = [np.argmin(dp[-1])]\n\n        for route_idx in range(segment.shape[0]-1, 0, -1):\n            index = map_idx[-1]\n            map_idx.append(trace[route_idx][int(index)])\n\n        map_idx       = np.array(map_idx[::-1]).astype('int32')\n        map_route     = []\n        \n        for global_idx, P in zip(map_idx, segment):\n            P_point   = Point(P) \n\n            baseIndex = global2base[global_idx]\n            basePos   = whichBase[global_idx]\n\n            l, r      =  max(0, baseIndex-5), min(baseIndex+6, all_point[basePos].shape[0])\n            geo       =  LineString(list(all_point[basePos][l : r]))\n            map_route.append((geo.interpolate(geo.project(P_point)).coords[0]))\n            map_segment.append(geo)\n        map_route = np.array(map_route).astype('float64')\n\n        mean_diff = np.mean(calc_haversine(segment[:, 0], segment[:, 1], map_route[:, 0], map_route[:, 1]))\n        diffs.append(mean_diff)\n\n#             if mean_diff > map_thres:\n#                 route_concat.append(segment)\n#             else:\n#                 route_concat.append(map_route)\n\n        if callable(map_thres):\n            if map_thres(mean_diff) == True:            \n                route_concat.append(map_route)\n            else:\n                route_concat.append(segment)\n        else:\n            if mean_diff < map_thres:            \n                route_concat.append(map_route)\n            else:\n                route_concat.append(segment)\n\n    route_concat =  np.concatenate(route_concat)\n    sub.loc[cond, ['latDeg', 'lngDeg']] = route_concat\n\n    if show==True:\n        show_map_route(route, route_concat)\n            \n    return sub, map_segment\n\n\ndef get_candidate_road_point(df, \\\n                  train_collection, \\\n                  test_collection, \\\n                  train_phone, \\\n                  test_phone,\n                  base_point_dist=2,\n                  map_dist=50):\n    \n    test_df = df.copy()\n    \n    if isinstance(test_phone, list) == True:\n        cond    = (test_df['phoneName'].isin(test_phone))&(test_df['collectionName']==test_collection) \n    else:\n        cond    = (test_df['phoneName']==test_phone)&(test_df['collectionName']==test_collection) \n    \n    df_soi  = test_df.loc[cond]\\\n                    .rename(columns={'phone': 'phoneName'})[['phoneName', 'millisSinceGpsEpoch', 'latDeg', 'lngDeg']]\n    \n    df_soi       = df_soi.sort_values(by='millisSinceGpsEpoch') \n    df_soi.index = range(len(df_soi))\n\n    gt = pd.read_csv(f'../input/google-smartphone-decimeter-challenge/train/{train_collection}/{train_phone}/ground_truth.csv')\n    gt = gt[['phoneName', 'millisSinceGpsEpoch', 'latDeg', 'lngDeg']]\n    gt['phoneName'] = train_collection + '_' + gt['phoneName']\n\n\n    base         = gt[['latDeg', 'lngDeg']].values.astype('float64')\n    route        = df_soi[['latDeg', 'lngDeg']].values.astype('float64')\n\n    base_reduce  = []\n    ori_idx      = []\n\n    for i, P in enumerate(base):\n        if i == 0:\n            base_reduce.append(base[0])\n            ori_idx.append(0)\n            continue\n\n        dist = calc_haversine(*P, *base_reduce[-1])\n\n        if dist >= base_point_dist:\n            base_reduce.append(P)\n            ori_idx.append(i)\n\n    base_reduce   = np.array(base_reduce)\n    base_idx      = np.array(ori_idx)\n\n    INF   = 9 * 1e9\n    dp    = np.ones((route.shape[0], base_reduce.shape[0])) * INF\n    trace = np.ones((route.shape[0], base_reduce.shape[0])) * -1\n\n    cands = []\n    for P in route:\n        P_rep  = np.tile(P[None, :], (base_reduce.shape[0], 1))\n        dist   = calc_haversine(P_rep[:, 0], P_rep[:, 1], base_reduce[:, 0], base_reduce[:, 1])\n\n        idx    = np.where(dist<map_dist)[0]\n        cands.append(idx)\n\n\n    for cand_idx in cands[0]:\n        dp[0][cand_idx] = calc_haversine(*route[0], *base_reduce[cand_idx])\n\n    for route_idx in tqdm(range(1, route.shape[0])):\n        route_dis = calc_haversine(*route[route_idx], *route[route_idx-1])\n\n        for cand_idx in cands[route_idx]:\n            gap_dist  = calc_haversine(*route[route_idx], *base_reduce[cand_idx])\n\n            map_dis   = 0\n            for pre_idx in range(cand_idx, -1, -1): \n                map_dis += calc_haversine(*base_reduce[pre_idx], *base_reduce[pre_idx+1]) if pre_idx < cand_idx else 0\n\n                if map_dis > route_dis + 40: break\n\n                new_val = gap_dist + abs(map_dis - route_dis) + dp[route_idx-1][pre_idx]\n\n                if new_val < dp[route_idx][cand_idx]:\n                    dp[route_idx][cand_idx]    = new_val\n                    trace[route_idx][cand_idx] = pre_idx    \n\n    map_idx = [np.argmin(dp[-1])]\n\n    for route_idx in range(route.shape[0]-1, 0, -1):\n        idx = map_idx[-1]\n        map_idx.append(trace[route_idx][int(idx)])\n\n    map_idx     = np.array(map_idx[::-1]).astype('int32')\n    map_segment = []\n\n    for reduce_idx, P in zip(map_idx, route):\n#         P_point  = Point(P)\n\n        l, r     =  max(0, reduce_idx-15), min(reduce_idx+16, base_reduce.shape[0])\n        map_segment.append(base_reduce[l : r])\n\n#         segment  =  LineString(list(base_reduce[l : r]))\n#         map_point.append((segment.interpolate(segment.project(P_point)).coords[0]))\n    \n#     test_df.loc[test_df['phone']==f'{test_collection}_{phone_test}', ['latDeg', 'lngDeg']] = map_point\n\n    return map_segment, base_reduce\n\n\ndef vel_0_process(train_df):\n    sub_df = train_df.copy()\n\n    for (collection, phone), df_collection in tqdm(sub_df.groupby(['collectionName', 'phoneName'])): \n        cond     = (sub_df['collectionName']==collection)&(sub_df['phoneName']==phone)\n\n        sub_df.loc[cond, ['Vx', 'Vy', 'Vz']] = filter_outlier(sub_df.loc[cond])[['Vx', 'Vy', 'Vz']].values\n\n        df_         = sub_df.loc[cond].copy()\n        df_['vel']  = df_[['Vx', 'Vy', 'Vz']].apply(lambda tup: np.linalg.norm(tup), axis=1)\n\n        if collection in ['2021-04-28-US-SJC-1', '2021-04-29-US-SJC-2', '2021-04-22-US-SJC-1']:\n            vel_thres = 0.87\n        else:\n            vel_thres = 0.87\n\n        df_.loc[df_['vel'] < vel_thres, 'latDeg'] = np.nan\n        df_.loc[df_['vel'] < vel_thres, 'lngDeg'] = np.nan\n\n        df_ = df_.interpolate(method='pad')\n        df_ = df_.interpolate(method='backfill')    \n\n        sub_df.loc[cond, ['latDeg', 'lngDeg']] = df_[['latDeg', 'lngDeg']].values\n        sub_df.loc[cond, 'vel']                = df_['vel'].values\n        \n    return sub_df\n\ndef get_param(collection_idx, param_set, group_collection):\n    if collection_idx in group_collection[0]:\n        return param_set[0], param_set[3+0]\n    \n    if collection_idx in group_collection[1]:\n        return param_set[1], param_set[3+1]\n    \n    if collection_idx in group_collection[2]:\n        return param_set[2], param_set[3+2]\n\n    \ndef opt_process(df):\n    train_df = df.copy()\n    alpha_param      = 1.0\n    beta_param       = 0.1\n\n    group_collection = [[1, 2, 3, 4, 5, 6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21],\n                        [22,23,25,26,28],\n                        [24,27,29]]\n\n    param_set        =  [0.1, 0.1, 0.1] + [1.0, 1.0, 1.0] # \n\n\n    for collection_idx, (collection, df_collection) in enumerate(train_df.groupby(['collectionName'])): \n\n        cond = train_df['collectionName']==collection\n\n        delta_t        = train_df.loc[cond, 'millisSinceGpsEpoch'].diff().values[1:] / 1000.0\n\n        xy_hat         = train_df.loc[cond, ['X', 'Y', 'Z']].values\n        delta_xy_hat   = filter_outlier(train_df.loc[cond], thres=2.4)[['Vx', 'Vy', 'Vz']].values[1:, :] # train_df.loc[cond, ['Vx', 'Vy', 'Vz']].values[1:, :] \n\n        delta_xy_hat[np.where(np.linalg.norm(delta_xy_hat, axis=1)<1.0)] = 0\n        delta_xy_hat   = delta_xy_hat * delta_t[:, None]\n\n        beta_param, alpha_param = get_param(collection_idx+1, param_set, group_collection)\n\n        N        = xy_hat.shape[0]\n        alpha    = (alpha_param)**(-2) * np.ones(N)\n        beta     = (beta_param + beta_param * delta_t)**(-2) # * np.ones(N)\n\n        A  = scipy.sparse.spdiags(alpha, [0], N, N)\n        B  = scipy.sparse.spdiags( beta, [0], N-1, N-1)\n        D  = scipy.sparse.spdiags(np.stack([-np.ones(N), np.ones(N)]), [0, 1], N-1, N)\n\n        Q       = A + (D.T @ B @ D)\n        c       = (A @ xy_hat) + (D.T @ (B @ delta_xy_hat))\n        xy_star = scipy.sparse.linalg.spsolve(Q, c)\n\n        train_df.loc[cond, 'latDeg'] = ecef2lla(*xy_star.T)[0].T[0]\n        train_df.loc[cond, 'lngDeg'] = ecef2lla(*xy_star.T)[1].T[0]\n        \n    return train_df\n\n\ndef map_to_ground_truth(route, map_segment, CHUNK_SIZE=20):\n    '''\n    Input: array of [lat, lng]\n    fixed heightAboveWgs84EllipsoidM = 63.5\n    \n    --------------\n    Return: (x, y, z) in ECEF\n    '''\n    \n    map_route = []\n    \n    for P, segment in zip(route, map_segment):\n        P_conv         =  Point(P)        \n        segment_conv   =  LineString(segment)\n        \n        x,   y         = segment_conv.interpolate(segment_conv.project(P_conv)).coords[0]\n        x_, y_,  z_    = pm.geodetic2ecef(x, y, 63.5)\n        \n        map_route.append([x_, y_, z_])\n\n    return np.array(map_route).astype('float64')\n\n\ndata_dir = Path(\"../input/google-smartphone-decimeter-challenge\")\n\ndef getBaseReduce(base, thres=5):\n    base_reduce = []\n    \n    for i, P in enumerate(base):\n        if i == 0:\n            base_reduce.append(base[0])\n            continue\n\n        dist = calc_haversine(*P, *base_reduce[-1])\n\n        if dist >= thres:\n            base_reduce.append(P)\n            \n    return np.array(base_reduce)\n\n\ndef getCandidate(route, base_reduce, thres=30):\n    cands = []\n    \n    for P in route:\n        P_rep  = np.tile(P[None, :], (base_reduce.shape[0], 1))\n        dist   = calc_haversine(P_rep[:, 0], P_rep[:, 1], base_reduce[:, 0], base_reduce[:, 1])\n\n        idx    = np.where(dist<thres)[0]\n        cands.append(idx)\n        \n    return cands\n\ndef get_map_data(train_group):\n    train_df  = pd.read_csv(data_dir / \"baseline_locations_train.csv\")\n    all_point = []\n\n    for (collection_name, phone_name), df in train_df.groupby([\"collectionName\", \"phoneName\"]):\n        if collection_name not in train_group: continue\n\n        path   = data_dir / f\"train/{collection_name}/{phone_name}/ground_truth.csv\"\n        df     = pd.read_csv(path)      \n\n        ## \n#         if collection_name == '2021-04-28-US-MTV-1':\n#             base         = df[['latDeg', 'lngDeg']][500: ].values.astype('float64')\n#         else:\n\n        base         = df[['latDeg', 'lngDeg']].values.astype('float64')\n        base_reduce  = getBaseReduce(base)\n        all_point.append(base_reduce)\n    \n    map_point   = np.concatenate(all_point)\n\n    global2base = np.zeros(map_point.shape[0]).astype('int32')\n    whichBase   = np.zeros(map_point.shape[0]).astype('int32')\n    base2global = {}\n    curLen      = 0\n\n    for i, base in enumerate(all_point): \n        for j in range(base.shape[0]):\n            global2base[curLen + j] = j\n            whichBase[curLen + j]   = i\n            base2global[(i, j)]     = curLen + j\n        curLen += base.shape[0]\n\n    return all_point, map_point, global2base, whichBase, base2global\n\n\ndef get_multiple_candidate_road_point(df, \\\n                                      test_phone, \\\n                                      test_collection, \\\n                                      all_point, map_point, global2base, whichBase, base2global, \\\n                                      map_thres       = 9, \\\n                                      candidate_thres = 100, \\\n                                      CHUNK_SIZE      = 20):\n\n    sub = df.copy()\n    \n    if isinstance(test_phone, list) == True:\n        cond    = (df['phoneName'].isin(test_phone))&(df['collectionName']==test_collection) \n    else:\n        cond    = (df['phoneName']==test_phone)&(df['collectionName']==test_collection) \n    \n    df_soi  = sub.loc[cond]\\\n                    .rename(columns={'phone': 'phoneName'})[['phoneName', 'millisSinceGpsEpoch', 'latDeg', 'lngDeg']]\n    \n    df_soi       = df_soi.sort_values(by='millisSinceGpsEpoch') \n    df_soi.index = range(len(df_soi))\n\n    \n    route        = df_soi[['latDeg', 'lngDeg']].values.astype('float64')\n    candidate    = []\n\n    for P in route:\n        P_rep  = np.tile(P[None, :], (map_point.shape[0], 1))\n        dist   = calc_haversine(P_rep[:, 0], P_rep[:, 1], map_point[:, 0], map_point[:, 1])\n        idx    = np.where(dist<candidate_thres)[0]\n        candidate.append(idx)\n\n    route_concat   = []\n    diffs          = []\n    \n    ## ground truth road candidates for each route segment (CHUNK_SIZE=20)\n    ## for segment i, the candidates is road_candidate[i] (a list of LineString object)\n    road_candidate = []\n    \n    for chunk_idx in tqdm(range(0, route.shape[0], CHUNK_SIZE)):\n        min_diff = float('inf')\n        segment = route[chunk_idx: min(chunk_idx+CHUNK_SIZE, route.shape[0])]\n        cands   = candidate[chunk_idx: min(chunk_idx+CHUNK_SIZE, route.shape[0])]\n\n        INF    = 9 * 1e9\n        dp     = np.ones((segment.shape[0], map_point.shape[0])) * INF\n        trace  = np.ones((segment.shape[0], map_point.shape[0])) * -1\n\n        for cand_idx in cands[0]:\n            dp[0][cand_idx] = calc_haversine(*route[0], *map_point[cand_idx])\n\n        for route_idx in range(1, segment.shape[0]):\n            route_dis = calc_haversine(*segment[route_idx], *segment[route_idx-1])\n\n            for cand_idx in cands[route_idx]:\n                gap_dist  = calc_haversine(*segment[route_idx], *map_point[cand_idx])\n\n                map_dis   = 0\n                baseIndex = global2base[cand_idx]\n                basePos   = whichBase[cand_idx]\n\n                for preBaseIdx in range(baseIndex, -1, -1): \n                    preGlobalIndex = base2global[(basePos, preBaseIdx)]\n\n                    map_dis += calc_haversine(*all_point[basePos][preBaseIdx], *all_point[basePos][preBaseIdx+1]) \\\n                               if preBaseIdx < baseIndex else 0\n\n                    if map_dis > route_dis + 40: break\n\n                    new_val = 2 * gap_dist + abs(map_dis - route_dis) + dp[route_idx-1][preGlobalIndex]\n\n                    if new_val < dp[route_idx][cand_idx]:\n                        dp[route_idx][cand_idx]    = new_val\n                        trace[route_idx][cand_idx] = preGlobalIndex    \n        \n        \n        gt_candidate = []\n        valid_idx    = np.where(dp[-1] != INF)[0]\n        base_pool    = set()\n        \n        for last_idx in sorted(valid_idx, key=lambda idx: dp[-1][idx]):\n        \n            ## get only one road segment from one base groundtruth\n            basePos   = whichBase[last_idx]\n            if basePos in base_pool:\n                continue\n            \n            map_idx      = [last_idx]\n\n            for route_idx in range(segment.shape[0]-1, 0, -1):\n                index = map_idx[-1]\n                map_idx.append(trace[route_idx][int(index)])\n\n            map_idx   = np.array(map_idx[::-1]).astype('int32')\n            map_route = []\n            \n            min_l, max_r  = INF, 0, \n            \n            for global_idx, P in zip(map_idx, segment):\n                P_point   = Point(P) \n\n                baseIndex = global2base[global_idx]\n                basePos   = whichBase[global_idx]\n\n                l, r      =  max(0, baseIndex-5), min(baseIndex+6, all_point[basePos].shape[0])\n                geo       =  LineString(list(all_point[basePos][l : r]))\n                map_route.append((geo.interpolate(geo.project(P_point)).coords[0]))\n                \n                min_l     = min(min_l, l)\n                max_r     = max(max_r, r)\n                \n            map_route = np.array(map_route).astype('float64')\n            mean_diff = np.mean(calc_haversine(segment[:, 0], segment[:, 1], map_route[:, 0], map_route[:, 1]))\n            min_diff=min(min_diff, mean_diff)\n            if mean_diff < map_thres:            \n                geo  =  LineString(list(all_point[basePos][min_l : max_r]))\n                gt_candidate.append(geo)\n                base_pool.add(basePos)\n                \n        diffs.append(min_diff)\n        if len(gt_candidate) > 0:\n            road_candidate.append([gt_candidate[0]])\n        else:\n            road_candidate.append([])\n            \n    return road_candidate, diffs\n\ndef map_to_multiple_ground_truth(route, map_segment, CHUNK_SIZE=20):\n    '''\n    Input: array of [lat, lng]\n    fixed heightAboveWgs84EllipsoidM = 63.5\n    \n    --------------\n    Return: (x, y, z) in ECEF\n    '''\n    \n    map_route = []\n    \n    for point_idx, P in enumerate(route):\n        P_conv         =  Point(P)       \n        segment_arr    =  map_segment[point_idx // CHUNK_SIZE]\n        \n        if len(segment_arr) == 0:\n            x_, y_,  z_ = pm.geodetic2ecef(P[0], P[1], 63.5)\n            map_route.append([x_, y_, z_])\n            continue\n            \n        min_diff = float('inf')\n        choose   = None\n        for segment in segment_arr:\n            x,   y         = segment.interpolate(segment.project(P_conv)).coords[0]\n            diff           = calc_haversine(P[0], P[1], x, y)  # np.linalg.norm(P - np.array([x, y]).astype('float64'))\n             \n            if diff < min_diff:\n                x_, y_,  z_    = pm.geodetic2ecef(x, y, 63.5)\n                min_diff       = diff\n                choose         = [x_, y_, z_]\n                \n        map_route.append(choose)\n\n    return np.array(map_route).astype('float64')","metadata":{"_kg_hide-input":false,"execution":{"iopub.status.busy":"2021-07-29T05:57:25.431051Z","iopub.execute_input":"2021-07-29T05:57:25.431364Z","iopub.status.idle":"2021-07-29T05:57:25.757262Z","shell.execute_reply.started":"2021-07-29T05:57:25.431333Z","shell.execute_reply":"2021-07-29T05:57:25.756022Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"-------------\n## Test","metadata":{}},{"cell_type":"code","source":"def advanced_opt(test_df, map_segment, cond, map_function=map_to_ground_truth, CHUNK_SIZE=20):\n    \n    test_opt_df    = test_df.copy()\n    delta_t        = test_df.loc[cond, 'millisSinceGpsEpoch'].diff().values[1:] / 1000.0    \n    xy_hat         = test_df.loc[cond, ['X', 'Y', 'Z']].values\n    delta_xy_hat   = filter_outlier(test_df.loc[cond], thres=2.4)[['Vx', 'Vy', 'Vz']].values[1:, :] # train_df.loc[cond, ['Vx', 'Vy', 'Vz']].values[1:, :] \n\n    delta_xy_hat[np.where(np.linalg.norm(delta_xy_hat, axis=1)<0.87)] = 0\n    delta_xy_hat   = delta_xy_hat * delta_t[:, None]\n\n    alpha_param      = 1.0\n    beta_param       = 0.1\n    gamma_param      = 0.2\n\n    N        = xy_hat.shape[0]\n    alpha    = (alpha_param)**(-2) * np.ones(N)\n    beta     = (beta_param + beta_param * delta_t)**(-2) # * np.ones(N)\n    gamma    = (gamma_param**(-2) * np.ones(N))\n\n    A  = scipy.sparse.spdiags(alpha, [0], N, N)\n    B  = scipy.sparse.spdiags( beta, [0], N-1, N-1)\n    D  = scipy.sparse.spdiags(np.stack([-np.ones(N), np.ones(N)]), [0, 1], N-1, N)\n    C  = scipy.sparse.spdiags(gamma, [0], N, N)\n\n    Q        = A + (D.T @ B @ D)\n    c        = (A @ xy_hat) + (D.T @ (B @ delta_xy_hat))\n    xy_star  = scipy.sparse.linalg.spsolve(Q, c)\n\n    res        = ecef2lla(*xy_star.T)\n    to_lat_lng = np.concatenate((res[0], res[1]), axis=1)\n\n    for r in tqdm(np.arange(0.01, 0.99, 0.01)):\n        xy_map     = map_function(to_lat_lng, map_segment, CHUNK_SIZE)\n\n        Q = ((1 - r) * A) + (r * C) + D.T @ B @ D\n        c = ((1 - r) * (A @ xy_hat)) + (r * (C @ xy_map)) + (D.T @ (B @ delta_xy_hat))\n        xy_star = scipy.sparse.linalg.spsolve(Q, c)\n\n        res        = ecef2lla(*xy_star.T)\n        to_lat_lng = np.concatenate((res[0], res[1]), axis=1)\n\n    res        = ecef2lla(*xy_star.T)\n    to_lat_lng = np.concatenate((res[0], res[1]), axis=1)\n    test_opt_df.loc[cond, ['latDeg', 'lngDeg']] = to_lat_lng \n\n    return test_opt_df","metadata":{"execution":{"iopub.status.busy":"2021-07-29T06:16:52.179115Z","iopub.status.idle":"2021-07-29T06:16:52.179694Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"test_opt_chunk  = add_velocity('../input/submission-test/4.342_(0429MTV)_(0428_0421_0316R)_(0426)_thres9_(phone_mean)_3.875.csv')\n\n## (04-29SJC)-(04-29MTV)\nmap_segment, base_reduce = get_candidate_road_point(test_opt_chunk, \\\n                                                    '2021-04-22-US-SJC-1', # train_collection_arr\n                                                    '2021-04-29-US-SJC-3', # test_collection_arr\n                                                    'Pixel4',                            # train_phone_arr\n                                                    ['SamsungS20Ultra', 'Pixel4'], )     # test_phone_arr\n\ncond               = (test_opt_chunk['collectionName'] == '2021-04-29-US-SJC-3')\ntest_opt_chunk     = advanced_opt(test_opt_chunk, map_segment, cond)\n\nmap_segment, base_reduce = get_candidate_road_point(test_opt_chunk, \\\n                                                    '2021-04-29-US-MTV-1', # train_collection_arr\n                                                    '2021-04-29-US-MTV-2', # test_collection_arr\n                                                    'Pixel4',                                    # train_phone_arr\n                                                    ['Pixel4', 'Pixel5', 'SamsungS20Ultra'])     # test_phone_arr\n\ncond               = (test_opt_chunk['collectionName'] == '2021-04-29-US-MTV-2')\ntest_opt_chunk     = advanced_opt(test_opt_chunk, map_segment, cond)","metadata":{"execution":{"iopub.status.busy":"2021-07-29T06:16:52.188764Z","iopub.status.idle":"2021-07-29T06:16:52.189253Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"test_opt_chunk['index'] = test_opt_chunk.groupby(['collectionName', 'phoneName']).apply(lambda gr: pd.Series(range(len(gr)))).values\nshow_gt(test_opt_chunk)","metadata":{"execution":{"iopub.status.busy":"2021-07-29T06:16:52.190278Z","iopub.status.idle":"2021-07-29T06:16:52.190996Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"test_opt_df  = vel_0_process(test_opt_chunk)\ntest_mean    = mean_with_other_phones(test_opt_df)\ntest_mean    = test_mean.sort_values(by=['phone', \"millisSinceGpsEpoch\"])\n\ntest_mean[['phone', 'millisSinceGpsEpoch', 'latDeg', 'lngDeg']].\\\nto_csv('3.875_(0429SJC_0429MTV)_(vel_0)_(phone_mean_ver2).csv', index=False)\n\n# test_mean[['phone', 'millisSinceGpsEpoch', 'latDeg', 'lngDeg']].to_csv('4.342_(0429SJC_0429MTV)_(0428_0421_0316R)_(0426)_(vel_0)_(phone_mean_ver2).csv', index=False)\n# test_mean[['phone', 'millisSinceGpsEpoch', 'latDeg', 'lngDeg']].to_csv('3.875_(0429SJC_0429MTV)_(0428_0421_0316R)_(vel_0)_(phone_mean_ver2).csv', index=False)\n# test_mean[['phone', 'millisSinceGpsEpoch', 'latDeg', 'lngDeg']].to_csv('3.875_(0429SJC_0429MTV)_(vel_0)_(phone_mean_ver2).csv', index=False)\n# test_mean[['phone', 'millisSinceGpsEpoch', 'latDeg', 'lngDeg']].to_csv('3.707_(0429SJC_0429MTV)_(vel_0)_(phone_mean_ver2).csv', index=False)\n# test_mean[['phone', 'millisSinceGpsEpoch', 'latDeg', 'lngDeg']].to_csv('3.463_(0429SJC_0429MTV)_(vel_0)_(phone_mean_ver2).csv', index=False)\n# test_mean[['phone', 'millisSinceGpsEpoch', 'latDeg', 'lngDeg']].to_csv('3.875_(0429SJC_adv_(0422_map))_(vel_0)_(phone_mean_ver2).csv', index=False)","metadata":{"execution":{"iopub.status.busy":"2021-07-29T06:16:52.192019Z","iopub.status.idle":"2021-07-29T06:16:52.19245Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{},"execution_count":null,"outputs":[]}]}