{"cells":[{"metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","trusted":true},"cell_type":"code","source":"# Operating system\nimport sys\nimport os\nfrom pathlib import Path\n\n# math\nimport numpy as np\n\n# data analysis\nimport pandas as pd\n\n#plotting 2D\nimport matplotlib.pyplot as plt\nfrom matplotlib.axes import Axes\nfrom matplotlib import animation, rc\nimport matplotlib.patches as patches\nimport matplotlib as mpl","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"# Lyft dataset SDK\n!pip install lyft-dataset-sdk\nfrom lyft_dataset_sdk.lyftdataset import LyftDataset\nfrom lyft_dataset_sdk.utils.data_classes import LidarPointCloud, Box, Quaternion\nfrom lyft_dataset_sdk.utils.geometry_utils import view_points, transform_matrix\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"!ln -s /kaggle/input/3d-object-detection-for-autonomous-vehicles/train_images images\n!ln -s /kaggle/input/3d-object-detection-for-autonomous-vehicles/train_maps maps\n!ln -s /kaggle/input/3d-object-detection-for-autonomous-vehicles/train_lidar lidar","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"lyft_dataset =  LyftDataset(data_path='.', json_path='/kaggle/input/3d-object-detection-for-autonomous-vehicles/train_data', verbose=True)\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"log_df = pd.DataFrame(lyft_dataset.log)\n# log_df = log_df[log_df['vehicle'].str.match('a101')]\n#da4ed9e02f64c544f4f1f10c6738216dcb0e6b0d50952e\nscene_df =  pd.DataFrame(lyft_dataset.scene)\nscene_df = pd.merge(log_df, scene_df, left_on='token', right_on='log_token',how='inner')\n\n# scene_df.head()\nsample_df = pd.DataFrame(lyft_dataset.sample)\nsample_df = pd.merge(scene_df[['log_token', 'date_captured', 'vehicle', 'token_y']], sample_df, left_on='token_y', right_on='scene_token',how='inner')\n# sample_df.head()\n\nsampledata_df = pd.DataFrame(lyft_dataset.sample_data)\nsampledata_df = pd.merge(sample_df[['log_token', 'date_captured', 'token', 'vehicle']], sampledata_df, left_on='token', right_on='sample_token',how='inner')\n# sampledata_df.head()\ncounts = sampledata_df.groupby(['vehicle','date_captured'])['channel'].value_counts().unstack().fillna(0)\n\ncounts","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"### a102 on 2019-05-24 looks good"},{"metadata":{},"cell_type":"markdown","source":"## Prepare data for plotting the trip"},{"metadata":{"_uuid":"d629ff2d2480ee46fbb7e2d37f6b5fab8052498a","_cell_guid":"79c7e3d0-c299-4dcb-8224-4455121ee9b0","trusted":true},"cell_type":"code","source":"# join log, scene, sample, data, ego pose and filter for car a102's ride on 2019-05-24\nlog_df = pd.DataFrame(lyft_dataset.log)\nlog_df = log_df[log_df['date_captured'].str.match('2019-05-24')]\nlog_df = log_df[log_df['vehicle'].str.match('a102')]\n\n\nscene_df =  pd.DataFrame(lyft_dataset.scene)\nscene_df = pd.merge(log_df, scene_df, left_on='token', right_on='log_token',how='inner')\n\nsample_df = pd.DataFrame(lyft_dataset.sample)\nsample_df = pd.merge(sample_df, scene_df[['vehicle', 'token_y']], left_on='scene_token', right_on='token_y',how='inner')\n\nsampledata_df = pd.DataFrame(lyft_dataset.sample_data)\nsampledata_df = pd.merge(sample_df[['token', 'vehicle']], sampledata_df, left_on='token', right_on='sample_token',how='inner')\n\nego_pose_df = pd.DataFrame(lyft_dataset.ego_pose)\nego_pose_df = pd.merge(sampledata_df[['token_x','ego_pose_token', 'channel','vehicle' ]], \n                                   ego_pose_df, left_on='ego_pose_token', right_on='token',how='inner')\n\nego_pose_df = ego_pose_df.drop(['token'], axis=1)\nego_pose_df.rename(columns={'token_x':'token'}, inplace=True)\n\n# ego_pose_df = ego_pose_df[ego_pose_df['vehicle'].str.match('a101')]\nego_pose_df.sort_values(by=['timestamp'])\n\ncalibrated_sensor_df = pd.DataFrame(lyft_dataset.calibrated_sensor)\n\n# pivot on sample token to spread channel translations across columns\npivot_df = ego_pose_df.pivot(index ='token', columns ='channel', values = ['translation','rotation']).reset_index()\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"ego_pose_df.head()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"calibrated_sensor_df.head()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"len(calibrated_sensor_df)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pivot_df.head()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"center_x = []\ncenter_y = []\nx = []\ny = []\nx0 = []\ny0 = []\nx1 = []\ny1 = []\nx2 = []\ny2 = []\nx3 = []\ny3 = []\nx4 = []\ny4 = []\nx5 = []\ny5 = []\nx6 = []\ny6 = []\nx7 = []\ny7 = []\nyaw = []\nnum_sample = len(pivot_df)\nfor i in range(num_sample):\n    token = pivot_df.iloc[i, 0]\n    my_sample = lyft_dataset.get('sample', token)\n    sample_lidar_token = my_sample[\"data\"]['LIDAR_TOP']\n    cam = lyft_dataset.get(\"sample_data\", sample_lidar_token)\n    poserecord = lyft_dataset.get(\"ego_pose\", cam[\"ego_pose_token\"])\n    yaw.append(Quaternion(poserecord[\"rotation\"]).yaw_pitch_roll[0])\n    \n    center_x.append(poserecord[\"translation\"][0])\n    center_y.append(poserecord[\"translation\"][1])\n    \n    sample_cam_token = my_sample[\"data\"]['CAM_FRONT']\n    cam = lyft_dataset.get(\"sample_data\", sample_cam_token)\n    cs_record = lyft_dataset.get(\"calibrated_sensor\", cam[\"calibrated_sensor_token\"])\n    sensor_vector = np.dot(Quaternion(poserecord[\"rotation\"]).rotation_matrix, cs_record[\"translation\"])\n    poserecord = lyft_dataset.get(\"ego_pose\", cam[\"ego_pose_token\"])\n    \n    x.append(poserecord[\"translation\"][0] + sensor_vector[0])\n    y.append(poserecord[\"translation\"][1] + sensor_vector[1])\n    \n    sample_cam_token = my_sample[\"data\"]['CAM_FRONT_LEFT']\n    cam = lyft_dataset.get(\"sample_data\", sample_cam_token)\n    #poserecord = lyft_dataset.get(\"ego_pose\", cam[\"ego_pose_token\"])\n    cs_record = lyft_dataset.get(\"calibrated_sensor\", cam[\"calibrated_sensor_token\"])\n    sensor_vector = np.dot(Quaternion(poserecord[\"rotation\"]).rotation_matrix, cs_record[\"translation\"])\n    \n    x0.append(poserecord[\"translation\"][0] + sensor_vector[0])\n    y0.append(poserecord[\"translation\"][1] + sensor_vector[1])\n\n    sample_cam_token = my_sample[\"data\"]['CAM_FRONT_RIGHT']\n    cam = lyft_dataset.get(\"sample_data\", sample_cam_token)\n    #poserecord = lyft_dataset.get(\"ego_pose\", cam[\"ego_pose_token\"])\n    cs_record = lyft_dataset.get(\"calibrated_sensor\", cam[\"calibrated_sensor_token\"])\n    sensor_vector = np.dot(Quaternion(poserecord[\"rotation\"]).rotation_matrix, cs_record[\"translation\"])\n    \n    x1.append(poserecord[\"translation\"][0] + sensor_vector[0])\n    y1.append(poserecord[\"translation\"][1] + sensor_vector[1])\n\n    sample_lidar_token = my_sample[\"data\"]['LIDAR_TOP']\n    cam = lyft_dataset.get(\"sample_data\", sample_lidar_token)\n    #poserecord = lyft_dataset.get(\"ego_pose\", cam[\"ego_pose_token\"])\n    cs_record = lyft_dataset.get(\"calibrated_sensor\", cam[\"calibrated_sensor_token\"])\n    sensor_vector = np.dot(Quaternion(poserecord[\"rotation\"]).rotation_matrix, cs_record[\"translation\"])\n    \n    x2.append(poserecord[\"translation\"][0] + sensor_vector[0])\n    y2.append(poserecord[\"translation\"][1] + sensor_vector[1])\n\n    sample_cam_token = my_sample[\"data\"]['CAM_BACK']\n    cam = lyft_dataset.get(\"sample_data\", sample_cam_token)\n    #poserecord = lyft_dataset.get(\"ego_pose\", cam[\"ego_pose_token\"])\n    cs_record = lyft_dataset.get(\"calibrated_sensor\", cam[\"calibrated_sensor_token\"])\n    sensor_vector = np.dot(Quaternion(poserecord[\"rotation\"]).rotation_matrix, cs_record[\"translation\"])\n    \n    x3.append(poserecord[\"translation\"][0] + sensor_vector[0])\n    y3.append(poserecord[\"translation\"][1] + sensor_vector[1])\n    \n    sample_cam_token = my_sample[\"data\"]['CAM_BACK_LEFT']\n    cam = lyft_dataset.get(\"sample_data\", sample_cam_token)\n    #poserecord = lyft_dataset.get(\"ego_pose\", cam[\"ego_pose_token\"])\n    cs_record = lyft_dataset.get(\"calibrated_sensor\", cam[\"calibrated_sensor_token\"])\n    sensor_vector = np.dot(Quaternion(poserecord[\"rotation\"]).rotation_matrix, cs_record[\"translation\"])\n    \n    x4.append(poserecord[\"translation\"][0] + sensor_vector[0])\n    y4.append(poserecord[\"translation\"][1] + sensor_vector[1])\n    \n    sample_cam_token = my_sample[\"data\"]['CAM_BACK_RIGHT']\n    cam = lyft_dataset.get(\"sample_data\", sample_cam_token)\n    #poserecord = lyft_dataset.get(\"ego_pose\", cam[\"ego_pose_token\"])\n    cs_record = lyft_dataset.get(\"calibrated_sensor\", cam[\"calibrated_sensor_token\"])\n    sensor_vector = np.dot(Quaternion(poserecord[\"rotation\"]).rotation_matrix, cs_record[\"translation\"])\n    \n    x5.append(poserecord[\"translation\"][0] + sensor_vector[0])\n    y5.append(poserecord[\"translation\"][1] + sensor_vector[1])\n    \n    sample_token = my_sample[\"data\"]['LIDAR_FRONT_LEFT']\n    cam = lyft_dataset.get(\"sample_data\", sample_token)\n    poserecord = lyft_dataset.get(\"ego_pose\", cam[\"ego_pose_token\"])\n    cs_record = lyft_dataset.get(\"calibrated_sensor\", cam[\"calibrated_sensor_token\"])\n    sensor_vector = np.dot(Quaternion(poserecord[\"rotation\"]).rotation_matrix, cs_record[\"translation\"])\n    \n    x6.append(poserecord[\"translation\"][0] - sensor_vector[0])\n    y6.append(poserecord[\"translation\"][1] - sensor_vector[1])\n    \n    sample_token = my_sample[\"data\"]['LIDAR_FRONT_RIGHT']\n    cam = lyft_dataset.get(\"sample_data\", sample_token)\n    poserecord = lyft_dataset.get(\"ego_pose\", cam[\"ego_pose_token\"])\n    cs_record = lyft_dataset.get(\"calibrated_sensor\", cam[\"calibrated_sensor_token\"])\n    sensor_vector = np.dot(Quaternion(poserecord[\"rotation\"]).rotation_matrix, cs_record[\"translation\"])\n    \n    x7.append(poserecord[\"translation\"][0] - sensor_vector[0])\n    y7.append(poserecord[\"translation\"][1] - sensor_vector[1])","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"cx = sorted(center_x)\ncx","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"fig = plt.figure(figsize=(16,8))\n        \nax = fig.add_subplot(111)\nax.set(xlim=(2110, 2130), ylim=(1020, 1035))\n\nfor i in range(num_sample):\n    if i % 5 == 0:\n        cx, cy = center_x[i], center_y[i]\n        angle = yaw[i] * 57 #get angle in degrees\n        rect = patches.Rectangle((-2.2,-1),4.4,2,linewidth=1,edgecolor='r',facecolor='none')\n        t1 = mpl.transforms.Affine2D().rotate_deg(angle) + mpl.transforms.Affine2D().translate(cx,cy)\n        rect.set_transform(t1 + ax.transData)\n        ax.add_patch(rect)\n\nax.scatter(center_x, center_y, s=100, c='purple', label='Center')\nax.scatter(x, y,s=50, c='r', label='Camera Front')\nax.scatter(x0, y0,s=50, c='g', label='Camera Front Left')\nax.scatter(x1, y1,s=50, c='orange', label='Camera Front Right')\nax.scatter(x2, y2,s=50, c='b', label='LIDAR Top')\nax.scatter(x3, y3,s=50, c='y', label='Camera Back')\nax.scatter(x4, y4,s=50, c='k', label='Camera Back Left')\nax.scatter(x5, y5,s=50, c='m', label='Camera Back Right')\nax.scatter(x6, y6,s=50, c='c', label='LIDAR Front Left')\nax.scatter(x7, y7,s=50, c='brown', label='LIDAR Front Right')\nax.legend()\nplt.show()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"fig = plt.figure(figsize=(16,8))\n        \nax = fig.add_subplot(111)\nax.set(xlim=(2000, 2400), ylim=(500, 1200))\n\nax.scatter(x, y,s=50, c='r', label='Camera Front')\nax.scatter(x0, y0,s=50, c='g', label='Camera Front Left')\nax.scatter(x1, y1,s=50, c='orange', label='Camera Front Right')\nax.scatter(x2, y2,s=50, c='b', label='LIDAR Top')\nax.scatter(x3, y3,s=50, c='y', label='Camera Back')\nax.scatter(x4, y4,s=50, c='k', label='Camera Back Left')\nax.scatter(x5, y5,s=50, c='m', label='Camera Back Right')\nax.scatter(x6, y6,s=50, c='c', label='LIDAR Front Left')\nax.scatter(x7, y7,s=50, c='brown', label='LIDAR Front Right')\nax.legend()\nplt.show()","execution_count":null,"outputs":[]}],"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":4,"nbformat_minor":1}