{"cells":[{"metadata":{},"cell_type":"markdown","source":"# Rendering LIDAR points inside a bounding box"},{"metadata":{},"cell_type":"markdown","source":"### This is part of a larger notebook to attempt feeding data into PointRCNN"},{"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\nos.environ[\"OMP_NUM_THREADS\"] = \"1\"\n\n# math\nimport numpy as np\n\n#progress bar\nfrom tqdm import tqdm, tqdm_notebook\ntqdm.pandas()\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\nfrom PIL import Image\n# data analysis\nimport pandas as pd\n","execution_count":null,"outputs":[]},{"metadata":{"_uuid":"d629ff2d2480ee46fbb7e2d37f6b5fab8052498a","_cell_guid":"79c7e3d0-c299-4dcb-8224-4455121ee9b0","trusted":true},"cell_type":"code","source":"#machine learning\nimport sklearn\nimport h5py\nimport sklearn.metrics\nimport tensorflow as tf\n\n\n# Lyft dataset SDK\n!pip install lyft-dataset-sdk\nfrom lyft_dataset_sdk.utils.map_mask import MapMask\nfrom lyft_dataset_sdk.lyftdataset import LyftDataset\nfrom lyft_dataset_sdk.utils.geometry_utils import view_points, box_in_image, BoxVisibility\nfrom lyft_dataset_sdk.utils.geometry_utils import view_points, transform_matrix\nfrom lyft_dataset_sdk.utils.data_classes import LidarPointCloud, Box, Quaternion\n\n\n# science\nimport scipy\nimport scipy.ndimage\nimport scipy.special\nfrom scipy.spatial.transform import Rotation as R\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"DATA_PATH = '.'\nARTIFACTS_FOLDER = \"./artifacts\"\nCWD = os.getcwd()\n!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\nlyft_dataset = LyftDataset(data_path='.', json_path='/kaggle/input/3d-object-detection-for-autonomous-vehicles/train_data', verbose=True)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"selected_log_entries = list(filter(lambda l: l['token'] == '71dfb15d2f88bf2aab2c5d4800c0d10a76c279b9fda98720781a406cbacc583b' , lyft_dataset.log))","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def set_lidar_pointcloud(row, df):\n    lidar_filepath = lyft_dataset.get_sample_data_path(row['sampledata_token'])\n    lidar_pointcloud = LidarPointCloud.from_file(lidar_filepath)\n    return lidar_pointcloud.points","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def delete_images():\n    !find ../data/train_lidar/. -name '*.bin' | xargs rm -f\n    !find ../data/train_images/. -name '*.jpeg' | xargs rm -f","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"\ndef extract_data(selected_log_entries):\n    log_df = pd.DataFrame(selected_log_entries)\n    log_df.rename(columns={'token':'log_token'}, inplace=True)\n\n\n    scene_df =  pd.DataFrame(lyft_dataset.scene)\n    scene_df = pd.merge(log_df, scene_df, left_on='log_token', right_on='log_token',how='inner')\n    scene_df.rename(columns={'token':'scene_token'}, inplace=True)\n    sample_df = pd.DataFrame(lyft_dataset.sample)\n    sample_df = pd.merge(sample_df, scene_df[['scene_token','vehicle','log_token']], left_on='scene_token', right_on='scene_token',how='inner')\n    sample_df.rename(columns={'token':'sample_token'}, inplace=True)\n    sample_df = sample_df[['log_token','vehicle','sample_token','data']]\n\n    sampledata_df = pd.DataFrame(lyft_dataset.sample_data)\n    sampledata_df.rename(columns={'token':'sampledata_token'}, inplace=True)\n    sampledata_df = pd.merge(sample_df, sampledata_df, left_on='sample_token', right_on='sample_token',how='inner')\n    sampledata_df = sampledata_df[[\n        'log_token',\n        'vehicle',\n        'sample_token', \n        'sampledata_token',\n        'ego_pose_token', \n        'channel',\n        'calibrated_sensor_token',\n        'fileformat',\n        'filename']]\n\n\n    ann_df = pd.DataFrame(lyft_dataset.sample_annotation)\n    ann_df.rename(columns={'token':'ann_token', 'rotation': 'ann_rotation', 'translation': 'ann_translation'}, inplace=True)\n    ann_df = pd.merge(ann_df, sample_df, left_on='sample_token', right_on='sample_token', how='inner')\n    ann_df.sort_values(by=['sample_token'], axis=0, inplace=True)\n    ann_df = ann_df[['ann_token',\n                     'sample_token',\n                     'size',\n                     'ann_rotation',\n                     'ann_translation',\n                     'data',\n                     'category_name']]\n    # sampledata_df = pd.merge(sampledata_df, ann_df, left_on='sample_token', right_on='sample_token',how='inner')\n\n    ep_df = pd.DataFrame(lyft_dataset.ego_pose)\n    ep_df.rename(columns={'token':'ego_pose_token', 'rotation': 'ep_rotation', 'translation': 'ep_translation'}, inplace=True)\n    ep_df = ep_df[['ego_pose_token',\n                     'ep_rotation',\n                     'ep_translation']]\n    sampledata_df = pd.merge(sampledata_df, ep_df, left_on='ego_pose_token', right_on='ego_pose_token',how='inner')\n\n\n    cs_df = pd.DataFrame(lyft_dataset.calibrated_sensor)\n    cs_df.rename(columns={'token':'calibrated_sensor_token', 'rotation': 'cs_rotation', 'translation': 'cs_translation'}, inplace=True)\n    cs_df = cs_df[['calibrated_sensor_token',\\\n                     'cs_rotation',\\\n                     'cs_translation',\\\n                     'camera_intrinsic'\n                  ]]\n    sampledata_df = pd.merge(sampledata_df, cs_df, left_on='calibrated_sensor_token', right_on='calibrated_sensor_token',how='inner')\n    files_df = sampledata_df[['sample_token', 'fileformat', 'filename']]\n    files_df['filename'] = 'train_' + files_df['filename'].astype(str)\n    lidar_df = files_df[files_df['fileformat'] == 'bin']\n    images_df = files_df[files_df['fileformat'] == 'jpeg']\n    \n    # delete_images()\n\n#     for index, entry in tqdm(lidar_df.iterrows()):\n#         zip_command = \"unzip ../3d-object-detection-for-autonomous-vehicles.zip \"\\\n#             + entry['filename']\\\n#             + \" -d \"\\\n#             + CWD + \"/../data/\"\n        \n    #     os.system(zip_command)\n#     for index, entry in tqdm(images_df.iterrows()):\n#         zip_command = \"unzip ../3d-object-detection-for-autonomous-vehicles.zip \"\\\n#             + entry['filename']\\\n#             + \" -d \"\\\n#             + CWD + \"/../data/\"\n        \n#         os.system(zip_command)\n\n    samplelidardata_df = sampledata_df[sampledata_df['fileformat'] == 'bin']\n    samplelidardata_df['lidar_pointcloud'] = samplelidardata_df.apply(lambda row: set_lidar_pointcloud(row, sampledata_df), axis=1)\n    \n    return (sampledata_df, samplelidardata_df, ann_df, lidar_df)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"(sampledata_df, lidardata_df, ann_df, lidar_df) = extract_data(selected_log_entries)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def car_to_sensor (coords, translation, rotation):\n    return  Quaternion(rotation).inverse.rotate(np.add(coords, np.negative(translation)))\n\ndef world_to_car (coords, translation, rotation):\n    return  Quaternion(rotation).inverse.rotate(np.add(coords, np.negative(translation)))\n\ndef car_to_world (coords, translation, rotation):\n    return  np.add(translation, Quaternion(rotation).rotate(coords))\n\ndef sensor_to_car (coords, translation, rotation):\n    return  np.add(translation, Quaternion(rotation).rotate(coords))","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def map_points_to_image(points, camera_token: str, camera_front_token: str, camera_back_token: str):\n        # based on devkit map_pointcloud_to_image\n\n        cam = lyft_dataset.get(\"sample_data\", camera_token)\n        cam_back = lyft_dataset.get(\"sample_data\", camera_back_token)\n        cam_front = lyft_dataset.get(\"sample_data\", camera_front_token)\n\n\n        cs_record = lyft_dataset.get(\"calibrated_sensor\", cam['calibrated_sensor_token'])\n        # print(cs_record)\n        image = Image.open(str(lyft_dataset.data_path / cam[\"filename\"]))\n\n        \n        points_t = points.T\n        \n        poserecord_front = lyft_dataset.get(\"ego_pose\", cam_front[\"ego_pose_token\"])\n        poserecord_back = lyft_dataset.get(\"ego_pose\", cam_back[\"ego_pose_token\"])\n        poserecord = lyft_dataset.get(\"ego_pose\", cam[\"ego_pose_token\"])\n\n        ep_t = np.array(poserecord[\"translation\"])\n        ep_r = poserecord_front[\"rotation\"]\n        print(ep_t, ep_r)\n        \n        cs_record = lyft_dataset.get(\"calibrated_sensor\", cam[\"calibrated_sensor_token\"])\n        cs_t = np.array(cs_record[\"translation\"])\n        cs_r = cs_record[\"rotation\"]\n\n        world_to_camera = lambda coords: [car_to_sensor(world_to_car(xyz, ep_t, ep_r), cs_t, cs_r) for xyz in coords]\n        points_t = world_to_camera(points_t)\n        points = np.array(points_t).T\n\n        depths = points[2, :]\n\n        # Retrieve the color from the depth.\n        coloring = depths\n        \n        # Take the actual picture (matrix multiplication with camera-matrix + renormalization).\n        points = view_points(points[:3, :], np.array(cs_record[\"camera_intrinsic\"]), normalize=True)\n\n        # Remove points that are either outside or behind the camera. Leave a margin of 1 pixel for aesthetic reasons.\n        mask = np.ones(depths.shape[0], dtype=bool)\n        mask = np.logical_and(mask, depths > 0)\n        mask = np.logical_and(mask, points[0, :] > 1)\n        mask = np.logical_and(mask, points[0, :] < image.size[0] - 1)\n        mask = np.logical_and(mask, points[1, :] > 1)\n        mask = np.logical_and(mask, points[1, :] < image.size[1] - 1)\n        points = points[:, mask]\n        coloring = coloring[mask]\n\n        return points, coloring, image","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"  def render_points_in_image(\n        points,\n        sample_token: str,\n        dot_size: int = 2,\n        camera_channel: str = \"CAM_FRONT\",\n        out_path: str = None,\n    ):\n        # based on devkit render_pointcloud_to_image\n\n        sample_record = lyft_dataset.get(\"sample\", sample_token)\n\n        camera_token = sample_record[\"data\"][camera_channel]\n        camera_front_token = sample_record[\"data\"]['CAM_FRONT']\n        camera_back_token = sample_record[\"data\"]['CAM_BACK']\n\n\n\n        points, coloring, im = map_points_to_image(points, camera_token, camera_front_token, camera_back_token)\n        plt.figure(figsize=(9, 16))\n        plt.imshow(im)\n        plt.scatter(points[0, :], points[1, :], c=coloring, s=dot_size)\n        plt.axis(\"off\")\n\n        if out_path is not None:\n            plt.savefig(out_path)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def map_pointcloud_to_box(points, corners):\n        corners = np.array(corners)\n        minx = np.min(corners[:,0])\n        maxx = np.max(corners[:,0])\n        miny = np.min(corners[:,1])\n        maxy = np.max(corners[:,1])\n        minz = np.min(corners[:,2])\n        maxz = np.max(corners[:,2])\n        \n        # Remove points that are outside the bounding box\n        mask = np.ones(points.shape[1], dtype=bool)\n        mask = np.logical_and(mask, points[0, :] >= minx)\n        mask = np.logical_and(mask, points[0, :] <= maxx)\n        mask = np.logical_and(mask, points[1, :] >= miny)\n        mask = np.logical_and(mask, points[1, :] <= maxy)\n        mask = np.logical_and(mask, points[2, :] >= minz)\n        mask = np.logical_and(mask, points[2, :] <= maxz)\n        box_points = points[:, mask]\n\n        return box_points","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def render_box_points_in_image(ds, s_df, a_df, sample_token, ann_token, camera_channel):\n    s_df = s_df[s_df['sample_token'] == sample_token]\n    s_df = s_df[s_df['channel'].str.match('LIDAR')]\n    \n    all_points = np.zeros((3,0))\n    for i in range(len(s_df)):\n        print(s_df.iloc[i]['channel'])\n        row = s_df.iloc[i]\n        cs_t = row['cs_translation']\n        cs_r = row['cs_rotation']\n        ep_t = row['ep_translation']\n        ep_r = row['ep_rotation']\n\n\n        pointcloud = s_df.iloc[i]['lidar_pointcloud']\n        box = lyft_dataset.get_box(ann_token)\n        # box corners are in world coordinates\n        corners = box.corners().T\n\n        # so transform point cloud to world coordinates\n        sensor_to_world = lambda coords: [car_to_world(sensor_to_car(xyz, cs_t, cs_r), ep_t, ep_r) for xyz in coords]\n        pc_points_t = pointcloud.T\n        pc_points_t = pc_points_t[:,:3]\n        pc_points_t = sensor_to_world(pc_points_t)\n        pc_points_t = np.array(pc_points_t).T\n        all_points = np.concatenate([all_points, pc_points_t], axis=1)\n    box = lyft_dataset.get_box(ann_token)\n    # box corners are in world coordinates\n    corners = box.corners().T\n\n    box_points = map_pointcloud_to_box(all_points, corners)\n    print(box_points.shape)\n\n\n    render_points_in_image(box_points,sample_token,\n        5,\n        camera_channel)\n    return box_points","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"box_points = render_box_points_in_image(lyft_dataset, lidardata_df, ann_df, ann_df.iloc[19]['sample_token'], ann_df.iloc[19]['ann_token'], 'CAM_FRONT_RIGHT')","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"box_points = render_box_points_in_image(lyft_dataset, lidardata_df, ann_df, ann_df.iloc[2]['sample_token'], ann_df.iloc[2]['ann_token'], 'CAM_BACK_RIGHT')","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"lyft_dataset.render_annotation(ann_df.iloc[19][\"ann_token\"])","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"len(ann_df)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"lyft_dataset.render_pointcloud_in_image(sample_token = ann_df.iloc[2]['sample_token'],\n                                        dot_size = 0.1,\n                                        camera_channel = 'CAM_BACK_RIGHT')","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}