{"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":"from glob import glob\nimport numpy as np\nimport pandas as pd\nimport math\nimport scipy.interpolate\nimport scipy.sparse\nfrom scipy.sparse import linalg\nfrom tqdm import tqdm\nfrom joblib import Parallel,delayed\nimport yaml\n\nfrom scipy.spatial import distance\nimport matplotlib.pyplot as plt\nimport seaborn as sns\n\npd.set_option('display.max_colwidth', None)","metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","_kg_hide-input":true,"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def compute_l2dist(sub1, sub2):\n    l2dist = np.sqrt((sub1.x.values-sub2.x.values)**2 +(sub1.y.values-sub2.y.values)**2)\n    return np.mean(l2dist)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"***","metadata":{}},{"cell_type":"code","source":"#submission = pd.read_csv(\"../input/iln-cnn1d-dset2/cnn1d-ds2-30f-5lt-30seq/submission.csv\")\nsub_cnn1d = pd.read_csv(\"../input/iln-cnn1d-dset2/cnn1d-ds2-30f-5lt-30seq-pl/submission.csv\")\nsub_tabnet = pd.read_csv(\"../input/iln-tabnet-dset2/tabnet-ds2-30f-5lt-30seq-pl/submission.csv\")\nsub_lgbm = pd.read_csv(\"../input/iln-lgbm-dset2/submission.csv\")","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"submission = sub_cnn1d.copy(deep=True)\nweights = [0.195101, 0.25560842, 0.54929058]\nprint(np.sum(weights))\n\nsubmission[\"x\"] = weights[0]*sub_cnn1d[\"x\"] + weights[1]*sub_tabnet[\"x\"] + weights[2]*sub_lgbm[\"x\"]\nsubmission[\"y\"] = weights[0]*sub_cnn1d[\"y\"] + weights[1]*sub_tabnet[\"y\"] + weights[2]*sub_lgbm[\"y\"]\n\nassert submission[[\"x\",\"y\"]].isna().sum(axis=0).sum() == 0\n\nsubmission.to_csv(\"./submission.csv\", index=False)\nsubmission","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"submission[\"site\"] = submission.site_path_timestamp.apply(lambda x: x.split(\"_\")[0])\nsubmission[\"path\"] = submission.site_path_timestamp.apply(lambda x: x.split(\"_\")[1])\nsubmission[\"timestamp\"] = submission.site_path_timestamp.apply(lambda x: x.split(\"_\")[2]).astype(int)\nsubmission","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"target_sites = submission.site.unique()\ndeltas = pd.read_csv(\"../input/iln-imu-predictions/delta_preds_test.csv\")","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"floor_map = {\n    \"B2\":-2, \"B1\":-1, \n    \"F1\":0, \"F2\":1, \"F3\":2, \"F4\":3, \"F5\":4, \"F6\":5, \"F7\":6, \"F8\":7, \"F9\":8,\n    \"1F\":0, \"2F\":1, \"3F\":2, \"4F\":3, \"5F\":4, \"6F\":5, \"7F\":6, \"8F\":7, \"9F\":8\n}\nwaypoints = pd.read_csv(\"../input/iln-waypoints/waypoints.csv\")\nwaypoints[\"floor\"] = waypoints[\"floor\"].map(floor_map)\nwaypoints = waypoints.loc[:,[\"site\",\"floor\",\"x\",\"y\"]].drop_duplicates(ignore_index=True)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"#waypoints_aug = pd.read_csv(\"../input/indoor-navigation-hand-labeled-waypoints/waypoint_by_hand.csv\")\n#waypoints = pd.concat([waypoints, waypoints_aug], axis=0, ignore_index=True)\n#waypoints","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"submission.rename({\"floor\":\"_floor\"}, axis=1, inplace=True)\nwaypoints.rename({\"floor\":\"_floor\"}, axis=1, inplace=True)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"leaks = pd.read_csv(\"../input/ilnleaksbackup/leaks.csv\")\nleaks","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# assert that floors are correct\nfloors = pd.read_csv(\"../input/iln-floor-corrections/floor_predictions.csv\")\nfloors.rename({\"floor\":\"floor_corr\"}, axis=1, inplace=True)\nmrg = pd.merge(submission, floors, how=\"inner\", on=[\"site\",\"path\"])\nassert (mrg[\"_floor\"] == mrg[\"floor_corr\"]).all()","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"***\n### iterative correction","metadata":{}},{"cell_type":"code","source":"def plot_subs(sub_raw, sub_corr, waypoints):\n    site = sub_raw.site.values[0]\n    floor = sub_raw._floor.values[0]\n\n    x_min = math.ceil(sub_raw.x.min() - 15)\n    x_max = math.floor(sub_raw.x.max() + 15)\n    y_min = math.ceil(sub_raw.y.min() - 15)\n    y_max = math.floor(sub_raw.y.max() + 15)\n    near_wps = waypoints.query(\"site==@site & _floor==@floor\").query(\"@x_min <= x <= @x_max\").query(\"@y_min <= y <= @y_max\")\n\n    print(f\"\\n{path}\")\n    plt.figure(figsize=(20,9))\n    plt.subplot(1,2,1)\n    plt.plot(sub_raw.x, sub_raw.y, \"o--\", label=\"pred\")\n    plt.grid()\n    plt.legend(loc=\"best\")\n    plt.scatter(near_wps.x, near_wps.y, marker=\"*\", s=150, c=\"r\")\n    plt.xlabel(\"x\")\n    plt.ylabel(\"y\")\n    plt.xticks(np.arange(x_min, x_max+1, 5.0))\n    plt.yticks(np.arange(y_min, y_max+1, 5.0))\n\n    plt.subplot(1,2,2)\n    plt.plot(sub_corr.x, sub_corr.y, \"o--\", label=\"pred\")\n    plt.grid()\n    plt.legend(loc=\"best\")\n    plt.scatter(near_wps.x, near_wps.y, marker=\"*\", s=150, c=\"r\")\n    plt.xlabel(\"x\")\n    plt.ylabel(\"y\")\n    plt.xticks(np.arange(x_min, x_max+1, 5.0))\n    plt.yticks(np.arange(y_min, y_max+1, 5.0))\n\n    plt.show()","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def add_closest_waypoint(preds, waypoints):\n    preds = preds.copy()\n    if 'x_wp' in preds.columns:\n        preds = preds.drop(\"x_wp\", axis=1)\n    if 'y_wp' in preds.columns:\n        preds = preds.drop(\"y_wp\", axis=1)\n        \n    dist_matrix = distance.cdist(preds[[\"x\",\"y\"]], waypoints[[\"x\",\"y\"]].values)\n    snap_idx = preds.query(\"snap\").index.values\n\n    n = len(preds)\n    closest_idx = list()\n    closest_dist = list()\n\n    for i in range(n):\n        if i in snap_idx:\n            c_idx = np.argmin(dist_matrix[i,:])\n            closest_idx.append(c_idx)\n            closest_dist.append(dist_matrix[i,c_idx])\n            continue\n        else:\n            if i-1 in snap_idx:\n                idx_min = np.argmin(dist_matrix[i-1,:])\n                dist_matrix[i,idx_min] = np.inf\n            if i+1 in snap_idx:\n                idx_min = np.argmin(dist_matrix[i+1,:])\n                dist_matrix[i,idx_min] = np.inf\n        c_idx = np.argmin(dist_matrix[i,:])\n        closest_idx.append(c_idx)\n        closest_dist.append(dist_matrix[i,c_idx])\n        \n    closest_wp = waypoints[[\"x\",\"y\"]].values[closest_idx]\n    closest_wp_dist = np.array(closest_dist)\n\n    preds[\"x_wp\"] = closest_wp[:,0]\n    preds[\"y_wp\"] = closest_wp[:,1]\n    preds[\"dist\"] = closest_wp_dist\n\n    return preds\n\ndef s2g_by_idx(pred, idx):\n    pred = pred.copy()\n    pred.loc[idx,\"snap\"] = True\n\n    # avoid snap two consecutive preds to the same waypoint\n    x_wp = pred.loc[idx,\"x_wp\"]\n    y_wp = pred.loc[idx,\"y_wp\"]\n    wp_index = pred.query(\"x_wp==@x_wp & y_wp==@y_wp & snap\").index\n    if (idx-1 in wp_index) or (idx+1 in wp_index):\n        return pred\n    else:\n        pred.loc[idx,\"x\"] = pred.loc[idx,\"x_wp\"]\n        pred.loc[idx,\"y\"] = pred.loc[idx,\"y_wp\"]\n        return pred\n\ndef correct_path(wp_preds, delta_preds, fixed_idx, lambda1=0.1, lambda2=0.5, lambda3=2):\n\n    T_ref  = wp_preds['timestamp'].values\n    xy_preds = wp_preds[['x', 'y']].values\n    delta_preds = delta_preds.loc[:,[\"timestamp\",\"dx\",\"dy\"]].values\n\n    if T_ref[-1] > delta_preds[-1, 0]:\n        delta_preds = [np.array([[0, 0, 0]]), delta_preds, np.array([[T_ref[-1], 0, 0]])]\n    else:\n        delta_preds = [np.array([[0, 0, 0]]), delta_preds]\n    delta_preds = np.concatenate(delta_preds)\n    \n    T_rel = delta_preds[:, 0]\n    delta_xy_preds = np.diff(scipy.interpolate.interp1d(T_rel, np.cumsum(delta_preds[:, 1:3], axis=0), axis=0)(T_ref), axis=0)\n\n    N = xy_preds.shape[0]\n    delta_t = np.diff(T_ref)\n        \n    alpha = lambda1 * np.ones(N)\n    beta  = (1-lambda1) * np.ones(N-1) / (1 + lambda2 * delta_t * 1e-3)**lambda3\n    \n    if fixed_idx is not None:\n        alpha[fixed_idx] = 1e16\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_preds) + (D.T @ (B @ delta_xy_preds))\n    xy_corr = scipy.sparse.linalg.spsolve(Q, c)\n    \n    output = wp_preds.copy()\n    output[\"x\"] = xy_corr[:, 0]\n    output[\"y\"] = xy_corr[:, 1]\n\n    return output\n\ndef iterative_correction(preds, near_waypoints, deltas, influence=1, max_threshold=5, cm_params=None):\n    df_corr = preds.reset_index(drop=True).copy()\n    df_corr[\"snap\"] = False\n\n    while True:\n        if df_corr.snap.all():\n            break\n        df_corr = add_closest_waypoint(df_corr, near_waypoints)\n        target_idx = df_corr.query(\"not snap\").dist.idxmin()\n        threshold = df_corr.query(\"not snap\").dist.min()\n        if threshold > max_threshold:\n            break\n\n        df_corr = s2g_by_idx(df_corr, target_idx)\n        fixed_idx = df_corr.query(\"snap\").index\n        imu_corrected = correct_path(df_corr, deltas, fixed_idx, **cm_params)\n\n        idx_to_correct = np.arange(max(target_idx-influence,0), min(target_idx+influence+1,len(preds)))\n        idx_to_correct = set(idx_to_correct) - set(df_corr.query(\"snap\").index)\n        idx_to_correct = list(idx_to_correct)\n\n        df_corr.loc[idx_to_correct,\"x\"] = imu_corrected.loc[idx_to_correct,\"x\"]\n        df_corr.loc[idx_to_correct,\"y\"] = imu_corrected.loc[idx_to_correct,\"y\"]\n\n    return df_corr\n\ndef leak_correction(pred, leak, near_waypoints, deltas, influence=2, max_threshold=2, cm_params=None):\n    pred_corr = pred.reset_index(drop=True)\n    pred_corr[\"snap\"] = False\n    pred_corr[\"moved\"] = False\n    target_indexes = list()\n\n    for _,row in leak.iterrows():\n        if (row.st_point_x>=0) & (row.st_point_y>=0):\n            pred_corr.loc[0,\"x\"] = row.st_point_x\n            pred_corr.loc[0,\"y\"] = row.st_point_y\n            pred_corr.loc[0,\"snap\"] = True\n            target_indexes.append(0)\n\n        if (row.en_point_x>=0) & (row.en_point_y>=0):\n            pred_corr.loc[len(pred_corr)-1,\"x\"] = row.en_point_x\n            pred_corr.loc[len(pred_corr)-1,\"y\"] = row.en_point_y\n            pred_corr.loc[len(pred_corr)-1,\"snap\"] = True\n            target_indexes.append(len(pred_corr)-1)\n    \n    fixed_idx = pred_corr.query(\"snap\").index.values\n    imu_corrected = correct_path(pred_corr, deltas, fixed_idx, **cm_params)\n    \n    for target_idx in target_indexes:\n        idx_to_correct = np.arange(max(target_idx-influence,0), min(target_idx+influence+1,len(pred_corr)))\n        idx_to_correct = set(idx_to_correct) - set(pred_corr.query(\"snap\").index)\n        idx_to_correct = list(idx_to_correct)\n        pred_corr.loc[idx_to_correct,\"x\"] = imu_corrected.loc[idx_to_correct,\"x\"]\n        pred_corr.loc[idx_to_correct,\"y\"] = imu_corrected.loc[idx_to_correct,\"y\"]\n        pred_corr.loc[idx_to_correct,\"moved\"] = True\n        \n    influence = 1\n        \n    while True:\n        pred_corr = add_closest_waypoint(pred_corr, near_waypoints)\n        candidates = pred_corr.query(\"moved and not snap\")\n        if len(candidates)==0:\n            break\n        target_idx = candidates.dist.idxmin()\n        threshold  = candidates.dist.min()\n        if threshold >= max_threshold:\n            break\n            \n        pred_corr = s2g_by_idx(pred_corr, target_idx)\n        was_snapped = (pred_corr.loc[target_idx,\"x\"]==pred_corr.loc[target_idx,\"x_wp\"]) & \\\n                      (pred_corr.loc[target_idx,\"y\"]==pred_corr.loc[target_idx,\"y_wp\"])\n        \n        fixed_idx = pred_corr.query(\"snap\").index\n        imu_corrected = correct_path(pred_corr, deltas, fixed_idx, **cm_params)\n        \n        idx_to_correct = np.arange(max(target_idx-influence,0), min(target_idx+influence+1,len(pred_corr)))\n        idx_to_correct = set(idx_to_correct) - set(pred_corr.query(\"snap\").index)\n        idx_to_correct = list(idx_to_correct)\n        \n        pred_corr.loc[idx_to_correct,\"x\"] = imu_corrected.loc[idx_to_correct,\"x\"]\n        pred_corr.loc[idx_to_correct,\"y\"] = imu_corrected.loc[idx_to_correct,\"y\"]\n        pred_corr.loc[idx_to_correct,\"moved\"] = True\n    \n    return pred_corr","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"cm_params = {\n    'lambda1': 0.11931087505938183,\n    'lambda2': 0.9788240879660587, \n    'lambda3': 1.1486681729781796\n}\n\nMAKE_PLOTS = False","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"corrected_preds = list()\n\nfor path in submission.path.unique():\n    \n    pred = submission.query(\"path==@path\")\n    site = pred.site.values[0]\n    floor = pred._floor.values[0]\n\n    x_min = math.ceil(pred.x.min() - 20)\n    x_max = math.floor(pred.x.max() + 20)\n    y_min = math.ceil(pred.y.min() - 20)\n    y_max = math.floor(pred.y.max() + 20)\n    near_wps = (\n        waypoints\n        .query(\"site==@site & _floor==@floor\")\n        .query(\"@x_min <= x <= @x_max\")\n        .query(\"@y_min <= y <= @y_max\")\n    )\n    path_deltas = deltas.query(\"site==@site & path==@path\")\n    leak = leaks.query(\"path == @path\").reset_index(drop=True)\n    \n    if len(leak)>0:\n        pred = leak_correction(pred, leak, near_wps, path_deltas, influence=2, max_threshold=2, cm_params=cm_params)\n    pred_corr = iterative_correction(pred, near_wps, path_deltas, influence=1, max_threshold=5, cm_params=cm_params)\n    corrected_preds.append(pred_corr)\n    \n    if MAKE_PLOTS:\n        plot_subs(pred, pred_corr, waypoints)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"submission = pd.concat(corrected_preds, ignore_index=True)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"cols_to_drop = [\"x_wp\",\"y_wp\",\"snap\",\"moved\",\"dist\", \"site\",\"path\",\"timestamp\"]\nsubmission.drop(cols_to_drop, axis=1, inplace=True)\nsubmission.rename({\"_floor\":\"floor\",\"x_hat\":\"x\", \"y_hat\":\"y\"}, axis=1, inplace=True)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"sub_raw = pd.read_csv(\"../input/indoor-location-navigation/sample_submission.csv\")\nsubmission_ic = pd.merge(sub_raw.loc[:,[\"site_path_timestamp\"]], submission,\n                         how=\"inner\", validate=\"one_to_one\")\nsubmission_ic.to_csv(\"submission_ic.csv\", index=False)\nsubmission_ic","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"***","metadata":{}}]}