{"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 os\nfrom glob import glob\nimport numpy as np\nimport pandas as pd\nimport plotly.express as px\nfrom dataclasses import dataclass\nfrom scipy.interpolate import InterpolatedUnivariateSpline","metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","execution":{"iopub.status.busy":"2023-06-10T05:26:04.695374Z","iopub.execute_input":"2023-06-10T05:26:04.695766Z","iopub.status.idle":"2023-06-10T05:26:04.700314Z","shell.execute_reply.started":"2023-06-10T05:26:04.695738Z","shell.execute_reply":"2023-06-10T05:26:04.699682Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## 1.1 データを読み込む","metadata":{}},{"cell_type":"code","source":"SAMPLE_PATH = \"/kaggle/input/smartphone-decimeter-2022/train/2021-12-08-US-LAX-5/XiaomiMi8\"\n\ngnss = pd.read_csv(os.path.join(SAMPLE_PATH, \"device_gnss.csv\"))\nimu = pd.read_csv(os.path.join(SAMPLE_PATH, \"device_imu.csv\"))\ngt = pd.read_csv(os.path.join(SAMPLE_PATH, \"ground_truth.csv\"))","metadata":{"execution":{"iopub.status.busy":"2023-06-10T05:26:04.702936Z","iopub.execute_input":"2023-06-10T05:26:04.704106Z","iopub.status.idle":"2023-06-10T05:26:05.194440Z","shell.execute_reply.started":"2023-06-10T05:26:04.704069Z","shell.execute_reply":"2023-06-10T05:26:05.193145Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"print(gnss.columns)\ndisplay(gnss.head())","metadata":{"execution":{"iopub.status.busy":"2023-06-10T05:26:05.196313Z","iopub.execute_input":"2023-06-10T05:26:05.197287Z","iopub.status.idle":"2023-06-10T05:26:05.221914Z","shell.execute_reply.started":"2023-06-10T05:26:05.197243Z","shell.execute_reply":"2023-06-10T05:26:05.221123Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## 2.1 ECEFからBLHに変換","metadata":{"execution":{"iopub.status.busy":"2023-06-10T05:04:54.398035Z","iopub.execute_input":"2023-06-10T05:04:54.398473Z","iopub.status.idle":"2023-06-10T05:04:54.403895Z","shell.execute_reply.started":"2023-06-10T05:04:54.398416Z","shell.execute_reply":"2023-06-10T05:04:54.402524Z"}}},{"cell_type":"code","source":"WGS84_SEMI_MAJOR_AXIS = 6378137.0\nWGS84_SEMI_MINOR_AXIS = 6356752.314245\nWGS84_SQUARED_FIRST_ECCENTRICITY = 6.69437999013e-3\nWGS84_SQUARED_SECOND_ECCENTRICITY = 6.73949674226e-3\n\nHAVERSINE_RADIUS = 6_371_000\n\n\n@dataclass\nclass ECEF:\n    x: np.array\n    y: np.array\n    z: np.array\n\n    def to_numpy(self):\n        return np.stack([self.x, self.y, self.z], axis=0)\n\n    @staticmethod\n    def from_numpy(pos):\n        x, y, z = [np.squeeze(w) for w in np.split(pos, 3, axis=-1)]\n        return ECEF(x=x, y=y, z=z)\n\n\n@dataclass\nclass BLH:\n    lat: np.array\n    lng: np.array\n    hgt: np.array\n\n\ndef ECEF_to_BLH(ecef):\n    a = WGS84_SEMI_MAJOR_AXIS\n    b = WGS84_SEMI_MINOR_AXIS\n    e2 = WGS84_SQUARED_FIRST_ECCENTRICITY\n    e2_ = WGS84_SQUARED_SECOND_ECCENTRICITY\n    x = ecef.x\n    y = ecef.y\n    z = ecef.z\n    r = np.sqrt(x**2 + y**2)\n    t = np.arctan2(z * (a / b), r)\n    B = np.arctan2(z + (e2_ * b) * np.sin(t) ** 3, r - (e2 * a) * np.cos(t) ** 3)\n    L = np.arctan2(y, x)\n    n = a / np.sqrt(1 - e2 * np.sin(B) ** 2)\n    H = (r / np.cos(B)) - n\n    return BLH(lat=B, lng=L, hgt=H)\n\n\ndef haversine_distance(blh_1, blh_2):\n    dlat = blh_2.lat - blh_1.lat\n    dlng = blh_2.lng - blh_1.lng\n    a = np.sin(dlat / 2) ** 2 + np.cos(blh_1.lat) * np.cos(blh_2.lat) * np.sin(dlng / 2) ** 2\n    dist = 2 * HAVERSINE_RADIUS * np.arcsin(np.sqrt(a))\n    return dist\n","metadata":{"execution":{"iopub.status.busy":"2023-06-10T05:26:05.222991Z","iopub.execute_input":"2023-06-10T05:26:05.223407Z","iopub.status.idle":"2023-06-10T05:26:05.235387Z","shell.execute_reply.started":"2023-06-10T05:26:05.223380Z","shell.execute_reply":"2023-06-10T05:26:05.233868Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# ecefデータの抽出\necef_m = gnss[[\"WlsPositionXEcefMeters\", \"WlsPositionYEcefMeters\", \"WlsPositionZEcefMeters\"]].copy()\ndisplay(ecef_m.head())","metadata":{"execution":{"iopub.status.busy":"2023-06-10T05:26:05.238426Z","iopub.execute_input":"2023-06-10T05:26:05.238837Z","iopub.status.idle":"2023-06-10T05:26:05.259379Z","shell.execute_reply.started":"2023-06-10T05:26:05.238808Z","shell.execute_reply":"2023-06-10T05:26:05.258124Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"ecef = ECEF.from_numpy(ecef_m.to_numpy())\nblh = ECEF_to_BLH(ecef)\nprint(blh)","metadata":{"execution":{"iopub.status.busy":"2023-06-10T05:26:05.261322Z","iopub.execute_input":"2023-06-10T05:26:05.262148Z","iopub.status.idle":"2023-06-10T05:26:05.283473Z","shell.execute_reply.started":"2023-06-10T05:26:05.262113Z","shell.execute_reply":"2023-06-10T05:26:05.282391Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"output_df = pd.DataFrame({\n    \"utcTimeMillis\": gnss['utcTimeMillis'].to_numpy(),\n    'LatitudeDegrees'  : np.degrees(blh.lat),\n    'LongitudeDegrees' : np.degrees(blh.lng),\n})\n\n# utcTimeMillisに重複があるため除去\noutput_df = output_df.groupby(\"utcTimeMillis\").mean().reset_index()\ndisplay(output_df)","metadata":{"execution":{"iopub.status.busy":"2023-06-10T05:26:05.284681Z","iopub.execute_input":"2023-06-10T05:26:05.284977Z","iopub.status.idle":"2023-06-10T05:26:05.307476Z","shell.execute_reply.started":"2023-06-10T05:26:05.284954Z","shell.execute_reply":"2023-06-10T05:26:05.305708Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"## 2.2 データの可視化","metadata":{"execution":{"iopub.status.busy":"2023-06-10T05:16:07.363604Z","iopub.execute_input":"2023-06-10T05:16:07.364376Z","iopub.status.idle":"2023-06-10T05:16:07.368214Z","shell.execute_reply.started":"2023-06-10T05:16:07.364347Z","shell.execute_reply":"2023-06-10T05:16:07.367042Z"}}},{"cell_type":"code","source":"def visualize_traffic(\n    df,\n    lat_col=\"LatitudeDegrees\",\n    lon_col=\"LongitudeDegrees\",\n    color_col=None,\n    label_col=None,\n    zoom=11.5,\n    opacity=1,\n):\n    fig = px.scatter_mapbox(\n        df,\n        # Here, plotly gets, (x,y) coordinates\n        lat=lat_col,\n        lon=lon_col,\n        # Here, plotly detects color of series\n        color=color_col,\n        labels=label_col,\n        zoom=zoom,\n        # center=center,\n        height=600,\n        width=800,\n        opacity=0.5,\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\n\nvisualize_traffic(output_df)","metadata":{"execution":{"iopub.status.busy":"2023-06-10T05:26:46.259560Z","iopub.execute_input":"2023-06-10T05:26:46.259903Z","iopub.status.idle":"2023-06-10T05:26:46.316812Z","shell.execute_reply.started":"2023-06-10T05:26:46.259875Z","shell.execute_reply":"2023-06-10T05:26:46.315575Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# 距離を算出\ndef haversine_distance(blh_1, blh_2):\n    dlat = blh_2.lat - blh_1.lat\n    dlng = blh_2.lng - blh_1.lng\n    a = np.sin(dlat/2)**2 + np.cos(blh_1.lat) * np.cos(blh_2.lat) * np.sin(dlng/2)**2\n    dist = 2 * HAVERSINE_RADIUS * np.arcsin(np.sqrt(a))\n    return dist\n\ndef pandas_haversine_distance(df1, df2):\n    blh1 = BLH(\n        lat=np.deg2rad(df1['LatitudeDegrees'].to_numpy()),\n        lng=np.deg2rad(df1['LongitudeDegrees'].to_numpy()),\n        hgt=0,\n    )\n    blh2 = BLH(\n        lat=np.deg2rad(df2['LatitudeDegrees'].to_numpy()),\n        lng=np.deg2rad(df2['LongitudeDegrees'].to_numpy()),\n        hgt=0,\n    )\n    return haversine_distance(blh1, blh2)\n\n# もしground truthがあれば\noutput_df = output_df.groupby(\"utcTimeMillis\").mean().reset_index()\noutput_df[\"dist_m\"] = pandas_haversine_distance(gt, output_df)\n\nvisualize_traffic(output_df, color_col=output_df[\"dist_m\"].to_numpy())","metadata":{"execution":{"iopub.status.busy":"2023-06-10T05:26:05.368156Z","iopub.execute_input":"2023-06-10T05:26:05.368469Z","iopub.status.idle":"2023-06-10T05:26:05.434713Z","shell.execute_reply.started":"2023-06-10T05:26:05.368423Z","shell.execute_reply":"2023-06-10T05:26:05.433083Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{},"execution_count":null,"outputs":[]}]}