{"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":"import numpy as np \nimport pandas as pd \nimport scipy.optimize as opt\nimport math\nfrom pathlib import Path\nfrom tqdm.notebook import tqdm\nimport pyproj\nimport matplotlib.pyplot as plt\nfrom scipy import stats","metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","_kg_hide-input":true,"execution":{"iopub.status.busy":"2021-06-15T06:10:00.383769Z","iopub.execute_input":"2021-06-15T06:10:00.384202Z","iopub.status.idle":"2021-06-15T06:10:00.715496Z","shell.execute_reply.started":"2021-06-15T06:10:00.384170Z","shell.execute_reply":"2021-06-15T06:10:00.714494Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def 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","metadata":{"_kg_hide-input":true,"execution":{"iopub.status.busy":"2021-06-15T06:10:01.067144Z","iopub.execute_input":"2021-06-15T06:10:01.067554Z","iopub.status.idle":"2021-06-15T06:10:01.082731Z","shell.execute_reply.started":"2021-06-15T06:10:01.067518Z","shell.execute_reply":"2021-06-15T06:10:01.081632Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def rotate_sat(sat_pos, tm, bias=0):\n    periods = 24 * 3600\n    res = sat_pos.copy()\n    ang = 2 * math.pi / periods * (tm - bias)\n    res[:,2] = sat_pos[:,2]\n    res[:,0] = np.cos(ang) * sat_pos[:, 0] + np.sin(ang) * sat_pos[:, 1] \n    res[:,1] = - np.sin(ang) * sat_pos[:, 0] + np.cos(ang) * sat_pos[:, 1]\n    return res\n\n# def move_sat(sat_pos, sat_vel, pr):\n#     print(sat_pos.shape, sat_vel.shape, pr.shape)\n#     return sat_pos - sat_vel * pr.reshape(-1, 1) / 299792458","metadata":{"execution":{"iopub.status.busy":"2021-06-15T06:10:01.809218Z","iopub.execute_input":"2021-06-15T06:10:01.809793Z","iopub.status.idle":"2021-06-15T06:10:01.816007Z","shell.execute_reply.started":"2021-06-15T06:10:01.809757Z","shell.execute_reply":"2021-06-15T06:10:01.815206Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def calc_pos_fix(sat_pos, pr, weights=1, x0=[0, 0, 0, 0]):\n    '''\n    Calculates gps fix with WLS optimizer\n    returns:\n    0 -> list with positions\n    1 -> pseudorange errs\n    '''\n    n = len(pr)\n    if n < 3:\n        return x0, []\n    sat_pos = rotate_sat(sat_pos, pr / 299792458)\n    Fx_pos = pr_residual(sat_pos, pr, weights=weights)\n    # bounds = (np.array([-np.inf, -np.inf, -np.inf, -100]), np.array([np.inf, np.inf, np.inf, 100]))\n    # opt_pos = opt.least_squares(Fx_pos, x0, bounds=bounds).x\n    opt_pos = opt.least_squares(Fx_pos, x0).x\n    return opt_pos, Fx_pos(opt_pos, weights=1)\n\n\ndef pr_residual(sat_pos, pr, weights=1):\n    # solve for pos\n    def Fx_pos(x_hat, weights=weights):\n        rows = weights * (np.linalg.norm(sat_pos - x_hat[:3], axis=1) + x_hat[3] - pr)\n        # rows = weights * (np.linalg.norm(rotate_sat(sat_pos, pr / 299792458, x_hat[3] / 299792458) - x_hat[:3], axis=1) + x_hat[3] - pr)\n        return rows\n    return Fx_pos","metadata":{"execution":{"iopub.status.busy":"2021-06-15T06:10:02.499741Z","iopub.execute_input":"2021-06-15T06:10:02.500346Z","iopub.status.idle":"2021-06-15T06:10:02.509251Z","shell.execute_reply.started":"2021-06-15T06:10:02.500295Z","shell.execute_reply":"2021-06-15T06:10:02.508470Z"},"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","metadata":{"execution":{"iopub.status.busy":"2021-06-15T06:10:02.716001Z","iopub.execute_input":"2021-06-15T06:10:02.716356Z","iopub.status.idle":"2021-06-15T06:10:02.723010Z","shell.execute_reply.started":"2021-06-15T06:10:02.716326Z","shell.execute_reply":"2021-06-15T06:10:02.721896Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def percentile50(x):\n    return np.percentile(x, 50)\ndef percentile95(x):\n    return np.percentile(x, 95)\n\ndef get_train_score(df, gt):\n    gt = gt.rename(columns={'latDeg':'latDeg_gt', 'lngDeg':'lngDeg_gt'})\n    df = df.merge(gt, on=['collectionName', 'phoneName', 'millisSinceGpsEpoch'], how='inner')\n    # calc_distance_error\n    df['err'] = calc_haversine(df['latDeg_gt'], df['lngDeg_gt'], df['latDeg'], df['lngDeg'])\n    # calc_evaluate_score\n    df['phone'] = df['collectionName'] + '_' + df['phoneName']\n    res = df.groupby('phone')['err'].agg([percentile50, percentile95])\n    res['p50_p90_mean'] = (res['percentile50'] + res['percentile95']) / 2 \n    score = res['p50_p90_mean'].mean()\n    return score","metadata":{"execution":{"iopub.status.busy":"2021-06-15T06:10:03.047143Z","iopub.execute_input":"2021-06-15T06:10:03.047700Z","iopub.status.idle":"2021-06-15T06:10:03.054879Z","shell.execute_reply.started":"2021-06-15T06:10:03.047654Z","shell.execute_reply":"2021-06-15T06:10:03.054113Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def estimation_pipeline(df_trails):\n    \"\"\" simple pipeline to estimate the GNSS receiver location by least square\n    Args:\n    df_trails: the df read from derived file\n\n    Returns:\n    result df with estimated degrees and heights\n    \"\"\"\n    df_trails[\"correctedPrM\"] = df_trails[\"rawPrM\"] + df_trails[\"satClkBiasM\"] - df_trails[\"isrbM\"] - df_trails[\"ionoDelayM\"] - df_trails[\"tropoDelayM\"]\n\n    results = []\n    results_bias = []\n    x = [0, 0, 0, 0]\n    ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')\n    lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')\n    \n    df_epochs = df_trails.groupby([\"collectionName\", \"phoneName\", \"millisSinceGpsEpoch\"])\n    for i, (indices, df_epoch) in enumerate(tqdm(df_epochs, desc=\"Estimate location by LS for epoch\")):\n        # if indices[0] == '2020-05-14-US-MTV-1':\n        #     if len(results_bias) > 1000:\n        #         break\n        # else:\n        #     continue\n        sat_pos = df_epoch[[\"xSatPosM\",\"ySatPosM\",\"zSatPosM\"]].to_numpy()\n        pseudoranges = np.squeeze(df_epoch[[\"correctedPrM\"]].to_numpy())\n        pseudoranges_sigma = np.squeeze(df_epoch[[\"rawPrUncM\"]].to_numpy())\n        x, _ = calc_pos_fix(sat_pos, pseudoranges, 1/pseudoranges_sigma, x)\n        # values = np.squeeze(pyproj.transform(ecef, lla, *x[:3], radians=False))\n        values = np.squeeze(ecef2lla(*x[:3]))\n        results.append([*indices, *values])\n        # results_bias.append(x[3])\n    return pd.DataFrame(results,columns=[\"collectionName\", \"phoneName\", \"millisSinceGpsEpoch\", \"latDeg\", \"lngDeg\", \"heightAboveWgs84EllipsoidM\"])\n            # , results_bias","metadata":{"execution":{"iopub.status.busy":"2021-06-15T06:10:03.438237Z","iopub.execute_input":"2021-06-15T06:10:03.438860Z","iopub.status.idle":"2021-06-15T06:10:03.449103Z","shell.execute_reply.started":"2021-06-15T06:10:03.438817Z","shell.execute_reply":"2021-06-15T06:10:03.448050Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Estimate whole GNSS\n","metadata":{}},{"cell_type":"code","source":"datapath = Path(\"../input/google-smartphone-decimeter-challenge/\")\nground_truths = (datapath / \"train\").rglob(\"ground_truth.csv\")\ndrived_files = (datapath / \"train\").rglob(\"*_derived.csv\")\n\ndf_gt = pd.concat([pd.read_csv(filepath) for filepath in tqdm(ground_truths, total=73, desc=\"Reading ground truth data\")], ignore_index=True)\ndf_baseline = pd.read_csv(datapath / 'baseline_locations_train.csv')\ndf_derived = pd.concat([pd.read_csv(filepath) for filepath in tqdm(drived_files, total=73, desc=\"Reading drived data\")], ignore_index=True)","metadata":{"execution":{"iopub.status.busy":"2021-06-15T06:10:04.144262Z","iopub.execute_input":"2021-06-15T06:10:04.144829Z","iopub.status.idle":"2021-06-15T06:10:20.658618Z","shell.execute_reply.started":"2021-06-15T06:10:04.144779Z","shell.execute_reply":"2021-06-15T06:10:20.657884Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"df_gt[\"receivedSvTimeInGpsNanos\"] = df_gt.millisSinceGpsEpoch*int(1e6)\ndf_derived_raw = df_derived.drop(\"millisSinceGpsEpoch\", axis=1)\n\ndf_merge = pd.merge_asof(df_derived_raw.sort_values('receivedSvTimeInGpsNanos'), df_gt.sort_values('receivedSvTimeInGpsNanos'), \n                                           on=\"receivedSvTimeInGpsNanos\", by=[\"collectionName\", \"phoneName\"], direction='nearest',tolerance=int(1e9))\ndf_merge = df_merge.sort_values(by=[\"collectionName\", \"phoneName\", \"millisSinceGpsEpoch\"], ignore_index=True)","metadata":{"execution":{"iopub.status.busy":"2021-06-15T06:10:20.659860Z","iopub.execute_input":"2021-06-15T06:10:20.660265Z","iopub.status.idle":"2021-06-15T06:10:34.170197Z","shell.execute_reply.started":"2021-06-15T06:10:20.660229Z","shell.execute_reply":"2021-06-15T06:10:34.169017Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# df_estimate = estimation_pipeline(df_merge)\n# df_sample_trails_estimate, results_bias = simple_pipeline(df_merge)","metadata":{"execution":{"iopub.status.busy":"2021-06-15T03:24:12.330538Z","iopub.execute_input":"2021-06-15T03:24:12.330926Z","iopub.status.idle":"2021-06-15T03:25:39.571788Z","shell.execute_reply.started":"2021-06-15T03:24:12.330894Z","shell.execute_reply":"2021-06-15T03:25:39.569959Z"},"collapsed":true,"jupyter":{"outputs_hidden":true},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"df_merged_baseline = pd.merge_asof(df_gt.sort_values('millisSinceGpsEpoch'),\n                                 df_baseline.sort_values('millisSinceGpsEpoch'), \n                                 on=\"millisSinceGpsEpoch\", by=[\"collectionName\", \"phoneName\"], \n                                 direction='nearest',tolerance=100000, suffixes=('_truth', '_pred'))\ndf_merged_baseline = df_merged_baseline.sort_values(by=[\"collectionName\", \"phoneName\", \"millisSinceGpsEpoch\"], ignore_index=True)\n\ndf_merged_SL = pd.merge_asof(df_gt.sort_values('millisSinceGpsEpoch'), \n                           df_estimate.sort_values('millisSinceGpsEpoch'), \n                           on=\"millisSinceGpsEpoch\", by=[\"collectionName\", \"phoneName\"], \n                           direction='nearest',tolerance=100000, suffixes=('_truth', '_pred'))\ndf_merged_SL = df_merged_SL.sort_values(by=[\"collectionName\", \"phoneName\", \"millisSinceGpsEpoch\"], ignore_index=True)\n\ncompared_cols = [\"latDeg_truth\",\"lngDeg_truth\",\"latDeg_pred\",\"lngDeg_pred\"]\nprint(\"Weighted Least Square (baseline) haversine distance (M):\", calc_haversine(*df_merged_baseline[compared_cols].to_numpy().transpose()).mean())\nprint(\"Weighted Least Square haversine distance (M):\", calc_haversine(*df_merged_SL[compared_cols].to_numpy().transpose()).mean())","metadata":{"execution":{"iopub.status.busy":"2021-06-15T03:25:39.572847Z","iopub.status.idle":"2021-06-15T03:25:39.573305Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"df_baseline = df_baseline.drop([\"latDeg\",\"lngDeg\",\"heightAboveWgs84EllipsoidM\"], axis=1)\ndf_merged = pd.merge_asof(df_baseline.sort_values('millisSinceGpsEpoch'), \n                            df_estimate.sort_values('millisSinceGpsEpoch'), \n                            on=\"millisSinceGpsEpoch\", by=[\"collectionName\", \"phoneName\"], direction='nearest', tolerance=100000)\ndf_merged = df_merged.sort_values(by=[\"phone\", \"millisSinceGpsEpoch\"], ignore_index=True)\n\ndf_submission = df_merged[[\"phone\", \"millisSinceGpsEpoch\", \"latDeg\", \"lngDeg\"]].copy()\ndf_submission.to_csv('submission.csv', index=False)","metadata":{"execution":{"iopub.status.busy":"2021-05-26T01:47:45.434341Z","iopub.execute_input":"2021-05-26T01:47:45.434635Z","iopub.status.idle":"2021-05-26T01:47:46.037209Z","shell.execute_reply.started":"2021-05-26T01:47:45.434608Z","shell.execute_reply":"2021-05-26T01:47:46.035729Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"df_baseline = df_baseline.drop([\"latDeg\",\"lngDeg\",\"heightAboveWgs84EllipsoidM\"], axis=1)\ndf_merged = pd.merge_asof(df_baseline.sort_values('millisSinceGpsEpoch'), \n                            df_estimate.sort_values('millisSinceGpsEpoch'), \n                            on=\"millisSinceGpsEpoch\", by=[\"collectionName\", \"phoneName\"], direction='nearest', tolerance=100000)\ndf_merged = df_merged.sort_values(by=[\"phone\", \"millisSinceGpsEpoch\"], ignore_index=True)\n\ndf_submission = df_merged[[\"phone\", \"millisSinceGpsEpoch\", \"latDeg\", \"lngDeg\"]].copy()\ndf_submission.to_csv('submission.csv', index=False)","metadata":{"execution":{"iopub.status.busy":"2021-05-26T01:47:45.434341Z","iopub.execute_input":"2021-05-26T01:47:45.434635Z","iopub.status.idle":"2021-05-26T01:47:46.037209Z","shell.execute_reply.started":"2021-05-26T01:47:45.434608Z","shell.execute_reply":"2021-05-26T01:47:46.035729Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"df_baseline = df_baseline.drop([\"latDeg\",\"lngDeg\",\"heightAboveWgs84EllipsoidM\"], axis=1)\ndf_merged = pd.merge_asof(df_baseline.sort_values('millisSinceGpsEpoch'), \n                            df_estimate.sort_values('millisSinceGpsEpoch'), \n                            on=\"millisSinceGpsEpoch\", by=[\"collectionName\", \"phoneName\"], direction='nearest', tolerance=100000)\ndf_merged = df_merged.sort_values(by=[\"phone\", \"millisSinceGpsEpoch\"], ignore_index=True)\n\ndf_submission = df_merged[[\"phone\", \"millisSinceGpsEpoch\", \"latDeg\", \"lngDeg\"]].copy()\ndf_submission.to_csv('submission.csv', index=False)","metadata":{"execution":{"iopub.status.busy":"2021-05-26T01:47:45.434341Z","iopub.execute_input":"2021-05-26T01:47:45.434635Z","iopub.status.idle":"2021-05-26T01:47:46.037209Z","shell.execute_reply.started":"2021-05-26T01:47:45.434608Z","shell.execute_reply":"2021-05-26T01:47:46.035729Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"df_baseline = df_baseline.drop([\"latDeg\",\"lngDeg\",\"heightAboveWgs84EllipsoidM\"], axis=1)\ndf_merged = pd.merge_asof(df_baseline.sort_values('millisSinceGpsEpoch'), \n                            df_estimate.sort_values('millisSinceGpsEpoch'), \n                            on=\"millisSinceGpsEpoch\", by=[\"collectionName\", \"phoneName\"], direction='nearest', tolerance=100000)\ndf_merged = df_merged.sort_values(by=[\"phone\", \"millisSinceGpsEpoch\"], ignore_index=True)\n\ndf_submission = df_merged[[\"phone\", \"millisSinceGpsEpoch\", \"latDeg\", \"lngDeg\"]].copy()\ndf_submission.to_csv('submission.csv', index=False)","metadata":{"execution":{"iopub.status.busy":"2021-05-26T01:47:45.434341Z","iopub.execute_input":"2021-05-26T01:47:45.434635Z","iopub.status.idle":"2021-05-26T01:47:46.037209Z","shell.execute_reply.started":"2021-05-26T01:47:45.434608Z","shell.execute_reply":"2021-05-26T01:47:46.035729Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"df_baseline = df_baseline.drop([\"latDeg\",\"lngDeg\",\"heightAboveWgs84EllipsoidM\"], axis=1)\ndf_merged = pd.merge_asof(df_baseline.sort_values('millisSinceGpsEpoch'), \n                            df_estimate.sort_values('millisSinceGpsEpoch'), \n                            on=\"millisSinceGpsEpoch\", by=[\"collectionName\", \"phoneName\"], direction='nearest', tolerance=100000)\ndf_merged = df_merged.sort_values(by=[\"phone\", \"millisSinceGpsEpoch\"], ignore_index=True)\n\ndf_submission = df_merged[[\"phone\", \"millisSinceGpsEpoch\", \"latDeg\", \"lngDeg\"]].copy()\ndf_submission.to_csv('submission.csv', index=False)","metadata":{"execution":{"iopub.status.busy":"2021-05-26T01:47:45.434341Z","iopub.execute_input":"2021-05-26T01:47:45.434635Z","iopub.status.idle":"2021-05-26T01:47:46.037209Z","shell.execute_reply.started":"2021-05-26T01:47:45.434608Z","shell.execute_reply":"2021-05-26T01:47:46.035729Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Test Sanjose","metadata":{}},{"cell_type":"code","source":"df_train = pd.read_csv('../input/public-5639-train-test/train_predicted.csv')\n\ndatapath = Path(\"../input/google-smartphone-decimeter-challenge/\")\nground_truths = (datapath / \"train\").rglob(\"ground_truth.csv\")\ndrived_files = (datapath / \"train\").rglob(\"*_derived.csv\")\n\ndf_gt = pd.concat([pd.read_csv(filepath) for filepath in tqdm(ground_truths, total=73, desc=\"Reading ground truth data\")], ignore_index=True)\ndf_baseline = pd.read_csv(datapath / 'baseline_locations_train.csv')\ndf_derived = pd.concat([pd.read_csv(filepath) for filepath in tqdm(drived_files, total=73, desc=\"Reading drived data\")], ignore_index=True)","metadata":{"execution":{"iopub.status.busy":"2021-06-15T04:15:02.268318Z","iopub.execute_input":"2021-06-15T04:15:02.268822Z","iopub.status.idle":"2021-06-15T04:15:03.440973Z","shell.execute_reply.started":"2021-06-15T04:15:02.268789Z","shell.execute_reply":"2021-06-15T04:15:03.44007Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"df_derived_raw = df_derived.drop(\"millisSinceGpsEpoch\", axis=1)\n\ndf_gt[\"receivedSvTimeInGpsNanos\"] = df_gt.millisSinceGpsEpoch*int(1e6)\ndf_merge_train_gt = pd.merge_asof(df_derived_raw.sort_values('receivedSvTimeInGpsNanos'), df_gt.sort_values('receivedSvTimeInGpsNanos'), \n                                           on=\"receivedSvTimeInGpsNanos\", by=[\"collectionName\", \"phoneName\"], direction='nearest',tolerance=int(1e9))\ndf_merge_train_gt = df_merge_train_gt.sort_values(by=[\"collectionName\", \"phoneName\", \"millisSinceGpsEpoch\"], ignore_index=True)\n\n\ndf_train[\"receivedSvTimeInGpsNanos\"] = df_train.millisSinceGpsEpoch*int(1e6)\ndf_merge_train = pd.merge_asof(df_derived_raw.sort_values('receivedSvTimeInGpsNanos'), df_train.sort_values('receivedSvTimeInGpsNanos'), \n                                           on=\"receivedSvTimeInGpsNanos\", by=[\"collectionName\", \"phoneName\"], direction='nearest',tolerance=int(1e9))\ndf_merge_train = df_merge_train.sort_values(by=[\"collectionName\", \"phoneName\", \"millisSinceGpsEpoch\"], ignore_index=True)","metadata":{"execution":{"iopub.status.busy":"2021-06-15T04:40:58.536869Z","iopub.execute_input":"2021-06-15T04:40:58.537311Z","iopub.status.idle":"2021-06-15T04:41:24.900073Z","shell.execute_reply.started":"2021-06-15T04:40:58.537274Z","shell.execute_reply":"2021-06-15T04:41:24.898882Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def get_bias(df_merge_traj):\n    ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')\n    lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')\n\n    pos_sat = df_merge_traj[['xSatPosM', 'ySatPosM', 'zSatPosM']].values\n\n    lonlatalt = df_merge_traj[['lngDeg', 'latDeg', 'heightAboveWgs84EllipsoidM']].values\n    xyz = pyproj.transform(lla, ecef, *lonlatalt.transpose(), radians=False)\n    pos_phone = np.vstack(xyz).transpose()\n    \n    correctedPrM = df_merge_traj[\"rawPrM\"] + df_merge_traj[\"satClkBiasM\"] - df_merge_traj[\"isrbM\"] - df_merge_traj[\"ionoDelayM\"] - df_merge_traj[\"tropoDelayM\"]\n    pr = np.squeeze(correctedPrM.values)\n\n    bias = pr - np.linalg.norm(pos_sat - pos_phone, axis=1)\n    return bias","metadata":{"execution":{"iopub.status.busy":"2021-06-15T04:48:37.31441Z","iopub.execute_input":"2021-06-15T04:48:37.314833Z","iopub.status.idle":"2021-06-15T04:48:37.323209Z","shell.execute_reply.started":"2021-06-15T04:48:37.314794Z","shell.execute_reply":"2021-06-15T04:48:37.322065Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"col = '2021-04-28-US-SJC-1'\n# col = '2020-05-14-US-MTV-1'\nph = 'Pixel4'\ndf_train_traj = df_train[(df_train['collectionName']==col) & (df_train['phoneName']==ph)]\ndf_gt_traj = df_gt[(df_gt['collectionName']==col) & (df_gt['phoneName']==ph)]\n\nfor (collection, phone), df_merge_gt_traj in df_merge_train_gt.groupby(['collectionName', 'phoneName']):\n    if collection == col:\n        if phone == ph:\n            break\n        \nfor (collection, phone), df_merge_train_traj in df_merge_train.groupby(['collectionName', 'phoneName']):\n    if collection == col:\n        if phone == ph:\n            break","metadata":{"execution":{"iopub.status.busy":"2021-06-15T04:56:19.872909Z","iopub.execute_input":"2021-06-15T04:56:19.873274Z","iopub.status.idle":"2021-06-15T04:56:22.591679Z","shell.execute_reply.started":"2021-06-15T04:56:19.873244Z","shell.execute_reply":"2021-06-15T04:56:22.590803Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"bias_gt = get_bias(df_merge_gt_traj)\nbias_train = get_bias(df_merge_train_traj)\n\ndf_bias = pd.DataFrame()\ndf_bias['millisSinceGpsEpoch'] = df_merge_gt_traj['millisSinceGpsEpoch']\ndf_bias['bias_gt'] = bias_gt\ndf_bias['bias_train'] = bias_train\ndf_bias_std = df_bias.groupby('millisSinceGpsEpoch').std()\ndf_bias_median = df_bias.groupby('millisSinceGpsEpoch').median()\n\ndf_bias['bias_gt'].describe()","metadata":{"execution":{"iopub.status.busy":"2021-06-15T04:57:36.934351Z","iopub.execute_input":"2021-06-15T04:57:36.934731Z","iopub.status.idle":"2021-06-15T04:57:37.045229Z","shell.execute_reply.started":"2021-06-15T04:57:36.934698Z","shell.execute_reply":"2021-06-15T04:57:37.044081Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"dif = np.linalg.norm(df_gt_traj[['latDeg', 'lngDeg']].values - df_train_traj[['latDeg', 'lngDeg']].values, axis=1)\n\nfig = plt.figure(figsize=(20, 10))\nax = fig.add_subplot(311)\nax.scatter(df_merge_gt_traj['millisSinceGpsEpoch'].values, bias_gt, s=1)\n\nax = fig.add_subplot(312)\nax.scatter(df_merge_gt_traj['millisSinceGpsEpoch'].values, bias_train, s=1)\nax.plot(df_bias_median.index.values,\n        # df_bias.groupby('millisSinceGpsEpoch').apply(lambda x: stats.trim_mean(x['bias_train'], 0.3)),\n        df_bias_median['bias_train'].rolling(5).mean(),\n        label='bias std (train)',\n        color='red')\nax.legend()\n\nax = fig.add_subplot(313)\nax.plot(df_bias_std.index.values, df_bias_std['bias_train'], label='bias std (train)')\n\nax.plot(df_bias_std.index.values, df_bias_std['bias_gt'], label='bias std (gt)', linestyle='--')\nax.plot(df_gt_traj['millisSinceGpsEpoch'].values, dif * 1e6, label='error')\nax.legend()\n\n# ax = fig.add_subplot(313)\n# ax.plot(df_gt_traj['millisSinceGpsEpoch'].values, dif)","metadata":{"execution":{"iopub.status.busy":"2021-06-15T05:06:28.464723Z","iopub.execute_input":"2021-06-15T05:06:28.465172Z","iopub.status.idle":"2021-06-15T05:06:29.439346Z","shell.execute_reply.started":"2021-06-15T05:06:28.465135Z","shell.execute_reply":"2021-06-15T05:06:29.438391Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def snap_to_grid_gnss(df_):\n    ","metadata":{},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def apply_dtw_snap_to_grid(df_input, df_gt, collections_to_snap = None, th_dtw=30.0 / 100_000, th_snap=10.0 / 100_000):\n    df_snapped = df_input.copy()\n    collections = df_snapped['collectionName'].unique()\n    for collection in tqdm(collections, desc = 'Apply DTW snap-to-grid (for {} trajs)'.format(\"whole\" if collections_to_snap is None else len(collections_to_snap))):\n        if collections_to_snap is not None:\n            if collection not in collections_to_snap:\n                continue\n        cond_col = df_snapped['collectionName'] == collection\n        phones = df_snapped[cond_col]['phoneName'].unique()\n        for phone in phones:\n            cond_traj = cond_col & (df_snapped['phoneName'] == phone)\n            time_traj = df_snapped[cond_traj]['millisSinceGpsEpoch'].values\n            latlng_traj = df_snapped[cond_traj][['latDeg', 'lngDeg']].values\n\n            segment_ids_list = get_segment_ids(latlng_traj, base_len=60, stride=30)\n            \n            snapped_count_list = np.zeros_like(time_traj)\n            snapped_results_list = np.zeros((len(time_traj), 2))\n            \n            for ids in segment_ids_list:\n                cond_seg = np.arange(0, latlng_traj.shape[0], 1)\n                cond_seg = (cond_seg >= ids[0]) & (cond_seg < ids[1])\n                latlng_seg = latlng_traj[cond_seg]\n                snapped_count_list += cond_seg\n                subtraj_opt, _ = search_closest_subtraj(latlng_seg, df_gt, threshold=th_dtw)\n                if subtraj_opt is not None: # もし良い感じのが見つかったら、、、\n                    grids = increase_points_array(subtraj_opt)\n                    snapped_latlng = snap_to_grid_gnss(latlng_seg, grids, derived, threshold=th_snap)\n                    snapped_results_list[ids[0]:ids[1]] += snapped_latlng\n                else:\n                    snapped_results_list[ids[0]:ids[1]] += latlng_seg\n            \n            # Add original value if never counted.\n            snapped_results_list[snapped_count_list == 0] += latlng_traj[snapped_count_list == 0]\n            snapped_count_list[snapped_count_list == 0] += 1\n\n            df_snapped.loc[cond_traj, ['latDeg', 'lngDeg']] = snapped_results_list / snapped_count_list.reshape(-1, 1)\n    return df_snapped\n\n\ndef search_closest_subtraj(latlng, df_gt, threshold=30/100_000, expand_idx_len=10):\n    \n    dist_opt = 1e9\n    subtraj_opt = None\n    for collection, df_col in df_gt.groupby('collectionName'):\n        # Phoneは1つだけ使えばOK\n        phone_here = df_col['phoneName'].unique()[0]\n        df_traj = df_col[df_col['phoneName'] == phone_here]\n        latlng_traj = df_traj[['latDeg', 'lngDeg']].values\n        \n        dist_start = np.linalg.norm(latlng_traj - latlng[0, :], axis=1)\n        dist_end = np.linalg.norm(latlng_traj - latlng[-1, :], axis=1)\n        start_cand_idx = np.where(dist_start < threshold)[0]\n        end_cand_idx = np.where(dist_end < threshold)[0]\n\n        for start_idx in reduce_array(start_cand_idx):\n            for end_idx in reduce_array(end_cand_idx):\n                if start_idx >= end_idx:\n                    continue\n                dist = calc_dtw(latlng[::20], latlng_traj[start_idx: end_idx][::20])[-1][-1][0]\n                # try:\n                #     dist = calc_dtw(latlng[::20], latlng_traj[start_idx: end_idx][::20])[-1][-1][0]\n                # except IndexError as e:\n                #     print(start_idx, end_idx)\n                if dist_opt > dist:\n                    dist_opt = dist\n                    subtraj_opt = latlng_traj[max(start_idx-expand_idx_len, 0): end_idx+expand_idx_len]\n\n    return subtraj_opt, dist_opt\n\n\ndef increase_points_array(data, min_dist=0.2/100_000):\n    data_increased = []\n    for i, point in enumerate(data):\n        if i == data.shape[0] - 1:\n            continue\n        data_increased.append(point)\n        if np.array_equal(data[i, 1:], data[i + 1, 1:]):\n            continue\n        latDeg0 = data[i, 0]\n        lngDeg0 = data[i, 1]\n        latDeg1 = data[i+1, 0]\n        lngDeg1 = data[i+1, 1]\n        num_to_increase = np.ceil(np.linalg.norm([latDeg0 - latDeg1, lngDeg0 - lngDeg1]) / min_dist)\n        for j in range(int(num_to_increase)):\n            latDeg = ((num_to_increase-j) * latDeg0 + j * latDeg1) / num_to_increase\n            lngDeg = ((num_to_increase-j) * lngDeg0 + j * lngDeg1) / num_to_increase\n            data_increased.append([latDeg, lngDeg])\n    data_increased = np.array(data_increased)\n    return data_increased\n\n\ndef reduce_array(array, length=50):\n    reduced = []\n    for idx in array:\n        if len(reduced) == 0:\n            reduced.append(idx)\n        else:\n            if reduced[-1] + length <= idx:\n                reduced.append(idx)\n    return reduced\n\n\ndef get_segment_ids(latlng, base_len=60, stride=30):\n    segment_ids = []\n    start = 0\n    end = start + base_len\n    while True:\n        if np.linalg.norm(latlng[min(end, latlng.shape[0]-1), :] - latlng[start, :]) > 200/100_000: \n            segment_ids.append([start, end])\n            start = end - stride\n            end = start + base_len\n        else:\n            end += stride\n        if end >= len(latlng):\n            segment_ids.append([latlng.shape[0]-1 - 250, latlng.shape[0]-1])\n            break\n    return segment_ids","metadata":{},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def snap_to_grid_gnss(latlng_seg, grids, derived, threshold=10/100_000):\n    latlng_snapped = copy.copy(latlng_seg)\n    for epoch in epochs:\n        if min_dist > threshold: # もし距離の近いものがなければそもそも無視。\n            continue\n        loss_function = get_loss_function(derived_epoch)\n        opt_pos = argmin loss_function([grids, bias], weights=weights)\n        latlng_snapped[HOGE] = opt_pos\n    return latlng_snapped","metadata":{},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"df_train_traj","metadata":{"execution":{"iopub.status.busy":"2021-06-15T06:09:53.592848Z","iopub.execute_input":"2021-06-15T06:09:53.593319Z","iopub.status.idle":"2021-06-15T06:09:53.666871Z","shell.execute_reply.started":"2021-06-15T06:09:53.593222Z","shell.execute_reply":"2021-06-15T06:09:53.665529Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{},"execution_count":null,"outputs":[]}]}